diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-20 09:41:16 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-20 09:41:16 +0200 |
commit | 7cff9b1e84e1c620d9af7fd8feae7ee77d8ef6a6 (patch) | |
tree | c511ff2b9c3c8074aec84c2272cd65c1bd1d1b54 /src | |
parent | 95260d453592903bfcd3dba9379db033738d5b89 (diff) | |
parent | deb426b66002da2562531b37e4ac0f8097c8a6eb (diff) | |
download | px4-firmware-7cff9b1e84e1c620d9af7fd8feae7ee77d8ef6a6.tar.gz px4-firmware-7cff9b1e84e1c620d9af7fd8feae7ee77d8ef6a6.tar.bz2 px4-firmware-7cff9b1e84e1c620d9af7fd8feae7ee77d8ef6a6.zip |
Merge branch 'master' of github.com:PX4/Firmware into fmuv2_bringup_new_state_machine_drton
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 35 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 69 | ||||
-rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 14 | ||||
-rw-r--r-- | src/systemcmds/config/config.c | 63 |
4 files changed, 174 insertions, 7 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 05739f04f..de6b753f1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -272,6 +272,13 @@ private: * @return OK if the value can be supported. */ int set_samplerate(unsigned frequency); + + /** + * Self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); }; /* helper macro for handling report buffer indices */ @@ -563,6 +570,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGRANGE: return _current_range; + case GYROIOCSELFTEST: + return self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -791,7 +801,8 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + if (_gyro_topic > 0) + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); /* stop the perf counter */ perf_end(_sample_perf); @@ -805,6 +816,28 @@ L3GD20::print_info() _num_reports, _oldest_report, _next_report, _reports); } +int +L3GD20::self_test() +{ + /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + return 1; + + return 0; +} + /** * Local functions in support of the shell command. */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 65fa33530..9d498bbd6 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -285,12 +285,26 @@ private: uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } /** - * Self test + * Measurement self test * * @return 0 on success, 1 on failure */ int self_test(); + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Gyro self test + * + * @return 0 on success, 1 on failure + */ + int gyro_self_test(); + /* set low pass filter frequency */ @@ -321,6 +335,7 @@ protected: void parent_poll_notify(); private: MPU6000 *_parent; + }; /** driver 'main' command */ @@ -653,6 +668,54 @@ MPU6000::self_test() return (_reads > 0) ? 0 : 1; } +int +MPU6000::accel_self_test() +{ + if (self_test()) + return 1; + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + return 1; +} + +int +MPU6000::gyro_self_test() +{ + if (self_test()) + return 1; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + return 1; + + return 0; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -835,7 +898,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_range_m_s2; case ACCELIOCSELFTEST: - return self_test(); + return accel_self_test(); default: /* give it to the superclass */ @@ -918,7 +981,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return _gyro_range_rad_s; case GYROIOCSELFTEST: - return self_test(); + return gyro_self_test(); default: /* give it to the superclass */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b3a5d012b..b6217a414 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -230,6 +230,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float if (orient < 0) return ERROR; + if (data_collected[orient]) { + sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + continue; + } + sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); @@ -391,6 +397,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + int errcount = 0; + while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); if (poll_ret == 1) { @@ -400,8 +408,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp accel_sum[i] += sensor.accelerometer_m_s2[i]; count++; } else { - return ERROR; + errcount++; + continue; } + + if (errcount > samples_num / 10) + return ERROR; } for (int i = 0; i < 3; i++) { diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 2dad2261b..42814f2b2 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -129,7 +129,19 @@ do_gyro(int argc, char *argv[]) ioctl(fd, GYROIOCSRANGE, i); } - } else if (!(argc > 0 && !strcmp(argv[0], "info"))) { + } else if (argc > 0) { + + if(!strcmp(argv[0], "check")) { + int ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret) { + warnx("gyro self test FAILED! Check calibration."); + } else { + warnx("gyro calibration and self test OK"); + } + } + + } else { warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); } @@ -148,6 +160,41 @@ do_gyro(int argc, char *argv[]) static void do_mag(int argc, char *argv[]) { + int fd; + + fd = open(MAG_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", MAG_DEVICE_PATH); + errx(1, "FATAL: no magnetometer found"); + + } else { + + if (argc > 0) { + + if (!strcmp(argv[0], "check")) { + int ret = ioctl(fd, MAGIOCSELFTEST, 0); + + if (ret) { + warnx("mag self test FAILED! Check calibration."); + } else { + warnx("mag calibration and self test OK"); + } + } + + } else { + warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t"); + } + + int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); + int range = -1;//ioctl(fd, MAGIOCGRANGE, 0); + + warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); + + close(fd); + } + exit(0); } @@ -183,7 +230,19 @@ do_accel(int argc, char *argv[]) /* set the range to i dps */ ioctl(fd, ACCELIOCSRANGE, i); } - } else if (!(argc > 0 && !strcmp(argv[0], "info"))) { + } else if (argc > 0) { + + if (!strcmp(argv[0], "check")) { + int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret) { + warnx("accel self test FAILED! Check calibration."); + } else { + warnx("accel calibration and self test OK"); + } + } + + } else { warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t"); } |