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 */
#define MAGIOCEXSTRAP _MAGIOC(6)
/** perform self test and report status */
#define MAGIOCSELFTEST _MAGIOC(7)
#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:
return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
default:
/* give it to the superclass */
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) &&
(-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)) {
/* scale is different from one */
scale_valid = true;
} else {
/* scale is one */
scale_valid = false;
} else {
scale_valid = true;
}
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.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
/* offset is different from zero */
offset_valid = true;
} else {
/* offset is zero */
offset_valid = false;
} else {
offset_valid = true;
}
if (_calibrated != (offset_valid && scale_valid)) {
warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ",
(offset_valid) ? "" : "offset invalid.");
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
/* notify about state change */
struct subsystem_info_s info = {
@ -1059,7 +1062,9 @@ int HMC5883::check_calibration()
SUBSYSTEM_TYPE_MAG};
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)

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

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