diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 0f65c45240ab1ce6358deaffa37508998924b2c3..df7854dfb7f38e51a26dac8b1da1f2c58559cc76 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -220,6 +220,7 @@ protected: #include "AP_InertialSensor_MPU6000.h" #include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_PX4.h" +#include "AP_InertialSensor_VRBRAIN.h" #include "AP_InertialSensor_UserInteract_Stream.h" #include "AP_InertialSensor_UserInteract_MAVLink.h" #include "AP_InertialSensor_Flymaple.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d6c43cc2f87668a1535375f31ad003e53c5cb86e --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp @@ -0,0 +1,264 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include <AP_HAL.h> +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "AP_InertialSensor_VRBRAIN.h" + +const extern AP_HAL::HAL& hal; + +#include <sys/types.h> +#include <sys/stat.h> +#include <fcntl.h> +#include <unistd.h> + +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_hrt.h> + +#include <AP_Notify.h> + +uint16_t AP_InertialSensor_VRBRAIN::_init_sensor( Sample_rate sample_rate ) +{ + // assumes max 2 instances + _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY); + _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY); + _gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY); + _gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY); + + if (_accel_fd[0] < 0) { + hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH); + } + if (_gyro_fd[0] < 0) { + hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH); + } + _num_accel_instances = _accel_fd[1] >= 0?2:1; + _num_gyro_instances = _gyro_fd[1] >= 0?2:1; + + switch (sample_rate) { + case RATE_50HZ: + _default_filter_hz = 15; + _sample_time_usec = 20000; + break; + case RATE_100HZ: + _default_filter_hz = 30; + _sample_time_usec = 10000; + break; + case RATE_200HZ: + _default_filter_hz = 30; + _sample_time_usec = 5000; + break; + case RATE_400HZ: + default: + _default_filter_hz = 30; + _sample_time_usec = 2500; + break; + } + + _set_filter_frequency(_mpu6000_filter); + + + + + return AP_PRODUCT_ID_VRBRAIN; + +} + +/* + set the filter frequency + */ +void AP_InertialSensor_VRBRAIN::_set_filter_frequency(uint8_t filter_hz) +{ + if (filter_hz == 0) { + filter_hz = _default_filter_hz; + } + for (uint8_t i=0; i<_num_gyro_instances; i++) { + ioctl(_gyro_fd[i], GYROIOCSLOWPASS, filter_hz); + } + for (uint8_t i=0; i<_num_accel_instances; i++) { + ioctl(_accel_fd[i], ACCELIOCSLOWPASS, filter_hz); + } +} + +/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ + +// multi-device interface +bool AP_InertialSensor_VRBRAIN::get_gyro_health(uint8_t instance) const +{ + if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) { + // not initialised yet, show as healthy to prevent scary GCS + // warnings + return true; + } + if (instance >= _num_gyro_instances) { + return false; + } + + if ((_last_get_sample_timestamp - _last_gyro_timestamp[instance]) > 2*_sample_time_usec) { + // gyros have not updated + return false; + } + return true; +} + +uint8_t AP_InertialSensor_VRBRAIN::get_gyro_count(void) const +{ + return _num_gyro_instances; +} + +bool AP_InertialSensor_VRBRAIN::get_accel_health(uint8_t k) const +{ + if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) { + // not initialised yet, show as healthy to prevent scary GCS + // warnings + return true; + } + if (k >= _num_accel_instances) { + return false; + } + + if ((_last_get_sample_timestamp - _last_accel_timestamp[k]) > 2*_sample_time_usec) { + // accels have not updated + return false; + } + if (fabsf(_accel[k].x) > 30 && fabsf(_accel[k].y) > 30 && fabsf(_accel[k].z) > 30 && + (_previous_accel[k] - _accel[k]).length() < 0.01f) { + // unchanging accel, large in all 3 axes. This is a likely + // accelerometer failure of the LSM303d + return false; + } + return true; + +} + +uint8_t AP_InertialSensor_VRBRAIN::get_accel_count(void) const +{ + return _num_accel_instances; +} + +bool AP_InertialSensor_VRBRAIN::update(void) +{ + if (!wait_for_sample(100)) { + return false; + } + + // get the latest sample from the sensor drivers + _get_sample(); + + + for (uint8_t k=0; k<_num_accel_instances; k++) { + _previous_accel[k] = _accel[k]; + _accel[k] = _accel_in[k]; + _accel[k].rotate(_board_orientation); + _accel[k].x *= _accel_scale[k].get().x; + _accel[k].y *= _accel_scale[k].get().y; + _accel[k].z *= _accel_scale[k].get().z; + _accel[k] -= _accel_offset[k]; + } + + for (uint8_t k=0; k<_num_gyro_instances; k++) { + _gyro[k] = _gyro_in[k]; + _gyro[k].rotate(_board_orientation); + _gyro[k] -= _gyro_offset[k]; + } + + if (_last_filter_hz != _mpu6000_filter) { + _set_filter_frequency(_mpu6000_filter); + _last_filter_hz = _mpu6000_filter; + } + + _have_sample_available = false; + + return true; +} + +float AP_InertialSensor_VRBRAIN::get_delta_time(void) const +{ + return _sample_time_usec * 1.0e-6f; +} + +float AP_InertialSensor_VRBRAIN::get_gyro_drift_rate(void) +{ + // assume 0.5 degrees/second/minute + return ToRad(0.5/60); +} + +void AP_InertialSensor_VRBRAIN::_get_sample(void) +{ + for (uint8_t i=0; i<_num_accel_instances; i++) { + struct accel_report accel_report; + while (_accel_fd[i] != -1 && + ::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) && + accel_report.timestamp != _last_accel_timestamp[i]) { + _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z); + _last_accel_timestamp[i] = accel_report.timestamp; + } + } + for (uint8_t i=0; i<_num_gyro_instances; i++) { + struct gyro_report gyro_report; + while (_gyro_fd[i] != -1 && + ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) && + gyro_report.timestamp != _last_gyro_timestamp[i]) { + _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z); + _last_gyro_timestamp[i] = gyro_report.timestamp; + } + } + _last_get_sample_timestamp = hrt_absolute_time(); +} + +bool AP_InertialSensor_VRBRAIN::_sample_available(void) +{ + uint64_t tnow = hrt_absolute_time(); + while (tnow - _last_sample_timestamp > _sample_time_usec) { + _have_sample_available = true; + _last_sample_timestamp += _sample_time_usec; + } + return _have_sample_available; +} + +bool AP_InertialSensor_VRBRAIN::wait_for_sample(uint16_t timeout_ms) +{ + if (_sample_available()) { + return true; + } + uint32_t start = hal.scheduler->millis(); + while ((hal.scheduler->millis() - start) < timeout_ms) { + uint64_t tnow = hrt_absolute_time(); + // we spin for the last timing_lag microseconds. Before that + // we yield the CPU to allow IO to happen + const uint16_t timing_lag = 400; + if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) { + hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag)); + } + if (_sample_available()) { + return true; + } + } + return false; +} + +/** + try to detect bad accel/gyro sensors + */ +bool AP_InertialSensor_VRBRAIN::healthy(void) const +{ + return get_gyro_health(0) && get_accel_health(0); +} + +uint8_t AP_InertialSensor_VRBRAIN::_get_primary_gyro(void) const +{ + for (uint8_t i=0; i<_num_gyro_instances; i++) { + if (get_gyro_health(i)) return i; + } + return 0; +} + +uint8_t AP_InertialSensor_VRBRAIN::get_primary_accel(void) const +{ + for (uint8_t i=0; i<_num_accel_instances; i++) { + if (get_accel_health(i)) return i; + } + return 0; +} + +#endif // CONFIG_HAL_BOARD + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h new file mode 100644 index 0000000000000000000000000000000000000000..e908f87a942228ac607b6e35233a1c277e72007c --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h @@ -0,0 +1,71 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_INERTIAL_SENSOR_VRBRAIN_H__ +#define __AP_INERTIAL_SENSOR_VRBRAIN_H__ + +#include <AP_HAL.h> +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#include <AP_Progmem.h> +#include "AP_InertialSensor.h" +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> + +class AP_InertialSensor_VRBRAIN : public AP_InertialSensor +{ +public: + + AP_InertialSensor_VRBRAIN() : + AP_InertialSensor(), + _last_get_sample_timestamp(0), + _sample_time_usec(0) + { + } + + /* Concrete implementation of AP_InertialSensor functions: */ + bool update(); + float get_delta_time() const; + float get_gyro_drift_rate(); + bool wait_for_sample(uint16_t timeout_ms); + bool healthy(void) const; + + // multi-device interface + bool get_gyro_health(uint8_t instance) const; + uint8_t get_gyro_count(void) const; + + bool get_accel_health(uint8_t instance) const; + uint8_t get_accel_count(void) const; + + uint8_t get_primary_accel(void) const; + +private: + uint8_t _get_primary_gyro(void) const; + + uint16_t _init_sensor( Sample_rate sample_rate ); + void _get_sample(void); + bool _sample_available(void); + Vector3f _accel_in[INS_MAX_INSTANCES]; + Vector3f _gyro_in[INS_MAX_INSTANCES]; + uint64_t _last_accel_timestamp[INS_MAX_INSTANCES]; + uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES]; + uint64_t _last_get_sample_timestamp; + uint64_t _last_sample_timestamp; + uint32_t _sample_time_usec; + bool _have_sample_available; + + // support for updating filter at runtime + uint8_t _last_filter_hz; + uint8_t _default_filter_hz; + + void _set_filter_frequency(uint8_t filter_hz); + + // accelerometer and gyro driver handles + uint8_t _num_accel_instances; + uint8_t _num_gyro_instances; + int _accel_fd[INS_MAX_INSTANCES]; + int _gyro_fd[INS_MAX_INSTANCES]; +}; +#endif +#endif // __AP_INERTIAL_SENSOR_VRBRAIN_H__