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