2 files changed, 63 insertions(+), 0 deletions(-)

A sensors/L3G4200D/L3G4200D.ha
A sensors/L3G4200D/types.ha

@@ 0,0 1,41 @@
+use fmt;
+use fs;
+use i2c;
+use i2c::smbus;
+use io;
+use os;
+use rt;
+
+export fn new(bus: int) (L3G4200D | fs::error | rt::errno) = {
+	const path = fmt::asprintf("/dev/i2c-{}", bus);
+	defer free(path);
+	const file = os::open(path, fs::flags::RDONLY)?;
+	rt::ioctl(file, i2c::I2C_SLAVE, 0x69)?;
+	return alloc(L3G4200D_impl {
+		file = file,
+	});
+};
+
+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)?;
+};
+
+fn read_data(sensor: L3G4200D, reg: u8) (f64 | rt::errno) = {
+	const raw = i2c::smbus::read_word_data(sensor.file, reg)?;
+	const raw = raw: i16;
+	return raw: f64 * FS_250DPS_So;
+};
+
+export fn read(sensor: L3G4200D) ((f64, f64, f64) | rt::errno) = {
+	return (
+		read_data(sensor, REG_OUT_X_L)?,
+		read_data(sensor, REG_OUT_Y_L)?,
+		read_data(sensor, REG_OUT_Z_L)?,
+	);
+};
+
+export fn destroy(sensor: L3G4200D) void = {
+	io::close(sensor.file)!;
+	free(sensor);
+};

@@ 0,0 1,22 @@
+use io;
+
+export type L3G4200D_impl = struct {
+	file: io::file,
+};
+
+export type L3G4200D = *L3G4200D_impl;
+
+def REG_CTRL_REG1: u8	= 0x20;
+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_REG4: u8	= 0x23;
+def CTRL_REG4_BDU: u8	= 1 << 7;
+
+def REG_OUT_X_L: u8	= 0x28;
+def REG_OUT_Y_L: u8	= 0x2a;
+def REG_OUT_Z_L: u8	= 0x2c;
+
+def FS_250DPS_So: f64	= 8.75f64 / 1000.0f64;