dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added preflight_check app which checks core system sensors, so far only mag

This commit is contained in:
Lorenz Meier 2012-11-15 13:21:00 +01:00
parent c4bf3ea3ed
commit f803540415
3 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
apps/drivers/drv_mag.h
View File

@ -108,4 +108,7 @@ ORB_DECLARE(sensor_mag);
/** excite strap */ /** excite strap */
#define MAGIOCEXSTRAP _MAGIOC(6) #define MAGIOCEXSTRAP _MAGIOC(6)
/** perform self test and report status */
#define MAGIOCSELFTEST _MAGIOC(7)
#endif /* _DRV_MAG_H */ #endif /* _DRV_MAG_H */

23
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -647,6 +647,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCEXSTRAP: case MAGIOCEXSTRAP:
return set_excitement(arg); return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
default: default:
/* give it to the superclass */ /* give it to the superclass */
return I2C::ioctl(filp, cmd, arg); return I2C::ioctl(filp, cmd, arg);
@ -1032,24 +1035,24 @@ int HMC5883::check_calibration()
if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) && if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) &&
(-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) && (-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) &&
(-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) { (-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) {
/* scale is different from one */ /* scale is one */
scale_valid = true;
} else {
scale_valid = false; scale_valid = false;
} else {
scale_valid = true;
} }
if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
/* offset is different from zero */ /* offset is zero */
offset_valid = true;
} else {
offset_valid = false; offset_valid = false;
} else {
offset_valid = true;
} }
if (_calibrated != (offset_valid && scale_valid)) { if (_calibrated != (offset_valid && scale_valid)) {
warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ", warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid."); (offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid); _calibrated = (offset_valid && scale_valid);
/* notify about state change */ /* notify about state change */
struct subsystem_info_s info = { struct subsystem_info_s info = {
@ -1059,7 +1062,9 @@ int HMC5883::check_calibration()
SUBSYSTEM_TYPE_MAG}; SUBSYSTEM_TYPE_MAG};
orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info); orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info);
} }
return 0;
/* return 0 if calibrated, 1 else */
return (!_calibrated);
} }
int HMC5883::set_excitement(unsigned enable) int HMC5883::set_excitement(unsigned enable)

1
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -54,6 +54,7 @@ CONFIGURED_APPS += systemcmds/mixer
CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/eeprom
CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/param
CONFIGURED_APPS += systemcmds/bl_update CONFIGURED_APPS += systemcmds/bl_update
CONFIGURED_APPS += systemcmds/preflight_check
#CONFIGURED_APPS += systemcmds/calibration #CONFIGURED_APPS += systemcmds/calibration
# Tutorial code from # Tutorial code from