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

Made accel / gyro self tests aware of offsets and scales, added support to config command to call these

This commit is contained in:
Lorenz Meier 2013-08-09 17:56:32 +02:00
parent 7861caf482
commit dccdc977d5
4 changed files with 174 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/drivers/l3gd20/l3gd20.cpp
View File

@ -260,6 +260,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 */
@ -519,6 +526,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);
@ -713,6 +723,7 @@ L3GD20::measure()
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0)
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
/* stop the perf counter */
@ -727,6 +738,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.
*/

69
src/drivers/mpu6000/mpu6000.cpp
View File

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

14
src/modules/commander/accelerometer_calibration.c
View File

@ -226,6 +226,12 @@ int do_accel_calibration_mesurements(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);
@ -380,6 +386,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) {
@ -389,8 +397,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++) {

63
src/systemcmds/config/config.c
View File

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