diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-11-15 11:55:55 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-11-15 11:55:55 -0800 |
commit | 33e750602ab384069b08ca17ca6589c08177f7a6 (patch) | |
tree | 8f2dbfe7779fc74436ea4ba243e2929c09d5b2ac /apps/drivers | |
parent | b7c6a11e6739d217e5df1e79b7f80399ff1fd8f8 (diff) | |
parent | 3016ae72a3b3b7d7bf1df937fd62a14f53eace6f (diff) | |
download | px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.tar.gz px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.tar.bz2 px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.zip |
Merge remote-tracking branch 'upstream/master' into io
Fixed Conflicts:
apps/multirotor_att_control/multirotor_att_control_main.c
rc loss failsafe throttle tested
Diffstat (limited to 'apps/drivers')
-rw-r--r-- | apps/drivers/drv_accel.h | 3 | ||||
-rw-r--r-- | apps/drivers/drv_gyro.h | 3 | ||||
-rw-r--r-- | apps/drivers/drv_led.h | 6 | ||||
-rw-r--r-- | apps/drivers/drv_mag.h | 3 | ||||
-rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 37 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 44 |
6 files changed, 76 insertions, 20 deletions
diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 3834795e0..794de584b 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -115,4 +115,7 @@ ORB_DECLARE(sensor_accel); /** get the current accel measurement range in g */ #define ACCELIOCGRANGE _ACCELIOC(8) +/** get the result of a sensor self-test */ +#define ACCELIOCSELFTEST _ACCELIOC(9) + #endif /* _DRV_ACCEL_H */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 0dbf05c5b..1d0fef6fc 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -111,4 +111,7 @@ ORB_DECLARE(sensor_gyro); /** get the current gyro measurement range in degrees per second */ #define GYROIOCGRANGE _GYROIOC(7) +/** check the status of the sensor */ +#define GYROIOCSELFTEST _GYROIOC(8) + #endif /* _DRV_GYRO_H */ diff --git a/apps/drivers/drv_led.h b/apps/drivers/drv_led.h index 7eb9e4b7c..21044f620 100644 --- a/apps/drivers/drv_led.h +++ b/apps/drivers/drv_led.h @@ -47,9 +47,9 @@ #define _LED_BASE 0x2800 /* PX4 LED colour codes */ -#define LED_AMBER 0 -#define LED_RED 0 /* some boards have red rather than amber */ -#define LED_BLUE 1 +#define LED_AMBER 1 +#define LED_RED 1 /* some boards have red rather than amber */ +#define LED_BLUE 0 #define LED_SAFETY 2 #define LED_ON _IOC(_LED_BASE, 0) diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 114bcb646..9aab995a1 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -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 */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 81bc8954b..3849a2e05 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -634,6 +634,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSSCALE: /* set new scale factors */ memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); + /* check calibration, but not actually return an error */ (void)check_calibration(); return 0; @@ -648,6 +649,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); @@ -1012,7 +1016,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (ret == OK) { - warnx("mag scale calibration successfully finished."); + if (!check_calibration()) { + warnx("mag scale calibration successfully finished."); + } else { + warnx("mag scale calibration finished with invalid results."); + ret == ERROR; + } } else { warnx("mag scale calibration failed."); @@ -1025,27 +1034,27 @@ int HMC5883::check_calibration() { bool scale_valid, offset_valid; - 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 { + if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { + /* 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 = { @@ -1055,7 +1064,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) diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 90786886a..ed79440cc 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -260,6 +260,13 @@ private: */ uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + /** + * Self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + }; /** @@ -494,6 +501,17 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return ret; } +int +MPU6000::self_test() +{ + if (_reads == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (_reads > 0) ? 0 : 1; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -592,9 +610,17 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case ACCELIOCSSCALE: - /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); - return OK; + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } case ACCELIOCGSCALE: /* copy scale out */ @@ -609,6 +635,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_rad_s = 8.0f * 9.81f; return -EINVAL; + case ACCELIOCSELFTEST: + return self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -656,6 +685,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_m_s2 = xx return -EINVAL; + case GYROIOCSELFTEST: + return self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -813,7 +845,11 @@ MPU6000::measure() * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + return; + + /* count measurement */ + _reads++; /* * Convert from big to little endian |