From cf4d9379a28c1a19aa420dc9830360979f1cada3 Mon Sep 17 00:00:00 2001 From: xdavidwu Date: Fri, 20 May 2022 15:41:38 +0800 Subject: [PATCH] sensors::L3G4200D: enable high-pass filter @0.05hz this helps with fighting nearly-constant drift --- sensors/L3G4200D/L3G4200D.ha | 12 +++++++----- sensors/L3G4200D/types.ha | 7 +++++++ 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/sensors/L3G4200D/L3G4200D.ha b/sensors/L3G4200D/L3G4200D.ha index 4267223..f12d19b 100644 --- a/sensors/L3G4200D/L3G4200D.ha +++ b/sensors/L3G4200D/L3G4200D.ha @@ -17,14 +17,16 @@ export fn new(bus: int) (L3G4200D | fs::error | rt::errno) = { }; export fn init(sensor: L3G4200D) (void | rt::errno) = { - i2c::smbus::write_byte_data(sensor.file, REG_CTRL_REG1, - CTRL_REG1_PD | CTRL_REG1_Zen | CTRL_REG1_Yen | CTRL_REG1_Xen)?; - i2c::smbus::write_byte_data(sensor.file, REG_CTRL_REG4, CTRL_REG4_BDU)?; + smbus::write_byte_data(sensor.file, REG_CTRL_REG1, + CTRL_REG1_PD | CTRL_REG1_Zen | CTRL_REG1_Yen | CTRL_REG1_Xen | CTRL_REG1_DR_100HZ)?; + smbus::write_byte_data(sensor.file, REG_CTRL_REG2, CTRL_REG2_HPCF_0_05HZ_AT_100HZ)?; + smbus::write_byte_data(sensor.file, REG_CTRL_REG4, CTRL_REG4_BDU)?; + smbus::write_byte_data(sensor.file, REG_CTRL_REG5, CTRL_REG5_OUT_HP_ONLY)?; }; fn read_data(sensor: L3G4200D, reg: u8) (f64 | rt::errno) = { - const raw = i2c::smbus::read_byte_data(sensor.file, reg)?; - const raw2 = i2c::smbus::read_byte_data(sensor.file, reg + 1)?; + const raw = smbus::read_byte_data(sensor.file, reg)?; + const raw2 = smbus::read_byte_data(sensor.file, reg + 1)?; const raw = (raw: u16 + (raw2: u16 << 8)): i16; return raw: f64 * FS_250DPS_So; }; diff --git a/sensors/L3G4200D/types.ha b/sensors/L3G4200D/types.ha index fa179af..7fcdb34 100644 --- a/sensors/L3G4200D/types.ha +++ b/sensors/L3G4200D/types.ha @@ -7,14 +7,21 @@ export type L3G4200D_impl = struct { export type L3G4200D = *L3G4200D_impl; def REG_CTRL_REG1: u8 = 0x20; +def CTRL_REG1_DR_100HZ: u8 = 0b00 << 6; def CTRL_REG1_PD: u8 = 1 << 3; def CTRL_REG1_Zen: u8 = 1 << 2; def CTRL_REG1_Yen: u8 = 1 << 1; def CTRL_REG1_Xen: u8 = 1 << 0; +def REG_CTRL_REG2: u8 = 0x21; +def CTRL_REG2_HPCF_0_05HZ_AT_100HZ: u8 = 0b111; + def REG_CTRL_REG4: u8 = 0x23; def CTRL_REG4_BDU: u8 = 1 << 7; +def REG_CTRL_REG5: u8 = 0x24; +def CTRL_REG5_OUT_HP_ONLY: u8 = 0b01; + def REG_OUT_X_L: u8 = 0x28; def REG_OUT_Y_L: u8 = 0x2a; def REG_OUT_Z_L: u8 = 0x2c; -- 2.45.2