diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ccd8c9ba4b..60ae288d3e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -193,7 +193,7 @@ DataFlash_APM1 DataFlash(&spi_semaphore); //////////////////////////////////////////////////////////////////////////////// // the rate we run the main loop at //////////////////////////////////////////////////////////////////////////////// -static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_100HZ; +static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_200HZ; //////////////////////////////////////////////////////////////////////////////// // Sensors @@ -953,7 +953,7 @@ void loop() // We want this to execute fast // ---------------------------- num_samples = ins.num_samples_available(); - if (num_samples >= 1) { + if (num_samples >= 2) { #if DEBUG_FAST_LOOP == ENABLED Log_Write_Data(DATA_FAST_LOOP, (int32_t)(timer - fast_loopTimer)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 9e18fd1aae..a673efb90f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -399,7 +399,7 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) case RATE_200HZ: default: rate = MPUREG_SMPLRT_200HZ; - default_filter = BITS_DLPF_CFG_98HZ; + default_filter = BITS_DLPF_CFG_42HZ; break; }