~xdavidwu/motion-control

c98682fdcb9334d4dc1df85726e361fe064968e0 — xdavidwu 2 years ago c0a764a
gy-801-dump: perform quaternion forward integration
A ahrs/integration/integration.ha => ahrs/integration/integration.ha +24 -0
@@ 0,0 1,24 @@
use math;
use math::quaternion;

export fn new() integration = {
	return alloc(integration_impl {
		q = (1.0, 0.0, 0.0, 0.0),
	});
};

export fn update(integration: integration, readings: (f64, f64, f64), dt: f64) void = {
	let angle = math::sqrtf64(readings.0 * readings.0
		+ readings.1 * readings.1 + readings.2 * readings.2);
	let axis = (readings.0 / angle, readings.1 / angle, readings.2 / angle);
	let q = quaternion::from_axis_angle(axis, angle * dt);
	integration.q = quaternion::product(integration.q, q);
};

export fn read_euler(integration: integration) (f64, f64, f64) = {
	return quaternion::to_euler(integration.q);
};

export fn destroy(integration: integration) void = {
	free(integration);
};

A ahrs/integration/types.ha => ahrs/integration/types.ha +7 -0
@@ 0,0 1,7 @@
use math::quaternion;

export type integration_impl = struct {
	q: quaternion::quaternion,
};

export type integration = *integration_impl;

A math/quaternion/algebra.ha => math/quaternion/algebra.ha +8 -0
@@ 0,0 1,8 @@
export fn product(a: quaternion, b: quaternion) quaternion = {
	return (
		a.0 * b.0 - a.1 * b.1 - a.2 * b.2 - a.3 * b.3,
		a.0 * b.1 + a.1 * b.0 + a.2 * b.3 - a.3 * b.2,
		a.0 * b.2 - a.1 * b.3 + a.2 * b.0 + a.3 * b.1,
		a.0 * b.3 + a.1 * b.2 - a.2 * b.1 + a.3 * b.0 ,
	);
};

A math/quaternion/conversion.ha => math/quaternion/conversion.ha +27 -0
@@ 0,0 1,27 @@
use fmt;
use math;

// quarternion from axis-angle, angle is in radians
export fn from_axis_angle(axis: (f64, f64, f64), angle: f64) quaternion = {
	const angle = angle / 2.0f64;
	const s = math::sinf64(angle);
	return (math::cosf64(angle), axis.0 * s, axis.1 * s, axis.2 * s);
};

export fn to_euler(q: quaternion) (f64, f64, f64) = {
	return (
		math::atanf64(2.0 * (q.0 * q.1 + q.2 * q.3) / (1.0 - 2.0 * (q.1 * q.1 + q.2 * q.2))),
		math::asinf64(2.0 * (q.0 * q.2 - q.3 * q.1)),
		math::atanf64(2.0 * (q.0 * q.3 + q.1 * q.2) / (1.0 - 2.0 * (q.2 * q.2 + q.3 * q.3))),
	);
};

@test fn conversion() void = {
	let euler = (2.0 / 180.0 * math::PI, 3.0 / 180.0 * math::PI, 4.0 / 180.0 * math::PI);
	fmt::println(euler.0, euler.1, euler.2)!;
	let angle = math::sqrtf64(euler.0 * euler.0 + euler.1 * euler.1 + euler.2 * euler.2);
	let axis = (euler.0 / angle, euler.1 / angle, euler.2 / angle);
	let q = from_axis_angle(axis, angle);
	let res = to_euler(q);
	fmt::println(res.0, res.1, res.2)!;
};

A math/quaternion/types.ha => math/quaternion/types.ha +3 -0
@@ 0,0 1,3 @@
export type quaternion = (f64, f64, f64, f64);

//export def IDENTITY: quaternion	= (1.0, 0.0, 0.0, 0.0);

M tools/gy-801-dump/main.ha => tools/gy-801-dump/main.ha +12 -0
@@ 1,9 1,11 @@
use ahrs::integration;
use fmt;
use fs;
use io;
use os;
use rt;
use linux::timerfd;
use math;
use sensors::ADXL345;
use sensors::L3G4200D;
use strconv;


@@ 54,13 56,23 @@ export fn main() void = {
	defer io::close(fd)!;
	timerfd::set(fd, (500 * time::MILLISECOND): timerfd::interval, 0)!;
	signal::handle(signal::SIGINT, &sigint);
	const integrator = integration::new();

	for (!exit) {
		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);

		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)!;
	};
};