diff options
Diffstat (limited to 'src/drivers/l3gd20/l3gd20.cpp')
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 35 |
1 files changed, 34 insertions, 1 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. */ |