Files
file	coordinate_conversions.c
	General conversions with different coordinate systems.

file	coordinate_conversions.h
	Header for Coordinate conversions library in coordinate_conversions.c.

file	lqg.c
	LQG Control algorithm.

file	lqg.h
	LQG Control algorithm.

file	pid.c
	PID Control algorithms.

file	pid.h
	PID Control algorithms.

Data Structures
struct	lpfilter_biquad_state

struct	lpfilter_biquad

struct	lpfilter_first_order

struct	lpfilter_state

struct	rtkf_state

struct	lqr_state

struct	lqg_state

struct	pid_deadband

struct	pid

Macros
#define	MAX_FILTER_WIDTH 16

#define	SOLVER_MIN 100

#define	SOLVER_MAX 11000

#define	SOLVER_KF_TORQUE_EPSILON 0.000000001f

#define	SOLVER_KF_BIAS_EPSILON 0.000000001f

#define	SOLVER_LQR_RATE_EPSILON 0.00000001f

#define	SOLVER_LQR_TORQUE_EPSILON 0.000001f

#define	P00 P[0][0]

#define	P10 P[1][0]

#define	P20 P[2][0]

#define	P01 P[0][1]

#define	P11 P[1][1]

#define	P21 P[2][1]

#define	P02 P[0][2]

#define	P12 P[1][2]

#define	P22 P[2][2]

#define	A00 A[0][0]

#define	A10 A[1][0]

#define	A20 A[2][0]

#define	A01 A[0][1]

#define	A11 A[1][1]

#define	A21 A[2][1]

#define	A02 A[0][2]

#define	A12 A[1][2]

#define	A22 A[2][2]

#define	Q00 Q[0][0]

#define	Q10 Q[1][0]

#define	Q20 Q[2][0]

#define	Q01 Q[0][1]

#define	Q11 Q[1][1]

#define	Q21 Q[2][1]

#define	Q02 Q[0][2]

#define	Q12 Q[1][2]

#define	Q22 Q[2][2]

#define	B0 B[0]

#define	B1 B[1]

#define	B2 B[2]

#define	X0 X[0]

#define	X1 X[1]

#define	X2 X[2]

#define	K0 K[0]

#define	K1 K[1]

#define	K2 K[2]

#define	LQG_SOLVER_FAILED -1

#define	LQG_SOLVER_RUNNING 0

#define	LQG_SOLVER_DONE 1

#define	MAX(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a > _b ? _a : _b; })

#define	MIN(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a < _b ? _a : _b; })

#define	sign(x) ((x) < 0 ? -1 : 1)
	This is but one definition of sign(.) More...

#define	TOL_EPS 0.0001f

#define	PSEUDOINV_CONVERGE_LIMIT 75

#define	matrix_mul_check(a, b, out, arows, acolsbrows, bcols)

#define	matrix_mul_scalar_check(a, scalar, out, rows, cols)

#define	matrix_add_check(a, b, out, rows, cols)

#define	matrix_sub_check(a, b, out, rows, cols)

#define	matrix_transpose_check(a, b, out, rows, cols)

#define	cast_uint32_t (uint32_t)

#define	powapprox powf

#define	expapprox expf

Typedefs
typedef struct lpfilter_state *	lpfilter_state_t

typedef struct rtkf_state *	rtkf_t

typedef struct lqr_state *	lqr_t

typedef struct lqg_state *	lqg_t

Functions
void	RneFromLLA (float LLA[3], float Rne[3][3])

void	Quaternion2RPY (const float q[4], float rpy[3])

void	RPY2Quaternion (const float rpy[3], float q[4])

void	Quaternion2R (float q[4], float Rbe[3][3])

void	Euler2R (float rpy[3], float Rbe[3][3])

void	R2Quaternion (float R[3][3], float q[4])

uint8_t	RotFrom2Vectors (const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3])

void	Rv2Rot (float Rv[3], float R[3][3])

void	CrossProduct (const float v1[3], const float v2[3], float result[3])

float	VectorMagnitude (const float v[3])

void	quat_inverse (float q[4])
	Compute the inverse of a quaternion. More...

void	quat_copy (const float q[4], float qnew[4])
	Duplicate a quaternion. More...

void	quat_mult (const float q1[4], const float q2[4], float qout[4])
	Multiply two quaternions into a third. More...

void	rot_mult (float R[3][3], const float vec[3], float vec_out[3], bool transpose)
	Rotate a vector by a rotation matrix. More...

void	lpfilter_construct_single_biquad (struct lpfilter_biquad *b, float cutoff, float dT, float q, uint8_t width)

void	lpfilter_construct_biquads (lpfilter_state_t filt, float cutoff, float dT, int o, uint8_t width)

void	lpfilter_create (lpfilter_state_t *filter_ptr, float cutoff, float dT, uint8_t order, uint8_t width)

float	lpfilter_run_single (lpfilter_state_t filter, uint8_t axis, float sample)

void	lpfilter_run (lpfilter_state_t filter, float *sample)

bool	rtkf_calculate_covariance_3x3 (float A[3][3], float K[3], float P[3][3], float Q[3][3], float R)

void	rtkf_stabilize_covariance (rtkf_t rtkf, int iterations)

void	rtkf_prediction_step (float A[3][3], float B[3], float K[3], float X[3], float signal, float input)

void	rtkf_predict_axis (rtkf_t rtkf, float signal, float input, float Xout[3])

void	rtkf_initialize_matrices_int (float A[3][3], float B[3], float beta, float tau, float Ts)

rtkf_t	rtkf_create (float beta, float tau, float Ts, float R, float q1, float q2, float q3, float biaslim)

int	rtkf_solver_status (rtkf_t rtkf)

bool	lqr_calculate_covariance_2x2 (float A[2][2], float B[2], float K[2], float P[2][2], float Q[2][2], float R)

void	lqr_stabilize_covariance (lqr_t lqr, int iterations)

int	lqr_solver_status (lqr_t lqr)

void	lqr_initialize_matrices_int (float A[2][2], float B[2], float beta, float tau, float Ts)

lqr_t	lqr_create (float beta, float tau, float Ts, float q1, float q2, float r)

void	lqr_update (lqr_t lqr, float q1, float q2, float r)

void	lqr_get_gains (lqr_t lqr, float K[2])

float	lqg_controller (lqg_t lqg, float signal, float setpoint)

lqg_t	lqg_create (rtkf_t rtkf, lqr_t lqr)

void	lqg_get_rtkf_state (lqg_t lqg, float rate, float torque, float *bias)

void	lqg_set_x0 (lqg_t lqg, float x0)

int	lqg_solver_status (lqg_t lqg)

rtkf_t	lqg_get_rtkf (lqg_t lqg)

lqr_t	lqg_get_lqr (lqg_t lqg)

void	lqg_run_covariance (lqg_t lqg, int iter)

float	bound_min_max (float val, float min, float max)
	Bound input value between min and max. More...

float	bound_sym (float val, float range)
	Bound input value within range (plus or minus) More...

float	circular_modulus_deg (float err)
	Circular modulus. More...

float	circular_modulus_rad (float err)

float	expo3 (float x, int32_t g)
	Approximation an exponential scale curve. More...

float	expoM (float x, int32_t g, float exponent)

float	interpolate_value (const float fraction, const float beginVal, const float endVal)

float	vectorn_magnitude (const float *v, int n)

float	vector3_distances (const float actual, const float desired, float *out, bool normalize)

void	vector2_clip (float *vels, float limit)

void	vector2_rotate (const float original, float out, float angle)

float	cubic_deadband (float in, float w, float b, float m, float r)

void	cubic_deadband_setup (float w, float b, float m, float r)

float	linear_interpolate (float const input, float const *curve, uint8_t num_points, const float input_min, const float input_max)

void	randomize_addseed (uint32_t seed)

static uint32_t	randomize_int32 ()

uint32_t	randomize_int (uint32_t interval)

void	apply_channel_deadband (float *value, float deadband)
	Apply deadband to Roll/Pitch/Yaw channels. More...

static bool	IS_NOT_FINITE (float x)

static int16_t	sin_approx (int32_t x)
	Fast approximation of sine; 3rd order taylor expansion. Based on http://www.coranac.com/2009/07/sines/. More...

static void	matrix_mul (const float a, const float b, float *out, int arows, int acolsbrows, int bcols)
	Multiplies out = a b. More...

static void	matrix_mul_scalar (const float a, float scalar, float out, int rows, int cols)
	Multiplies a matrix by a scalar Can safely be done in-place. (e.g. is slow and not vectorized/unrolled) More...

static void	matrix_add (const float a, const float b, float *out, int rows, int cols)
	Adds two matrices Can safely be done in-place. (e.g. is slow and not vectorized/unrolled) More...

static void	matrix_sub (const float a, const float b, float *out, int rows, int cols)
	Subtracts two matrices Can safely be done in-place. (e.g. is slow and not vectorized/unrolled) More...

static void	matrix_transpose (const float a, float out, int arows, int acols)
	Transposes a matrix. More...

static bool	matrix_pseudoinv_convergecheck (const float a, const float b, const float *prod, int rows, int cols)

static bool	matrix_pseudoinv_step (const float a, float ainv, int arows, int acols)

static float	matrix_getmaxabs (const float *a, int arows, int acols)
	Finds the largest value in a matrix. More...

static bool	matrix_pseudoinv (const float a, float out, int arows, int acols)
	Finds a pseudo-inverse of a matrix. More...

static float	fastlog2 (float x)

static float	fastpow2 (float p)

static float	fastpow (float x, float p)

static float	fastexp (float p)

float	pid_apply (struct pid *pid, const float err)
	Methods to use the pid structures. More...

float	pid_apply_antiwindup (struct pid *pid, const float err, float min_bound, float max_bound, float aw_bound)

float	pid_apply_setpoint (struct pid pid, struct pid_deadband deadband, const float setpoint, const float measured)

float	pid_apply_setpoint_antiwindup (struct pid pid, struct pid_deadband deadband, const float setpoint, const float measured, float min_bound, float max_bound, float aw_bound)

void	pid_zero (struct pid *pid)

void	pid_configure_derivative (float cutoff, float g)
	Configure the common terms that alter ther derivative. More...

void	pid_configure (struct pid *pid, float p, float i, float d, float iLim, float dT)

void	pid_configure_deadband (struct pid_deadband *deadband, float width, float slope)

Variables
static const float	lpfilter_butterworth_factors [16]

float	lpfilter_biquad_state::x1

float	lpfilter_biquad_state::x2

float	lpfilter_biquad_state::y1

float	lpfilter_biquad_state::y2

float	lpfilter_biquad::b0

float	lpfilter_biquad::a1

float	lpfilter_biquad::a2

struct lpfilter_biquad_state *	lpfilter_biquad::s

float	lpfilter_first_order::alpha

float *	lpfilter_first_order::prev

struct lpfilter_first_order *	lpfilter_state::first_order

struct lpfilter_biquad *	lpfilter_state::biquad [4]

uint8_t	lpfilter_state::order

uint8_t	lpfilter_state::width

static uint32_t	random_state [4] = { 0xdeadbeef, 0xfeedfeed, 0xcafebabe, 1234 }

static float	deriv_tau = 7.9577e-3f
	Store the shared time constant for the derivative cutoff. More...

static float	deriv_gamma = 1.0
	Store the setpoint weight to apply for the derivative term. More...

Detailed Description

Macro Definition Documentation

#define A00 A[0][0]

Definition at line 87 of file lqg.c.

#define A01 A[0][1]

Definition at line 90 of file lqg.c.

#define A02 A[0][2]

Definition at line 93 of file lqg.c.

#define A10 A[1][0]

Definition at line 88 of file lqg.c.

#define A11 A[1][1]

Definition at line 91 of file lqg.c.

#define A12 A[1][2]

Definition at line 94 of file lqg.c.

#define A20 A[2][0]

Definition at line 89 of file lqg.c.

#define A21 A[2][1]

Definition at line 92 of file lqg.c.

#define A22 A[2][2]

Definition at line 95 of file lqg.c.

#define B0 B[0]

Definition at line 107 of file lqg.c.

#define B1 B[1]

Definition at line 108 of file lqg.c.

#define B2 B[2]

Definition at line 109 of file lqg.c.

#define cast_uint32_t (uint32_t)

Definition at line 537 of file misc_math.h.

#define expapprox expf

Definition at line 579 of file misc_math.h.

#define K0 K[0]

Definition at line 115 of file lqg.c.

#define K1 K[1]

Definition at line 116 of file lqg.c.

#define K2 K[2]

Definition at line 117 of file lqg.c.

#define LQG_SOLVER_DONE 1

Definition at line 38 of file lqg.h.

#define LQG_SOLVER_FAILED -1

Definition at line 36 of file lqg.h.

#define LQG_SOLVER_RUNNING 0

Definition at line 37 of file lqg.h.

#define matrix_add_check	(	a,
		b,
		out,
		rows,
		cols
	)

Value:

do { \
                /* Note the dimensions each get used multiple times */  \
                const float (*my_a)[rows*cols] = &(a); \
                const float (*my_b)[rows*cols] = &(b); \
                float (*my_out)[rows*cols] = &(out); \
		matrix_add(*my_a, *my_b, *my_out, rows, cols) \
        } while (0);

Definition at line 464 of file misc_math.h.

#define matrix_mul_check	(	a,
		b,
		out,
		arows,
		acolsbrows,
		bcols
	)

Value:

do { \
                /* Note the dimensions each get used multiple times */  \
                const float (*my_a)[arows*acolsbrows] = &(a); \
                const float (*my_b)[acolsbrows*bcols] = &(b); \
                float (*my_out)[arows*bcols] = &(out); \
		matrix_mul(*my_a, *my_b, *my_out, arows, acolsbrows, bcols); \
        } while (0);

Definition at line 447 of file misc_math.h.

#define matrix_mul_scalar_check	(	a,
		scalar,
		out,
		rows,
		cols
	)

Value:

do { \
                /* Note the dimensions each get used multiple times */  \
                const float (*my_a)[rows*cols] = &(a); \
                float (*my_out)[rows*cols] = &(out); \
		matrix_mul_scalar(*my_a, scalar, my_out, rows, cols); \
        } while (0);

Definition at line 456 of file misc_math.h.

#define matrix_sub_check	(	a,
		b,
		out,
		rows,
		cols
	)

Value:

do { \
                /* Note the dimensions each get used multiple times */  \
                const float (*my_a)[rows*cols] = &(a); \
                const float (*my_b)[rows*cols] = &(b); \
                float (*my_out)[rows*cols] = &(out); \
		matrix_sub(*my_a, *my_b, *my_out, rows, cols) \
        } while (0);

Definition at line 473 of file misc_math.h.

#define matrix_transpose_check	(	a,
		b,
		out,
		rows,
		cols
	)

Value:

do { \
                /* Note the dimensions each get used multiple times */  \
                const float (*my_a)[rows*cols] = &(a); \
                const float (*my_b)[rows*cols] = &(b); \
                float (*my_out)[rows*cols] = &(out); \
		matrix_transpose(*my_a, *my_b, *my_out, rows, cols) \
        } while (0);

Definition at line 482 of file misc_math.h.

#define MAX	(	a,
		b
	)	({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a > _b ? _a : _b; })

Definition at line 40 of file misc_math.h.

#define MAX_FILTER_WIDTH 16

Definition at line 37 of file lpfilter.c.

#define MIN	(	a,
		b
	)	({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a < _b ? _a : _b; })

Definition at line 41 of file misc_math.h.

#define P00 P[0][0]

Definition at line 77 of file lqg.c.

#define P01 P[0][1]

Definition at line 80 of file lqg.c.

#define P02 P[0][2]

Definition at line 83 of file lqg.c.

#define P10 P[1][0]

Definition at line 78 of file lqg.c.

#define P11 P[1][1]

Definition at line 81 of file lqg.c.

#define P12 P[1][2]

Definition at line 84 of file lqg.c.

#define P20 P[2][0]

Definition at line 79 of file lqg.c.

#define P21 P[2][1]

Definition at line 82 of file lqg.c.

#define P22 P[2][2]

Definition at line 85 of file lqg.c.

#define powapprox powf

Definition at line 578 of file misc_math.h.

#define PSEUDOINV_CONVERGE_LIMIT 75

Definition at line 406 of file misc_math.h.

#define Q00 Q[0][0]

Definition at line 97 of file lqg.c.

#define Q01 Q[0][1]

Definition at line 100 of file lqg.c.

#define Q02 Q[0][2]

Definition at line 103 of file lqg.c.

#define Q10 Q[1][0]

Definition at line 98 of file lqg.c.

#define Q11 Q[1][1]

Definition at line 101 of file lqg.c.

#define Q12 Q[1][2]

Definition at line 104 of file lqg.c.

#define Q20 Q[2][0]

Definition at line 99 of file lqg.c.

#define Q21 Q[2][1]

Definition at line 102 of file lqg.c.

#define Q22 Q[2][2]

Definition at line 105 of file lqg.c.

#define sign ( x ) ((x) < 0 ? -1 : 1)

This is but one definition of sign(.)

Definition at line 44 of file misc_math.h.

#define SOLVER_KF_BIAS_EPSILON 0.000000001f

Definition at line 72 of file lqg.c.

#define SOLVER_KF_TORQUE_EPSILON 0.000000001f

Definition at line 71 of file lqg.c.

#define SOLVER_LQR_RATE_EPSILON 0.00000001f

Definition at line 73 of file lqg.c.

#define SOLVER_LQR_TORQUE_EPSILON 0.000001f

Definition at line 74 of file lqg.c.

#define SOLVER_MAX 11000

Definition at line 65 of file lqg.c.

#define SOLVER_MIN 100

Definition at line 64 of file lqg.c.

#define TOL_EPS 0.0001f

Definition at line 275 of file misc_math.h.

#define X0 X[0]

Definition at line 111 of file lqg.c.

#define X1 X[1]

Definition at line 112 of file lqg.c.

#define X2 X[2]

Definition at line 113 of file lqg.c.

Typedef Documentation

typedef struct lpfilter_state* lpfilter_state_t

Definition at line 33 of file lpfilter.h.

typedef struct lqg_state* lqg_t

Definition at line 43 of file lqg.h.

typedef struct lqr_state* lqr_t

Definition at line 41 of file lqg.h.

typedef struct rtkf_state* rtkf_t

Definition at line 40 of file lqg.h.

Function Documentation

void apply_channel_deadband	(	float *	value,
		float	deadband
	)

Apply deadband to Roll/Pitch/Yaw channels.

Definition at line 373 of file misc_math.c.

float bound_min_max	(	float	val,
		float	min,
		float	max
	)

Bound input value between min and max.

Bound input value between min and max

Definition at line 38 of file misc_math.c.

float bound_sym	(	float	val,
		float	range
	)

Bound input value within range (plus or minus)

Definition at line 50 of file misc_math.c.

float circular_modulus_deg ( float err )

Circular modulus.

Circular modulus [degrees]. Compute the equivalent angle between [-180,180] for the input angle. This is useful taking the difference between two headings and working out the relative rotation to get there quickest.

Parameters

[in] err input value in degrees.

Returns: The equivalent angle between -180 and 180

Definition at line 62 of file misc_math.c.

float circular_modulus_rad ( float err )

Circular modulus [radians]. Compute the equivalent angle between [-pi,pi] for the input angle. This is useful taking the difference between two headings and working out the relative rotation to get there quickest.

Parameters

[in] err input value in radians.

Returns: The equivalent angle between -pi and pi

Definition at line 86 of file misc_math.c.

void CrossProduct	(	const float	v1[3],
		const float	v2[3],
		float	result[3]
	)

Definition at line 316 of file coordinate_conversions.c.

float cubic_deadband	(	float	in,
		float	w,
		float	b,
		float	m,
		float	r
	)

Apply a "cubic deadband" to the input.

Parameters

[in]	in	the value to deadband
[in]	w	deadband width
[in]	b	slope of deadband at in=0
[in]	m	cubic weighting calculated by vtol_deadband_setup
[in]	r	integrated response at in=w, calculated by vtol_deadband_setup

"Real" deadbands are evil. Control systems end up fighting the edge. You don't teach your integrator about emerging drift. Discontinuities in your control inputs cause all kinds of neat stuff. As a result this calculates a cubic function within the deadband which has a low slope within the middle, but unity slope at the edge.

Definition at line 246 of file misc_math.c.

void cubic_deadband_setup	(	float	w,
		float	b,
		float *	m,
		float *	r
	)

Calculate the "cubic deadband" system parameters.

Parameters

[in]	The	width of the deadband
[in]	Slope	of deadband at in=0, sane values between 0 and 1.
[out]	m	cubic weighting of function
[out]	integrated	response at in=w

Definition at line 266 of file misc_math.c.

void Euler2R	(	float	rpy[3],
		float	Rbe[3][3]
	)

Definition at line 154 of file coordinate_conversions.c.

float expo3	(	float	x,
		int32_t	g
	)

Approximation an exponential scale curve.

Approximation an exponential scale curve

Parameters

[in]	x	input from [-1,1]
[in]	g	sets the exponential amount [0,100]

Returns: rescaled input

This function is also used for the expo plot in GCS (config/stabilization/advanced). Please adapt changes here also to /ground/gcs/src/plugins/config/expocurve.cpp

Definition at line 111 of file misc_math.c.

float expoM	(	float	x,
		int32_t	g,
		float	exponent
	)

Definition at line 116 of file misc_math.c.

static float fastexp ( float p )

inlinestatic

Definition at line 572 of file misc_math.h.

static float fastlog2 ( float x )

inlinestatic

Definition at line 541 of file misc_math.h.

static float fastpow	(	float	x,
		float	p
	)

inlinestatic

Definition at line 566 of file misc_math.h.

static float fastpow2 ( float p )

inlinestatic

Definition at line 554 of file misc_math.h.

float interpolate_value	(	const float	fraction,
		const float	beginVal,
		const float	endVal
	)

Interpolate values (groundspeeds, altitudes) over flight legs

Parameters

[in]	fraction	how far we are through the leg
[in]	beginVal	the configured value for the beginning of the leg
[in]	endVal	the configured value for the end of the leg

Returns: the interpolated value

Simple linear interpolation with clipping to ends (fraction>=0, <=1).

Definition at line 145 of file misc_math.c.

static bool IS_NOT_FINITE ( float x )

inlinestatic

Definition at line 81 of file misc_math.h.

float linear_interpolate	(	float const	input,
		float const *	curve,
		uint8_t	num_points,
		const float	input_min,
		const float	input_max
	)

Perform a linear interpolation over N points

output range is defined by the curve vector curve is defined in N intervals connecting N+1 points

Parameters

[in]	input	the input value, in [input_min,input_max]
[in]	curve	the array of points in the curve
[in]	num_points	the number of points in the curve
[in]	input_min	the lower range of the input values
[in]	input_max	the upper range of the input values

Returns: the output value, in [0,1]

Definition at line 290 of file misc_math.c.

void lpfilter_construct_biquads	(	lpfilter_state_t	filt,
		float	cutoff,
		float	dT,
		int	o,
		uint8_t	width
	)

Definition at line 102 of file lpfilter.c.

void lpfilter_construct_single_biquad	(	struct lpfilter_biquad *	b,
		float	cutoff,
		float	dT,
		float	q,
		uint8_t	width
	)

Definition at line 85 of file lpfilter.c.

void lpfilter_create	(	lpfilter_state_t *	filter_ptr,
		float	cutoff,
		float	dT,
		uint8_t	order,
		uint8_t	width
	)

Definition at line 128 of file lpfilter.c.

void lpfilter_run	(	lpfilter_state_t	filter,
		float *	sample
	)

Definition at line 220 of file lpfilter.c.

float lpfilter_run_single	(	lpfilter_state_t	filter,
		uint8_t	axis,
		float	sample
	)

Definition at line 177 of file lpfilter.c.

float lqg_controller	(	lqg_t	lqg,
		float	signal,
		float	setpoint
	)

Definition at line 516 of file lqg.c.

lqg_t lqg_create	(	rtkf_t	rtkf,
		lqr_t	lqr
	)

Definition at line 535 of file lqg.c.

lqr_t lqg_get_lqr ( lqg_t lqg )

Definition at line 576 of file lqg.c.

rtkf_t lqg_get_rtkf ( lqg_t lqg )

Definition at line 571 of file lqg.c.

void lqg_get_rtkf_state	(	lqg_t	lqg,
		float *	rate,
		float *	torque,
		float *	bias
	)

Definition at line 550 of file lqg.c.

void lqg_run_covariance	(	lqg_t	lqg,
		int	iter
	)

Definition at line 581 of file lqg.c.

void lqg_set_x0	(	lqg_t	lqg,
		float	x0
	)

Definition at line 557 of file lqg.c.

int lqg_solver_status ( lqg_t lqg )

Definition at line 564 of file lqg.c.

bool lqr_calculate_covariance_2x2	(	float	A[2][2],
		float	B[2],
		float	K[2],
		float	P[2][2],
		float	Q[2][2],
		float	R
	)

Definition at line 309 of file lqg.c.

lqr_t lqr_create	(	float	beta,
		float	tau,
		float	Ts,
		float	q1,
		float	q2,
		float	r
	)

Definition at line 460 of file lqg.c.

void lqr_get_gains	(	lqr_t	lqr,
		float	K[2]
	)

Definition at line 487 of file lqg.c.

void lqr_initialize_matrices_int	(	float	A[2][2],
		float	B[2],
		float	beta,
		float	tau,
		float	Ts
	)

Definition at line 442 of file lqg.c.

int lqr_solver_status ( lqr_t lqr )

Definition at line 424 of file lqg.c.

void lqr_stabilize_covariance	(	lqr_t	lqr,
		int	iterations
	)

Definition at line 405 of file lqg.c.

void lqr_update	(	lqr_t	lqr,
		float	q1,
		float	q2,
		float	r
	)

Definition at line 477 of file lqg.c.

static void matrix_add	(	const float *	a,
		const float *	b,
		float *	out,
		int	rows,
		int	cols
	)

inlinestatic

Adds two matrices Can safely be done in-place. (e.g. is slow and not vectorized/unrolled)

Parameters

[in]	a	Matrix of dimension rows x cols
[in]	b	Matrix of dimension rows x cols
[in]	scalar	the amount to multiply by
[out]	out	Matrix of dimensions rows x cols
[in]	rows	Matrix dimension
[in]	cols	Matrix dimension

Definition at line 216 of file misc_math.h.

static float matrix_getmaxabs	(	const float *	a,
		int	arows,
		int	acols
	)

inlinestatic

Finds the largest value in a matrix.

Parameters

[in]	a	Matrix of dimension arows x acols
[in]	arows	Matrix dimension
[in]	acols	Matrix dimension

Definition at line 388 of file misc_math.h.

static void matrix_mul	(	const float *	a,
		const float *	b,
		float *	out,
		int	arows,
		int	acolsbrows,
		int	bcols
	)

inlinestatic

Multiplies out = a b.

Matrices are stored in row order, that is a[i*cols + j] a, b, and output must be distinct.

Parameters

[in]	a	multiplicand dimensioned arows by acolsbrows
[in]	b	multiplicand dimensioned acolsbrows by bcols
[out]	output	product dimensioned arows by bcols
[in]	arows	matrix dimension
[in]	acolsbrows	matrix dimension
[in]	bcols	matrix dimension

Definition at line 121 of file misc_math.h.

static void matrix_mul_scalar	(	const float *	a,
		float	scalar,
		float *	out,
		int	rows,
		int	cols
	)

inlinestatic

Multiplies a matrix by a scalar Can safely be done in-place. (e.g. is slow and not vectorized/unrolled)

Parameters

[in]	a	Matrix of dimension rows x cols
[in]	scalar	the amount to multiply by
[out]	out	Product matrix of dimensions rows x cols
[in]	rows	Matrix dimension
[in]	cols	Matrix dimension

Definition at line 193 of file misc_math.h.

static bool matrix_pseudoinv	(	const float *	a,
		float *	out,
		int	arows,
		int	acols
	)

inlinestatic

Finds a pseudo-inverse of a matrix.

This is an inverse when an inverse exists, and is equivalent to least squares when the system is overdetermined or underdetermined. AKA happiness.

It works by iterative approximation. Note it may run away or orbit the solution in a few degenerate cases and return failure.

Parameters

[in]	a	Matrix of dimension arows x acols
[out]	out	Pseudoinverse matrix of dimensions arows x acols
[in]	arows	Matrix dimension
[in]	acols	Matrix dimension

Returns: true if a good pseudoinverse was found, false otherwise.

Definition at line 423 of file misc_math.h.

static bool matrix_pseudoinv_convergecheck	(	const float *	a,
		const float *	b,
		const float *	prod,
		int	rows,
		int	cols
	)

inlinestatic

Definition at line 280 of file misc_math.h.

static bool matrix_pseudoinv_step	(	const float *	a,
		float *	ainv,
		int	arows,
		int	acols
	)

inlinestatic

Definition at line 354 of file misc_math.h.

static void matrix_sub	(	const float *	a,
		const float *	b,
		float *	out,
		int	rows,
		int	cols
	)

inlinestatic

Subtracts two matrices Can safely be done in-place. (e.g. is slow and not vectorized/unrolled)

Parameters

[in]	a	Matrix of dimension rows x cols
[in]	b	Matrix of dimension rows x cols
[in]	scalar	the amount to multiply by
[out]	out	Matrix of dimensions rows x cols
[in]	rows	Matrix dimension
[in]	cols	Matrix dimension

Definition at line 241 of file misc_math.h.

static void matrix_transpose	(	const float *	a,
		float *	out,
		int	arows,
		int	acols
	)

inlinestatic

Transposes a matrix.

Not safe in place.

Parameters

[in]	a	Matrix of dimension arows x acols
[out]	out	Matrix of dimension acols x arows
[in]	arows	Matrix dimension
[in]	acols	Matrix dimension

Definition at line 265 of file misc_math.h.

float pid_apply	(	struct pid *	pid,
		const float	err
	)

Methods to use the pid structures.

Update the PID computation

Parameters

[in]	pid	The PID struture which stores temporary information
[in]	err	The error term

Returns: Output the computed controller value

Definition at line 48 of file pid.c.

float pid_apply_antiwindup	(	struct pid *	pid,
		const float	err,
		float	min_bound,
		float	max_bound,
		float	aw_bound
	)

Update the PID computation and apply anti windup limit

Parameters

[in]	pid	The PID struture which stores temporary information
[in]	err	The error term
[in]	min_bound	The minimum output
[in]	max_bound	The maximum output
[in]	aw_bound	The absolute boundary for slope-related anti-windup.
[in]	dT	The time step

Returns: Output the computed controller value

based on "Feedback Systems" by Astrom and Murray, PID control chapter.

Definition at line 86 of file pid.c.

float pid_apply_setpoint	(	struct pid *	pid,
		struct pid_deadband *	deadband,
		const float	setpoint,
		const float	measured
	)

Update the PID computation with setpoint weighting on the derivative

Parameters

[in]	pid	The PID struture which stores temporary information
[in]	setpoint	The setpoint to use
[in]	measured	The measured value of output

Returns: Output the computed controller value

This version of apply uses setpoint weighting for the derivative component so the gain on the gyro derivative can be different than the gain on the setpoint derivative

Definition at line 141 of file pid.c.

float pid_apply_setpoint_antiwindup	(	struct pid *	pid,
		struct pid_deadband *	deadband,
		const float	setpoint,
		const float	measured,
		float	min_bound,
		float	max_bound,
		float	aw_bound
	)

Update the PID computation with setpoint weighting on the derivative and apply anti windup limit

Parameters

[in]	pid	The PID struture which stores temporary information
[in]	err	The error term
[in]	min_bound	The minimum output
[in]	max_bound	The maximum output
[in]	aw_bound	The absolute boundary for slope-related anti-windup.

Returns: Output the computed controller value

Definition at line 187 of file pid.c.

void pid_configure	(	struct pid *	pid,
		float	p,
		float	i,
		float	d,
		float	iLim,
		float	dT
	)

Configure the settings for a pid structure

Parameters

[out]	pid	The PID structure to configure
[in]	p	The proportional term
[in]	i	The integral term
[in]	d	The derivative term

Definition at line 280 of file pid.c.

void pid_configure_deadband	(	struct pid_deadband *	deadband,
		float	width,
		float	slope
	)

Set the deadband values

Parameters

[out]	pid	The PID structure to configure
[in]	deadband	Deadband width in degrees per second
[in]	slope	Deadband slope (0..1)

Definition at line 298 of file pid.c.

void pid_configure_derivative	(	float	cutoff,
		float	g
	)

Configure the common terms that alter ther derivative.

Parameters

[in]	cutoff	The cutoff frequency (in Hz)
[in]	gamma	The gamma term for setpoint shaping (unsused now)

Definition at line 267 of file pid.c.

void pid_zero ( struct pid * pid )

Reset a bit

Parameters

[in] pid The pid to reset

Definition at line 252 of file pid.c.

void quat_copy	(	const float	q[4],
		float	qnew[4]
	)

Duplicate a quaternion.

Parameters

[in]	q	quaternion in
[out]	qnew	quaternion to copy to

Definition at line 345 of file coordinate_conversions.c.

void quat_inverse ( float q[4] )

Compute the inverse of a quaternion.

Parameters

[in] out] q The matrix to invert

Definition at line 333 of file coordinate_conversions.c.

void quat_mult	(	const float	q1[4],
		const float	q2[4],
		float	qout[4]
	)

Multiply two quaternions into a third.

Parameters

[in]	q1	First quaternion
[in]	q2	Second quaternion
[out]	qout	Output quaternion

Definition at line 359 of file coordinate_conversions.c.

void Quaternion2R	(	float	q[4],
		float	Rbe[3][3]
	)

Definition at line 137 of file coordinate_conversions.c.

void Quaternion2RPY	(	const float	q[4],
		float	rpy[3]
	)

Definition at line 62 of file coordinate_conversions.c.

void R2Quaternion	(	float	R[3][3],
		float	q[4]
	)

Definition at line 173 of file coordinate_conversions.c.

void randomize_addseed ( uint32_t seed )

Definition at line 315 of file misc_math.c.

uint32_t randomize_int ( uint32_t interval )

Todo:: This could improve a lot from constant static value evaluation in the optimizer, so it may belong in the header as inline...

Definition at line 340 of file misc_math.c.

static uint32_t randomize_int32 ( )

static

Todo:: Not re-entrant. Need to think how best to handle this once there's more users of this API.

Definition at line 321 of file misc_math.c.

void RneFromLLA	(	float	LLA[3],
		float	Rne[3][3]
	)

Definition at line 41 of file coordinate_conversions.c.

void rot_mult	(	float	R[3][3],
		const float	vec[3],
		float	vec_out[3],
		bool	transpose
	)

Rotate a vector by a rotation matrix.

Parameters

[in]	R	a three by three rotation matrix (first index is row)
[in]	vec	the source vector
[in]	transpose	If false use R, else if true use R'
[out]	vec_out	the output vector

Definition at line 374 of file coordinate_conversions.c.

uint8_t RotFrom2Vectors	(	const float	v1b[3],
		const float	v1e[3],
		const float	v2b[3],
		const float	v2e[3],
		float	Rbe[3][3]
	)

Definition at line 231 of file coordinate_conversions.c.

void RPY2Quaternion	(	const float	rpy[3],
		float	q[4]
	)

Definition at line 108 of file coordinate_conversions.c.

bool rtkf_calculate_covariance_3x3	(	float	A[3][3],
		float	K[3],
		float	P[3][3],
		float	Q[3][3],
		float	R
	)

Definition at line 126 of file lqg.c.

rtkf_t rtkf_create	(	float	beta,
		float	tau,
		float	Ts,
		float	R,
		float	q1,
		float	q2,
		float	q3,
		float	biaslim
	)

Definition at line 274 of file lqg.c.

void rtkf_initialize_matrices_int	(	float	A[3][3],
		float	B[3],
		float	beta,
		float	tau,
		float	Ts
	)

Definition at line 255 of file lqg.c.

void rtkf_predict_axis	(	rtkf_t	rtkf,
		float	signal,
		float	input,
		float	Xout[3]
	)

Definition at line 233 of file lqg.c.

void rtkf_prediction_step	(	float	A[3][3],
		float	B[3],
		float	K[3],
		float	X[3],
		float	signal,
		float	input
	)

Definition at line 219 of file lqg.c.

int rtkf_solver_status ( rtkf_t rtkf )

Definition at line 290 of file lqg.c.

void rtkf_stabilize_covariance	(	rtkf_t	rtkf,
		int	iterations
	)

Definition at line 195 of file lqg.c.

void Rv2Rot	(	float	Rv[3],
		float	R[3][3]
	)

Definition at line 287 of file coordinate_conversions.c.

static int16_t sin_approx ( int32_t x )

inlinestatic

Fast approximation of sine; 3rd order taylor expansion. Based on http://www.coranac.com/2009/07/sines/.

Parameters

[in] x angle, 2^15 units/circle

Returns: approximately sin(x / 2^15 * 2 * PI) in Q12 form

Definition at line 90 of file misc_math.h.

void vector2_clip	(	float *	vels,
		float	limit
	)

Clip a velocity 2-vector while maintaining vector direction.

Parameters

[in,out]	vels	velocity to clip
[in]	limit	desired limit magnitude.

if mag(vels) > limit, vels=vels / mag(vels) * limit

Definition at line 208 of file misc_math.c.

void vector2_rotate	(	const float *	original,
		float *	out,
		float	angle
	)

Rotate a 2-vector by a specified angle.

Parameters

[in]	original	the input vector
[out]	out	the rotated output vector
[in]	angle	in degrees

Definition at line 225 of file misc_math.c.

float vector3_distances	(	const float *	actual,
		const float *	desired,
		float *	out,
		bool	normalize
	)

Subtract two 3-vectors, and optionally normalize to return an error value.

Parameters

[in]	actual	the measured process value
[in]	desired	the setpoint of the system
[out]	the	resultant error vector desired-actual
[in]	normalize	True if it's desired to normalize the output vector

Returns: the norm, for fun.

Definition at line 178 of file misc_math.c.

float VectorMagnitude ( const float v[3] )

Definition at line 324 of file coordinate_conversions.c.

float vectorn_magnitude	(	const float *	v,
		int	n
	)

Calculate pythagorean magnitude of a vector.

Parameters

[in]	v	pointer to a float array
[in]	n	length of the amount to take the magnitude of

Returns: Root of sum of squares of the vector

Note that sometimes we only take the magnitude of the first 2 elements of a 3-vector to get the horizontal component.

Definition at line 159 of file misc_math.c.

Variable Documentation

float lpfilter_biquad::a1

Definition at line 67 of file lpfilter.c.

float lpfilter_biquad::a2

Definition at line 67 of file lpfilter.c.

float lpfilter_first_order::alpha

Definition at line 72 of file lpfilter.c.

float lpfilter_biquad::b0

Definition at line 67 of file lpfilter.c.

struct lpfilter_biquad* lpfilter_state::biquad[4]

Definition at line 79 of file lpfilter.c.

float deriv_gamma = 1.0

static

Store the setpoint weight to apply for the derivative term.

Definition at line 40 of file pid.c.

float deriv_tau = 7.9577e-3f

static

Store the shared time constant for the derivative cutoff.

Definition at line 37 of file pid.c.

struct lpfilter_first_order* lpfilter_state::first_order

Definition at line 78 of file lpfilter.c.

const float lpfilter_butterworth_factors[16]

static

Initial value:

= {
        
        1.4142f,
        
        1.0f,
        
        0.7654f, 1.8478f,
        
        0.6180f, 1.6180f,
        
        0.5176f, 1.4142f, 1.9319f,
        
        0.4450f, 1.2470f, 1.8019f,
        
        0.3902f, 1.1111f, 1.6629f, 1.9616f
}

Definition at line 39 of file lpfilter.c.

uint8_t lpfilter_state::order

Definition at line 80 of file lpfilter.c.

float* lpfilter_first_order::prev

Definition at line 73 of file lpfilter.c.

uint32_t random_state[4] = { 0xdeadbeef, 0xfeedfeed, 0xcafebabe, 1234 }

static

Definition at line 313 of file misc_math.c.

struct lpfilter_biquad_state* lpfilter_biquad::s

Definition at line 68 of file lpfilter.c.

uint8_t lpfilter_state::width

Definition at line 81 of file lpfilter.c.

float lpfilter_biquad_state::x1

Definition at line 63 of file lpfilter.c.

float lpfilter_biquad_state::x2

Definition at line 63 of file lpfilter.c.

float lpfilter_biquad_state::y1

Definition at line 63 of file lpfilter.c.

float lpfilter_biquad_state::y2

Definition at line 63 of file lpfilter.c.

Files

Data Structures

Macros

Typedefs

Functions

Variables

Detailed Description

Macro Definition Documentation

Typedef Documentation

Function Documentation

Variable Documentation