main.c

卡尔曼滤波在AVR单片机的应用

/*
    Copyright (c) 2007 Michael P. Thompson <[e-mail]>

    Permission is hereby granted, free of charge, to any person 
    obtaining a copy of this software and associated documentation 
    files (the "Software"), to deal in the Software without 
    restriction, including without limitation the rights to use, copy, 
    modify, merge, publish, distribute, sublicense, and/or sell copies 
    of the Software, and to permit persons to whom the Software is 
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be 
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
    DEALINGS IN THE SOFTWARE.

    $Id$
*/

#include <math.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include "adc.h"
#include "kalman.h"
#include "timer.h"
#include "usart.h"

// Gyro Information
// Each ADC unit equals: 3.3 / 1024 = 0.00322265625 V = 3.22265625 mV
// Gyro measures rate at 2 mV/degree/second
// Each ADC unit equals: 3.22265625 / 2.0 = 1.611328125 degrees/sec
// ADC samples taken at 50.0801282 Hz = 19.968 ms = 0.019968 S = dt
// Multiply ADC value by time: v * 1.611328125 * dt = v * 1.611328125 * 0.019968 = v * 0.032175

// Acceleromter Information
// Each ADC unit equals: 3.3 / 1024 = 0.00322265625 V = 3.22265625 mV
// Accelerometer measures acceleration at 300 mV/g
// Each ADC unit equals: 3.22265625 / 300.0 = 0.01074219 g

kalman x_kalman_state;
kalman y_kalman_state;

int main(void)
{
    // Initialize the timer.
    timer_init();

    // Initialize the USART.
    usart_init(BAUD2UBRR_57600);

    // Initialize the ADC.
    adc_init();

    // Initialize the Kalman states for the x and y axis.
    kalman_init(&x_kalman_state);
    kalman_init(&y_kalman_state);

    // Enable interrupts.
    sei();

#if 0
    // XXX Used for initial performance testing of the kalman module.

    // Loop forever.
    for (;;)
    {
        uint8_t i;
        uint8_t j;
        float output;

        usart_xmit_printf("Starting...\n\r");

        // Loop 100 times.
        for (i = 0; i < 2; ++i)
        {
            // Loop 10 times.
            for (j = 0; j < 10; ++j)
            {
                // Update the Kalman state 10 times.
                output = kalman_update(&x_kalman_state, 0.016088, 1.091216);
                output = kalman_update(&x_kalman_state, 0.016088, 1.652305);
                output = kalman_update(&x_kalman_state, 0.016088, 1.668337);
                output = kalman_update(&x_kalman_state, 0.016088, 1.112400);
                output = kalman_update(&x_kalman_state, 0.016088, 1.091216);
                output = kalman_update(&x_kalman_state, 0.016088, 1.101706);
                output = kalman_update(&x_kalman_state, 0.016088, 1.101706);
                output = kalman_update(&x_kalman_state, 0.048263, 1.652305);
                output = kalman_update(&x_kalman_state, 0.016088, 1.101706);
                output = kalman_update(&x_kalman_state, 0.016088, 1.101706);
            }
        }

        usart_xmit_printf("Done!\n\r\n\r\n\r");
    }
#endif

    // Loop forever.
    for (;;)
    {
        // Get the gyro values.
        if (adc_is_ready())
        {
            float gyro_x;
            float gyro_y;
            float accel_x;
            float accel_y;
            float accel_z;
            float pitch_x;
            float pitch_y;
            float output_x;
            float output_y;

            // Get the x and y gyro values.
            gyro_x = (float) adc_get_gyro_x();
            gyro_y = (float) adc_get_gyro_y();

            // Get the x, y and z accelerometer values.
            accel_x = (float) adc_get_accel_x();
            accel_y = (float) adc_get_accel_y();
            accel_z = (float) adc_get_accel_z();

            // Reset the ready flag.
            adc_reset_ready();

            // Zero adjust the gyro values.  A better way of dynamically determining 
            // these values must be found rather than using hard coded constants.
            gyro_x -= 470.5;
            gyro_y -= 440.5;

            // Convert the gyro values from raw ADC value to degrees per sample.
            // Each ADC unit equals: 3.3 / 1024 = 0.00322265625 V = 3.22265625 mV
            // Gyro measures rate at 2 mV/degree/second
            // Each ADC unit equals: 3.22265625 / 2.0 = 1.611328125 degrees/sec
            // ADC samples taken at 10.02 Hz so dt = 99.8 ms = 0.0998 sec
            // Multiply ADC value by time: v * 1.611328125 * 0.0998 = v * 0.16081
            gyro_x *= 0.16081;
            gyro_y *= 0.16081;

            // Zero adjust the accelerometer values.  A better way of dynamically determining 
            // these values must be found rather than using hard coded constants.
            accel_x -= 506.0;
            accel_y -= 506.0;
            accel_z -= 506.0;

            // Determine the pitch along x and y axis in radians.  We don't need to worry
            // about converting the accelerometer values to degrees because the ratio between
            // the accelerometer values is important, not the absolute values.
            pitch_x = atan2(accel_x / 1.0, accel_z / 1.0);
            pitch_y = atan2(accel_y / 1.0, accel_z / 1.0);

            // Convert the the pitch from radians to degrees.
            pitch_x *= 57.2957795;
            pitch_y *= 57.2957795;

            // Filter the rate and pitch information through the Kalman filters.
            output_x = kalman_update(&x_kalman_state, gyro_x, pitch_x);
            output_y = kalman_update(&y_kalman_state, gyro_y, pitch_y);

            // Skip outputting the current values if the previous values are not yet finished.
            if (usart_xmit_buffer_ready())
            {
                // usart_xmit_printf("{%f, %f},\n\r", gyro_x, pitch_x);
                // usart_xmit_printf("{%f, %f},\n\r", gyro_y, pitch_y);
                usart_xmit_printf("x= %-+6.1f  y= %-+6.1f   \r\n", output_x, output_y);
            }
        }
    }

    return 0;
}