math_sdr.c File Reference

: Contains math and utility functions for SDR code. More...

#include <math.h>
#include "main.h"
#include "math_sdr.h"

Functions
uint32_t	crc32 (const uint8_t *data, size_t len)
	Computes a CRC-32 checksum over a block of data.
float	clamp_float (float value, float minimum, float maximum)
	Clamps a floating-point value between minimum and maximum bounds.
QUAT	eul_to_quat (float yaw, float pitch, float roll)
	Converts ZYX Euler angles to a quaternion.
QUAT	quat_mult (QUAT a, QUAT b)
	Hamilton product of two quaternions.
float	quat_dot (QUAT a, QUAT b)
	Provides the dot product of two quaternions.
QUAT	quat_add (QUAT a, QUAT b)
	Adds two quaternions component-wise.
QUAT	quat_scale (QUAT q, float s)
	Scales a quaternion by a scalar value.
QUAT	quat_normalize (QUAT q)
	Normalizes a quaternion to unit length.
QUAT	quat_conj (QUAT q)
	Computes the conjugate of a quaternion.
QUAT	quat_rotate_world_to_body (QUAT attitude, QUAT vector_world)
	Rotates a vector from the world frame into the body frame.
QUAT	quat_rotate_body_to_world (QUAT attitude, QUAT vector_body)
	Rotates a vector from the body frame into the world frame.
bool	quat_is_finite (QUAT quaternion)
	Determines whether every quaternion component is finite.
bool	vector_is_finite (VECTOR_3F vector)
	Determines whether every vector component is finite.
float	vector_magnitude (VECTOR_3F vector)
	Calculates the magnitude of a three-dimensional vector.
bool	vector_normalize (VECTOR_3F *vector)
	Normalizes a three-dimensional vector in place.
VECTOR_3F	vector_cross (VECTOR_3F a, VECTOR_3F b)
	Calculates the cross product of two three-dimensional vectors.
VECTOR_3F	vector_add (VECTOR_3F a, VECTOR_3F b)
	Adds two three-dimensional vectors.
VECTOR_3F	vector_scale (VECTOR_3F vector, float scalar)
	Scales a three-dimensional vector by a scalar.

Detailed Description

: Contains math and utility functions for SDR code.

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

Function Documentation

clamp_float()

float clamp_float	(	float	value,
		float	minimum,
		float	maximum )

Clamps a floating-point value between minimum and maximum bounds.

Parameters

value	Value to clamp.
minimum	Minimum value.
maximum	Maximum value.

crc32()

uint32_t crc32	(	const uint8_t *	data,
		size_t	len )

Computes a CRC-32 checksum over a block of data.

Parameters

[in]	data	Pointer to the input data buffer.
[in]	len	Number of bytes in the buffer.

Returns

The 32-bit CRC checksum of the input data.

eul_to_quat()

QUAT eul_to_quat	(	float	yaw,
		float	pitch,
		float	roll )

Converts ZYX Euler angles to a quaternion.

Note

Angles must be provided in radians.

The rotation order is ZYX (standard aerospace convention).

Todo

The rotation sequence has not been validated yet.

Parameters

yaw	Rotation about the Z axis in radians.
pitch	Rotation about the Y axis in radians.
roll	Rotation about the X axis in radians.

Returns

The quaternion representing the rotation.

quat_add()

QUAT quat_add	(	QUAT	a,
		QUAT	b )

Adds two quaternions component-wise.

Parameters

a	The first quaternion.
b	The second quaternion.

Returns

The quaternion sum a + b.

quat_conj()

QUAT quat_conj

(

QUAT

q

)

Computes the conjugate of a quaternion.

Parameters

q	The input quaternion.

Returns

The conjugate quaternion (w, -x, -y, -z).

quat_dot()

float quat_dot	(	QUAT	a,
		QUAT	b )

Provides the dot product of two quaternions.

Note

Quaternion dot products are commutative.

Parameters

a	The left-hand quaternion.
b	The right-hand quaternion.

Returns

The quaternion scalar product a . b

quat_mult()

QUAT quat_mult	(	QUAT	a,
		QUAT	b )

Hamilton product of two quaternions.

Note

Quaternion multiplication is NOT commutative.

Parameters

a	The left-hand quaternion.
b	The right-hand quaternion.

Returns

The quaternion product a * b.

quat_normalize()

QUAT quat_normalize

(

QUAT

q

)

Normalizes a quaternion to unit length.

Parameters

q	The quaternion to normalize.

Returns

The normalized unit quaternion.

Divides each component by the quaternion's norm. If the norm is zero (e.g. a zero-initialized quaternion), returns the identity quaternion (1, 0, 0, 0) to avoid a divide-by-zero. Values near but not equal to zero are divided normally and may exhibit floating-point roundoff.

quat_rotate_body_to_world()

QUAT quat_rotate_body_to_world	(	QUAT	attitude,
		QUAT	vector_body )

Rotates a vector from the body frame into the world frame.

The attitude quaternion represents the body-to-world rotation:

vector_world = q * vector_body * conjugate(q)

Parameters

attitude	Body-to-world attitude quaternion.
vector_body	Pure quaternion containing the body-frame vector.

Returns

Pure quaternion containing the world-frame vector.

quat_rotate_world_to_body()

QUAT quat_rotate_world_to_body	(	QUAT	attitude,
		QUAT	vector_world )

Rotates a vector from the world frame into the body frame.

The attitude quaternion represents the body-to-world rotation:

vector_body = conjugate(q) * vector_world * q

Parameters

attitude	Body-to-world attitude quaternion.
vector_world	Pure quaternion containing the world-frame vector.

Returns

Pure quaternion containing the body-frame vector.

quat_scale()

QUAT quat_scale	(	QUAT	q,
		float	s )

Scales a quaternion by a scalar value.

Parameters

q	The quaternion to scale.
s	The scalar factor.

Returns

The quaternion q scaled by s (q * s).