mahony.c File Reference

: Mahony attitude filter interface. More...

#include <math.h>
#include <stddef.h>
#include "mahony.h"

Macros
#define	MAHONY_ACCEL_MIN_MAGNITUDE   (0.85f * GRAVITY)
#define	MAHONY_ACCEL_MAX_MAGNITUDE   (1.15f * GRAVITY)
#define	MAHONY_INTEGRAL_LIMIT_RAD_S   0.25f

Functions
MAHONY_STATUS	mahony_init (MAHONY_FILTER *filter, QUAT initial_attitude, float proportional_gain, float integral_gain)
	Initializes a Mahony attitude filter.
MAHONY_STATUS	mahony_update_gyro (MAHONY_FILTER *filter, VECTOR_3F gyro_body_rad_s, float delta_time_s)
	Propagates attitude using body-frame gyroscope measurements.
MAHONY_STATUS	mahony_update_imu (MAHONY_FILTER *filter, VECTOR_3F gyro_body_rad_s, VECTOR_3F accel_body, float delta_time_s, bool use_accel)
	Updates attitude using gyroscope propagation and accelerometer proportional feedback.

Detailed Description

: Mahony attitude filter interface.

Copyright

Copyright (c) 2025 Sun Devil Rocketry. All rights reserved.

This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is covered under the BSD-3-Clause.

https://opensource.org/license/bsd-3-clause

==============================================================================
                      ##### Integration Guide #####
==============================================================================
[..]
(+) Initialize the filter with a pointer to an existing or uninitialized filter, 
    an initial attiutude estimate, and the proportional and integral gains for
    accelerometer correction.
(+) Call updates to the filter with gyroscope and accelerometer vectors and 
    time elapsed. Optionally disable accelerometer fusion during dynamic 
    flight.
******************************************************************************

Function Documentation

mahony_init()

MAHONY_STATUS mahony_init	(	MAHONY_FILTER *	filter,
		QUAT	initial_attitude,
		float	proportional_gain,
		float	integral_gain )

Initializes a Mahony attitude filter.

Parameters

filter	Filter instance to initialize.
initial_attitude	Initial body-to-world attitude quaternion.
proportional_gain	Proportional correction gain.
integral_gain	Integral correction gain.

Returns

MAHONY_OK when initialization succeeds; otherwise a Mahony status code describing the failure.

mahony_update_gyro()

MAHONY_STATUS mahony_update_gyro	(	MAHONY_FILTER *	filter,
		VECTOR_3F	gyro_body_rad_s,
		float	delta_time_s )

Propagates attitude using body-frame gyroscope measurements.

Verify the filter, gyro, and timestep are valid. Convert the body-frame angular velocity into a pure quaternion. Use the quaternion differential equation to calculate how quickly the body-to-world attitude is changing. Multiply that derivative by the timestep, add it to the current attitude, and normalize the result.

Parameters

filter	Initialized filter instance.
gyro_body_rad_s	Body-frame angular velocity in radians per second.
delta_time_s	Elapsed time in seconds.

Returns

MAHONY_OK when initialization succeeds; otherwise a Mahony status code describing the failure.

mahony_update_imu()

MAHONY_STATUS mahony_update_imu	(	MAHONY_FILTER *	filter,
		VECTOR_3F	gyro_body_rad_s,
		VECTOR_3F	accel_body,
		float	delta_time_s,
		bool	use_accel )

Updates attitude using gyroscope propagation and accelerometer proportional feedback.

The gyroscope must be expressed in the body frame in radians per second. The accelerometer must be expressed in the body frame. Its magnitude is removed internally because the filter uses only its measured direction.

Accelerometer feedback is applied only when the caller enables it and the measured acceleration magnitude falls within the configured validity range. Invalid accelerometer samples are ignored while gyro propagation continues.

Parameters

filter	Initialized filter instance.
gyro_body_rad_s	Body-frame angular velocity in radians per second.
accel_body	Body-frame accelerometer measurement.
delta_time_s	Elapsed time in seconds.
use_accel	Whether accelerometer feedback should be applied.

Returns

MAHONY_OK when the attitude was updated; otherwise a Mahony status code describing the failure.