AP_InertialSensor: panic on bad gyro or accel sample rates

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -122,7 +122,11 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
                 break;
         }
         // calculate gyro sample time
-        _gyro_sample_time[i] = 1.0f / ioctl(fd,  GYROIOCGSAMPLERATE, 0);
+        int samplerate = ioctl(fd,  GYROIOCGSAMPLERATE, 0);
+        if (samplerate < 100 || samplerate > 10000) {
+            hal.scheduler->panic("Invalid gyro sample rate");
+        }
+        _gyro_sample_time[i] = 1.0f / samplerate;
     }
 
     for (uint8_t i=0; i<_num_accel_instances; i++) {
@@ -156,7 +160,11 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
                 break;
         }
         // calculate accel sample time
-        _accel_sample_time[i] = 1.0f / ioctl(fd,  ACCELIOCGSAMPLERATE, 0);
+        int samplerate = ioctl(fd,  ACCELIOCGSAMPLERATE, 0);
+        if (samplerate < 100 || samplerate > 10000) {
+            hal.scheduler->panic("Invalid accel sample rate");
+        }
+        _accel_sample_time[i] = 1.0f / samplerate;
     }
 
     _set_accel_filter_frequency(_accel_filter_cutoff());
@@ -180,13 +188,8 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
 void AP_InertialSensor_PX4::_set_accel_filter_frequency(uint8_t filter_hz)
 {
     for (uint8_t i=0; i<_num_accel_instances; i++) {
-        int samplerate = ioctl(_accel_fd[i],  ACCELIOCGSAMPLERATE, 0);
-        if(samplerate < 100 || samplerate > 2000) {
-            // sample rate doesn't seem sane, turn off filter
-            _accel_filter[i].set_cutoff_frequency(0, 0);
-        } else {
-            _accel_filter[i].set_cutoff_frequency(samplerate, filter_hz);
-        }
+        float samplerate = 1.0f / _accel_sample_time[i];
+        _accel_filter[i].set_cutoff_frequency(samplerate, filter_hz);
     }
 }
 
@@ -196,13 +199,8 @@ void AP_InertialSensor_PX4::_set_accel_filter_frequency(uint8_t filter_hz)
 void AP_InertialSensor_PX4::_set_gyro_filter_frequency(uint8_t filter_hz)
 {
     for (uint8_t i=0; i<_num_gyro_instances; i++) {
-        int samplerate = ioctl(_gyro_fd[i],  GYROIOCGSAMPLERATE, 0);
-        if(samplerate < 100 || samplerate > 2000) {
-            // sample rate doesn't seem sane, turn off filter
-            _gyro_filter[i].set_cutoff_frequency(0, 0);
-        } else {
-            _gyro_filter[i].set_cutoff_frequency(samplerate, filter_hz);
-        }
+        float samplerate = 1.0f / _gyro_sample_time[i];
+        _gyro_filter[i].set_cutoff_frequency(samplerate, filter_hz);
     }
 }