diff options
| author | Julian Oes <julian@oes.ch> | 2013-07-30 15:10:22 +0200 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2013-07-30 18:20:31 +0200 |
| commit | 7e9a18da795e56e229957dba47ed7468eac10697 (patch) | |
| tree | ffc3f7ac20f8ef1a46460fcb4144382ba3d0f303 /src/drivers/l3gd20 | |
| parent | 1543c990031941cde44c9674cdcc571de53162f5 (diff) | |
| download | px4-firmware-7e9a18da795e56e229957dba47ed7468eac10697.tar.gz px4-firmware-7e9a18da795e56e229957dba47ed7468eac10697.tar.bz2 px4-firmware-7e9a18da795e56e229957dba47ed7468eac10697.zip | |
Changed gyro scaling according to datasheet
Diffstat (limited to 'src/drivers/l3gd20')
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 10 |
1 files changed, 7 insertions, 3 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a9af6711b..423adc76d 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -559,28 +559,32 @@ int L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; + float new_range_scale_dps_digit; - if (max_dps == 0) + if (max_dps == 0) { max_dps = 2000; - + } if (max_dps <= 250) { _current_range = 250; bits |= RANGE_250DPS; + new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { _current_range = 500; bits |= RANGE_500DPS; + new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { _current_range = 2000; bits |= RANGE_2000DPS; + new_range_scale_dps_digit = 70e-3f; } else { return -EINVAL; } _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; - _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); return OK; |