update gyro and accel values from backends */
| 1919 | update gyro and accel values from backends |
| 1920 | */ |
| 1921 | void AP_InertialSensor::update(void) |
| 1922 | { |
| 1923 | // during initialisation update() may be called without |
| 1924 | // wait_for_sample(), and a wait is implied |
| 1925 | wait_for_sample(); |
| 1926 | |
| 1927 | for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { |
| 1928 | // mark sensors unhealthy and let update() in each backend |
| 1929 | // mark them healthy via _publish_gyro() and |
| 1930 | // _publish_accel() |
| 1931 | _gyro_healthy[i] = false; |
| 1932 | _accel_healthy[i] = false; |
| 1933 | _delta_velocity_valid[i] = false; |
| 1934 | _delta_angle_valid[i] = false; |
| 1935 | } |
| 1936 | for (uint8_t i=0; i<_backend_count; i++) { |
| 1937 | _backends[i]->update(); |
| 1938 | } |
| 1939 | |
| 1940 | if (!_startup_error_counts_set) { |
| 1941 | for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { |
| 1942 | _accel_startup_error_count[i] = _accel_error_count[i]; |
| 1943 | _gyro_startup_error_count[i] = _gyro_error_count[i]; |
| 1944 | } |
| 1945 | |
| 1946 | if (_startup_ms == 0) { |
| 1947 | _startup_ms = AP_HAL::millis(); |
| 1948 | } else if (AP_HAL::millis()-_startup_ms > 2000) { |
| 1949 | _startup_error_counts_set = true; |
| 1950 | } |
| 1951 | } |
| 1952 | |
| 1953 | for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { |
| 1954 | if (_accel_error_count[i] < _accel_startup_error_count[i]) { |
| 1955 | _accel_startup_error_count[i] = _accel_error_count[i]; |
| 1956 | } |
| 1957 | if (_gyro_error_count[i] < _gyro_startup_error_count[i]) { |
| 1958 | _gyro_startup_error_count[i] = _gyro_error_count[i]; |
| 1959 | } |
| 1960 | } |
| 1961 | |
| 1962 | // adjust health status if a sensor has a non-zero error count |
| 1963 | // but another sensor doesn't. |
| 1964 | bool have_zero_accel_error_count = false; |
| 1965 | bool have_zero_gyro_error_count = false; |
| 1966 | for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { |
| 1967 | if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) { |
| 1968 | have_zero_accel_error_count = true; |
| 1969 | } |
| 1970 | if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) { |
| 1971 | have_zero_gyro_error_count = true; |
| 1972 | } |
| 1973 | } |
| 1974 | |
| 1975 | for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { |
| 1976 | if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) { |
| 1977 | // we prefer not to use a gyro that has had errors |
| 1978 | _gyro_healthy[i] = false; |
no test coverage detected