File content as of revision 2:5aa75c3d8cc3:

/* This file is part of the Razor AHRS Firmware */
#include "Razor_AHRS.h"
#include <math.h>

void IMU::read_sensors() {
    Read_Gyro(); // Read gyroscope
    Read_Accel(); // Read accelerometer
    Read_Magn(); // Read magnetometer
}

// Read every sensor and record a time stamp
// Init DCM with unfiltered orientation
// TODO re-init global vars?
void IMU::reset_sensor_fusion() {
    float temp0[3] = { accel[0], accel[1], accel[2] };
    float temp1[3];
    float temp2[3];
    float xAxis[] = {1.0f, 0.0f, 0.0f};

    read_sensors();
    timestamp = timer.read_ms();

    // GET PITCH
    // Using y-z-plane-component/x-component of gravity vector
    pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2])));

    // GET ROLL
    // Compensate pitch of gravity vector

    Vector_Cross_Product(temp1, temp0, xAxis);
    Vector_Cross_Product(temp2, xAxis, temp1);
    // Normally using x-z-plane-component/y-component of compensated gravity vector
    // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
    // Since we compensated for pitch, x-z-plane-component equals z-component:
    roll = atan2(temp2[1], temp2[2]);

    // GET YAW
    Compass_Heading();
    yaw = MAG_Heading;

    // Init rotation matrix
    init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
}

// Apply calibration to raw sensor readings
void IMU::compensate_sensor_errors() {
    // Compensate accelerometer error
    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;

    // Compensate magnetometer error
    magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
    magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
    magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;

    // Compensate gyroscope error
    gyro[0] -= GYRO_AVERAGE_OFFSET_X;
    gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
    gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
}

// Reset calibration session if reset_calibration_session_flag is set
void IMU::check_reset_calibration_session() {
    // Raw sensor values have to be read already, but no error compensation applied

    // Reset this calibration session?
    if (!reset_calibration_session_flag) return;

    // Reset acc and mag calibration variables
    for (int i = 0; i < 3; i++) {
        accel_min[i] = accel_max[i] = accel[i];
        magnetom_min[i] = magnetom_max[i] = magnetom[i];
    }

    // Reset gyro calibration variables
    gyro_num_samples = 0;  // Reset gyro calibration averaging
    gyro_average[0] = gyro_average[1] = gyro_average[2] = 0;

    reset_calibration_session_flag = false;
}

void IMU::turn_output_stream_on() {
    output_stream_on = true;
    statusLed = 1;
}

void IMU::turn_output_stream_off() {
    output_stream_on = false;
    statusLed = 0;
}

// Blocks until another byte is available on serial port
char IMU::readChar() {
    while (pc.rxBufferGetCount() < 1) { } // Block
    return pc.getc();
}

void IMU::readInput() {
    // Read incoming control messages
    if (pc.rxBufferGetCount() >= 2) {
        if (pc.getc() == '#') { // Start of new control message
            int command = pc.getc(); // Commands
            if (command == 'f') // request one output _f_rame
                output_single_on = true;
            else if (command == 's') { // _s_ynch request
                // Read ID
                char id[2];
                id[0] = readChar();
                id[1] = readChar();

                // Reply with synch message
                pc.printf("#SYNCH");
                pc.putc(id[0]);
                pc.putc(id[1]);
                pc.printf(NEW_LINE);
            } else if (command == 'o') { // Set _o_utput mode
                char output_param = readChar();
                if (output_param == 'n') { // Calibrate _n_ext sensor
                    curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
                    reset_calibration_session_flag = true;
                } else if (output_param == 't') // Output angles as _t_ext
                    output_mode = OUTPUT__MODE_ANGLES_TEXT;
                else if (output_param == 'b') // Output angles in _b_inary form
                    output_mode = OUTPUT__MODE_ANGLES_BINARY;
                else if (output_param == 'c') { // Go to _c_alibration mode
                    output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
                    reset_calibration_session_flag = true;
                } else if (output_param == 's') // Output _s_ensor values as text
                    output_mode = OUTPUT__MODE_SENSORS_TEXT;
                else if (output_param == '0') { // Disable continuous streaming output
                    turn_output_stream_off();
                    reset_calibration_session_flag = true;
                } else if (output_param == '1') { // Enable continuous streaming output
                    reset_calibration_session_flag = true;
                    turn_output_stream_on();
                } else if (output_param == 'e') { // _e_rror output settings
                    char error_param = readChar();
                    if (error_param == '0') output_errors = false;
                    else if (error_param == '1') output_errors = true;
                    else if (error_param == 'c') { // get error count
                        pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors);
                    }
                }
            }
#if OUTPUT__HAS_RN_BLUETOOTH == true
            // Read messages from bluetooth module
            // For this to work, the connect/disconnect message prefix of the module has to be set to "#".
            else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
                turn_output_stream_on();
            else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
                turn_output_stream_off();
#endif // OUTPUT__HAS_RN_BLUETOOTH == true
        } else { } // Skip character
    }
}

IMU::IMU()
        : gyro_num_samples(0)
        , yaw(0)
        , pitch(0)
        , roll(0)
        , MAG_Heading(0)
        , timestamp(0)
        , timestamp_old(0)
        , G_Dt(0)
        , output_mode(-1) // Select your startup output mode here!// Select your startup output mode here!
        , output_stream_on(false)
        , output_single_on(true)
        , curr_calibration_sensor(0)
        , reset_calibration_session_flag(true)
        , num_accel_errors(0)
        , num_magn_errors(0)
        , num_gyro_errors(0)
        , output_errors(true)
        , statusLed(LED1)
        , pc(USBTX, USBRX)
        , Wire(p28, p27) {

    accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0;
    accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0;
    accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0;

    Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0;
    Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0;
    Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0;

    DCM_Matrix[0][0] = 1;
    DCM_Matrix[0][1] = 0;
    DCM_Matrix[0][2] = 0;
    DCM_Matrix[1][0] = 0;
    DCM_Matrix[1][1] = 1;
    DCM_Matrix[1][2] = 0;
    DCM_Matrix[2][0] = 0;
    DCM_Matrix[2][1] = 0;
    DCM_Matrix[2][2] = 1;

    Update_Matrix[0][0] = 0;
    Update_Matrix[0][1] = 1;
    Update_Matrix[0][2] = 2;
    Update_Matrix[1][0] = 3;
    Update_Matrix[1][1] = 4;
    Update_Matrix[1][2] = 5;
    Update_Matrix[2][0] = 6;
    Update_Matrix[2][1] = 7;
    Update_Matrix[2][2] = 8;

    Temporary_Matrix[0][0] = 0;
    Temporary_Matrix[0][1] = 0;
    Temporary_Matrix[0][2] = 0;
    Temporary_Matrix[1][0] = 0;
    Temporary_Matrix[1][1] = 0;
    Temporary_Matrix[1][2] = 0;
    Temporary_Matrix[2][0] = 0;
    Temporary_Matrix[2][1] = 0;
    Temporary_Matrix[2][2] = 0;

    timer.start();
    // Init serial output
    pc.baud(OUTPUT__BAUD_RATE);

    // Init status LED
    statusLed = 0;

    // Init sensors
    wait_ms(50);  // Give sensors enough time to start
    I2C_Init();
    Accel_Init();
    Magn_Init();
    Gyro_Init();

    // Read sensors, init DCM algorithm
    wait_ms(20);  // Give sensors enough time to collect data
    reset_sensor_fusion();

    // Init output
#if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false)
    turn_output_stream_off();
#else
    turn_output_stream_on();
#endif
}

// Main loop
void IMU::loop() {
    timestamp_old = timestamp;
    timestamp = timer.read_ms();
    if (timestamp > timestamp_old)
        G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
    else
        G_Dt = 0;

    // Update sensor readings
    read_sensors();

    if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode
        check_reset_calibration_session();  // Check if this session needs a reset
        if (output_stream_on || output_single_on)
            output_calibration(curr_calibration_sensor);
    } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) {
        // Apply sensor calibration
        compensate_sensor_errors();

        if (output_stream_on || output_single_on)
            output_sensors();
    } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) {
        // Apply sensor calibration
        compensate_sensor_errors();

        // Run DCM algorithm
        Compass_Heading(); // Calculate magnetic heading
        Matrix_update();
        Normalize();
        Drift_correction();
        Euler_angles();

        if (output_stream_on || output_single_on)
            output_angles();
    }

    output_single_on = false;

#if DEBUG__PRINT_LOOP_TIME == true
    pc.printf("loop time (ms) = %f" NEW_LINE, timer.read_ms() - timestamp);
#endif
}

int main() {
    IMU imu;
    
    Ticker looper;
    looper.attach(&imu, &IMU::loop, (float)OUTPUT__DATA_INTERVAL / 1000.0f); // 50Hz update
    
    while (1)
    {
        imu.readInput();
        wait_ms(5);
    }
    
    return 0;
}