Orientation Calculation

Calculate Orientation using the Kalman-Filter, Accelerometer and Gyroscope. More...

Files
file	orientation.h
	Orientation API Header.
file	orientation.c
	Orientation API Implementation.

Data Structures
struct	Angles
	Can store orientation in Euler Space. More...

Macros
#define	TODEG(x)   ((x * 180) / M_PI)
	Convert Radians to Degrees. More...

Functions
Error	orientationInit (void)
	Initializes the Orientation API. More...
Error	orientationTask (void)
	Calculate the current orientation. More...
void	zeroOrientation (void)
	Sets the current orientation to zero. More...

Variables
Angles	orientation
	Current Aircraft orientation. More...
Angles	orientation = {.pitch = 0, .roll = 0, .yaw = 0}
	Current Aircraft orientation. More...
Angles	orientationError = {.pitch = 0, .roll = 0, .yaw = 0}
	Current Aircraft orientation offset. More...
Kalman	pitchData
	Kalman-State for Pitch Angle. More...
Kalman	rollData
	Kalman-State for Roll Angle. More...

Detailed Description

Calculate Orientation using the Kalman-Filter, Accelerometer and Gyroscope.

Macro Definition Documentation

#define TODEG

(

x

)

((x * 180) / M_PI)

Convert Radians to Degrees.

Definition at line 55 of file orientation.c.

Referenced by orientationTask().

Function Documentation

Error orientationInit

(

void

)

Initializes the Orientation API.

Also initializes the Accelerometer, Gyroscope and Magnetometer. I2C should already be initialized!

Returns

TWI_NO_ANSWER, TWI_WRITE_ERROR, ARGUMENT_ERROR or SUCCESS.

Examples:

hardwareTest.c, and uartFlight.c.

Definition at line 73 of file orientation.c.

References accInit(), CHECKERROR, complementaryInit(), gyroInit(), kalmanInit(), r250DPS, r4G, and SUCCESS.

                            {
    Error e = accInit(r4G);
    CHECKERROR(e);
    e = gyroInit(r250DPS);
    CHECKERROR(e);

#if ORIENTATION_FILTER == FILTER_KALMAN
    kalmanInit(&pitchData);
    kalmanInit(&rollData);
#elif ORIENTATION_FILTER == FILTER_COMPLEMENTARY
    complementaryInit(&pitchData);
    complementaryInit(&rollData);
#endif

    return SUCCESS;
}

Error orientationTask

(

void

)

Calculate the current orientation.

It will be stored in the global orientation Struct.

Returns

TWI_NO_ANSWER, TWI_WRITE_ERROR, ARGUMENT_ERROR or SUCCESS.

Examples:

uartFlight.c.

Definition at line 90 of file orientation.c.

References accRead(), CHECKERROR, complementaryExecute(), ERROR, getSystemTime(), gyroRead(), kalmanInnovate(), orientation, Angles::pitch, Angles::roll, SUCCESS, TODEG, Angles::vPitch, Angles::vRoll, Angles::vYaw, Vector3f::x, xySelfReset(), Vector3f::y, Angles::yaw, and Vector3f::z.

                            {
    Vector3f g, a;
    Error e = accRead(&a); // Read Accelerometer
    CHECKERROR(e);
    e = gyroRead(&g); // Read Gyroscope
    CHECKERROR(e);

    // Calculate Pitch & Roll from Accelerometer Data
    double roll = atan(a.x / hypot(a.y, a.z));
    double pitch = atan(a.y / hypot(a.x, a.z));
    roll = TODEG(roll);
    pitch = TODEG(pitch); // As Degree, not radians!

    // Filter Roll and Pitch with Gyroscope Data from the corresponding axis
#if ORIENTATION_FILTER == FILTER_KALMAN
    kalmanInnovate(&pitchData, pitch, g.x);
    kalmanInnovate(&rollData, roll, g.y);
    orientation.roll = rollData.x1;
    orientation.pitch = pitchData.x1;
#elif ORIENTATION_FILTER == FILTER_COMPLEMENTARY
    complementaryExecute(&pitchData, pitch, g.x);
    complementaryExecute(&rollData, roll, g.y);
    orientation.roll = rollData.angle;
    orientation.pitch = pitchData.angle;
#endif

    // Zero Offset for angles
    orientation.roll -= orientationError.roll;
    orientation.pitch -= orientationError.pitch;
    orientation.yaw -= orientationError.yaw;

    // Store Angle Speeds
    orientation.vRoll = g.y;
    orientation.vPitch = g.x;
    orientation.vYaw = g.z;

    // Self-Reset if data is garbage and we just came up
    if (getSystemTime() < 2000) {
        if (isnan(orientation.roll) || isnan(orientation.pitch) || isnan(orientation.yaw)) {
            xySelfReset();
            return ERROR;
        }
    }

    return SUCCESS;
}

void zeroOrientation

(

void

)

Sets the current orientation to zero.

Examples:

uartFlight.c.

Definition at line 137 of file orientation.c.

References orientation, Angles::pitch, Angles::roll, and Angles::yaw.

                           {
    orientationError.roll = orientation.roll + orientationError.roll;
    orientationError.pitch = orientation.pitch + orientationError.pitch;
    orientationError.yaw = orientation.yaw + orientationError.yaw;
}

Variable Documentation

Angles orientation

Current Aircraft orientation.

Examples:

uartFlight.c.

Definition at line 58 of file orientation.c.

Referenced by orientationTask(), pidTask(), and zeroOrientation().

Angles orientation = {.pitch = 0, .roll = 0, .yaw = 0}

Current Aircraft orientation.

Definition at line 58 of file orientation.c.

Referenced by orientationTask(), pidTask(), and zeroOrientation().

Angles orientationError = {.pitch = 0, .roll = 0, .yaw = 0}

Current Aircraft orientation offset.

Definition at line 61 of file orientation.c.

Kalman pitchData

Kalman-State for Pitch Angle.

Definition at line 64 of file orientation.c.

Kalman rollData

Kalman-State for Roll Angle.

Definition at line 65 of file orientation.c.