INS: set default filter to 20hz for APM2.x and PX4

This commit is contained in:
Randy Mackay 2013-03-01 19:35:34 +09:00
parent 6dd549dea8
commit 318a831b57
2 changed files with 4 additions and 4 deletions

View File

@ -575,12 +575,12 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
_sample_shift = 2;
break;
case RATE_100HZ:
default_filter = BITS_DLPF_CFG_42HZ;
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 1;
break;
case RATE_200HZ:
default:
default_filter = BITS_DLPF_CFG_42HZ;
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 0;
break;
}

View File

@ -33,12 +33,12 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
break;
case RATE_100HZ:
_sample_divider = 2;
_default_filter_hz = 42;
_default_filter_hz = 20;
break;
case RATE_200HZ:
default:
_sample_divider = 1;
_default_filter_hz = 42;
_default_filter_hz = 20;
break;
}