From 7baf72f5f8f5e69274037360a21522ff3cac830d Mon Sep 17 00:00:00 2001 From: xdavidwu Date: Tue, 17 May 2022 21:37:20 +0800 Subject: [PATCH] gy-801-dump: bump gyro read rate --- tools/gy-801-dump/main.ha | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/tools/gy-801-dump/main.ha b/tools/gy-801-dump/main.ha index f4293fa..0b15473 100644 --- a/tools/gy-801-dump/main.ha +++ b/tools/gy-801-dump/main.ha @@ -54,25 +54,29 @@ export fn main() void = { const fd = timerfd::new(time::clock::MONOTONIC, 0)!; defer io::close(fd)!; - timerfd::set(fd, (500 * time::MILLISECOND): timerfd::interval, 0)!; + timerfd::set(fd, (10 * time::MILLISECOND): timerfd::interval, 0)!; signal::handle(signal::SIGINT, &sigint); const integrator = integration::new(); - for (!exit) { + for (let i = 0; !exit; i += 1) { timerfd::read(fd)!; - const acc_readings = ADXL345::read(ADXL345)!; const gyro_readings = L3G4200D::read(L3G4200D)!; let rps = (gyro_readings.0 / 180.0 * math::PI, gyro_readings.1 / 180.0 * math::PI, gyro_readings.2 / 180.0 * math::PI); - integration::update(integrator, rps, 0.5); - let attitude = integration::read_euler(integrator); + integration::update(integrator, rps, 0.01); + if (i % 50 == 0) { + const acc_readings = ADXL345::read(ADXL345)!; + let attitude = integration::read_euler(integrator); - fmt::printfln("Accelerometer: {} {} {} g", acc_readings.0, acc_readings.1, acc_readings.2)!; - fmt::printfln("Gyroscope: {} {} {} dps", gyro_readings.0, gyro_readings.1, gyro_readings.2)!; - fmt::printfln("Attitude: {} {} {} deg", - attitude.0 / math::PI * 180.0, - attitude.1 / math::PI * 180.0, - attitude.2 / math::PI * 180.0)!; + fmt::printfln("Accelerometer: {} {} {} g", + acc_readings.0, acc_readings.1, acc_readings.2)!; + fmt::printfln("Gyroscope: {} {} {} dps", + gyro_readings.0, gyro_readings.1, gyro_readings.2)!; + fmt::printfln("Attitude: {} {} {} deg", + attitude.0 / math::PI * 180.0, + attitude.1 / math::PI * 180.0, + attitude.2 / math::PI * 180.0)!; + }; }; }; -- 2.45.2