UAVX Multicopter Flight Controller.

Dependencies:   mbed

Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
diff -r 000000000000 -r 62a1c91a859a gyro.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gyro.c	Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,364 @@
+// ===============================================================================================
+// =                              UAVXArm Quadrocopter Controller                                =
+// =                           Copyright (c) 2008 by Prof. Greg Egan                             =
+// =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
+// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// ===============================================================================================
+
+//    This is part of UAVXArm.
+
+//    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
+//    General Public License as published by the Free Software Foundation, either version 3 of the
+//    License, or (at your option) any later version.
+
+//    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
+//    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+//    See the GNU General Public License for more details.
+
+//    You should have received a copy of the GNU General Public License along with this program.
+//    If not, see http://www.gnu.org/licenses/
+
+#include "UAVXArm.h"
+const real32 GyroToRadian[UnknownGyro] = {
+    0.023841,       // MLX90609
+    0.044680,       // ADXRS150
+    0.007149,       // IDG300
+    0.011796,       // LY530
+    0.017872,       // ADXRS300
+    0.000607,       // ITG3200
+    1.0             // Infrared Sensors
+    // add others as required
+};
+
+void ReadGyros(void);
+void GetGyroRates(void);
+void CheckGyroFault(uint8, uint8, uint8);
+void ErectGyros(void);
+void InitGyros(void);
+void GyroTest(void);
+void ShowGyroType(void);
+
+real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
+uint8 GyroType;
+
+void GetGyroRates(void) {
+    static uint8 g;
+
+    ReadGyros();
+    for ( g = 0; g < (uint8)3; g++ )
+        Gyro[g] = ( GyroADC[g] - GyroNeutral[g] ) ;
+} // GetGyroRates
+
+void ReadGyros(void) {
+    switch ( GyroType ) {
+        case ITG3200Gyro:
+            ReadITG3200Gyro();
+            break;
+        case IRSensors:
+            GetIRAttitude();
+            break;
+        default :
+            ReadAnalogGyros();
+            break;
+    } // switch
+} // ReadGyros
+
+void ErectGyros(void) {
+    static int16 i, g;
+
+    LEDRed_ON;
+
+    for ( g = 0; g <(int8)3; g++ )
+        GyroNeutral[g] = 0.0;
+
+    for ( i = 0; i < 32 ; i++ ) {
+        Delay1mS(10);
+
+        ReadGyros();
+        for ( g = 0; g <(int8)3; g++ )
+            GyroNeutral[g] += GyroADC[g];
+    }
+
+    for ( g = 0; g <(int8)3; g++ ) {
+        GyroNeutral[g] *= 0.03125;
+        Gyro[g] = 0.0;
+    }
+
+    LEDRed_OFF;
+
+} // ErectGyros
+
+void GyroTest(void) {
+    TxString("\r\nGyro Test - ");
+    ShowGyroType();
+
+    ReadGyros();
+
+    TxString("\r\n\tRoll:   \t");
+    TxVal32(GyroADC[Roll] * 1000.0, 3, 0);
+    TxNextLine();
+    TxString("\tPitch:  \t");
+    TxVal32(GyroADC[Pitch] * 1000.0, 3, 0);
+    TxNextLine();
+    TxString("\tYaw:    \t");
+    TxVal32(GyroADC[Yaw] * 1000.0, 3, 0);
+    TxNextLine();
+
+    TxString("Expect ~0.0 ( ~0.5 for for analog gyros)\r\n");
+
+    switch ( GyroType ) {
+        case ITG3200Gyro:
+            ITG3200Test();
+            break;
+        default:
+            break;
+    } // switch
+
+    if ( F.GyroFailure )
+        TxString("\r\nFAILED\r\n");
+
+} // GyroTest
+
+void InitGyros(void) {
+    if ( ITG3200GyroActive() )
+        GyroType = ITG3200Gyro;
+    else
+        GyroType = P[GyroRollPitchType];
+
+    switch ( GyroType ) {
+        case ITG3200Gyro:
+            InitITG3200Gyro();
+            break;
+        case IRSensors:
+            InitIRSensors();
+        default:
+            InitAnalogGyros();
+            break;
+    } // switch
+
+    Delay1mS(50);
+    ErectGyros();
+
+} // InitGyros
+
+void ShowGyroType(void) {
+    switch ( GyroType ) {
+        case MLX90609Gyro:
+            TxString("MLX90609");
+            break;
+        case ADXRS150Gyro:
+            TxString("ADXRS613/150");
+            break;
+        case IDG300Gyro:
+            TxString("IDG300");
+            break;
+        case LY530Gyro:
+            TxString("ST-AY530");
+            break;
+        case ADXRS300Gyro:
+            TxString("ADXRS610/300");
+            break;
+        case ITG3200Gyro:
+            TxString("ITG3200");
+            break;
+        case IRSensors:
+            TxString("IR Sensors");
+            break;
+        default:
+            TxString("unknown");
+            break;
+    } // switch
+} // ShowGyroType
+
+//________________________________________________________________________________________
+
+// Analog Gyros
+
+void ReadAnalogGyros(void);
+void InitAnalogGyros(void);
+void CheckAnalogGyroFault(uint8, uint8, uint8);
+void AnalogGyroTest(void);
+
+void ReadAnalogGyros(void) {
+    static uint8 g;
+
+    GyroADC[Roll] = RollADC.read();
+    GyroADC[Pitch] = PitchADC.read();
+    GyroADC[Yaw] = YawADC.read();
+
+    for ( g = 0; g < (uint8)3; g++ )
+        GyroADC[g] *= GyroToRadian[GyroType];
+} // ReadAnalogGyros
+
+void InitAnalogGyros(void) {
+    // nothing to do
+    F.GyroFailure = false;
+} // InitAnalogGyros
+
+//________________________________________________________________________________________
+
+// ITG3200 3-axis I2C Gyro
+
+void ReadITG3200(void);
+uint8 ReadByteITG3200(uint8);
+void WriteByteITG3200(uint8, uint8);
+void InitITG3200(void);
+void ITG3200Test(void);
+boolean ITG3200Active(void);
+
+real32 ITG3200Temperature;
+
+void ReadITG3200Gyro(void) {
+    static char G[6];
+    static uint8 g;
+    static i16u GX, GY, GZ;
+
+    I2CGYRO.start();
+    if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto SGerror;
+    I2CGYRO.stop();
+
+    I2CGYRO.read(ITG3200_R, G, 6);
+
+    GX.b0 = G[1];
+    GX.b1 = G[0];
+    GY.b0 = G[3];
+    GY.b1 = G[2];
+    GZ.b0 = G[5];
+    GZ.b1 = G[4];
+
+    if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up 
+        GyroADC[Roll] = -(real32)GY.i16;
+        GyroADC[Pitch] = -(real32)GX.i16;
+        GyroADC[Yaw] = -(real32)GZ.i16;
+    } else { // SparkFun 6DOF breakout pins forward components down
+        GyroADC[Roll] = -(real32)GX.i16;
+        GyroADC[Pitch] = -(real32)GY.i16;
+        GyroADC[Yaw] = (real32)GZ.i16;
+    }
+
+    for ( g = 0; g < (uint8)3; g++ )
+        GyroADC[g] *= GyroToRadian[ITG3200Gyro];
+
+    return;
+
+SGerror:
+    I2CGYRO.stop();
+    // GYRO FAILURE - FATAL
+    Stats[GyroFailS]++;
+    F.GyroFailure = true;
+    return;
+} // ReadITG3200Gyro
+
+uint8 ReadByteITG3200(uint8 a) {
+    static uint8 d;
+
+    I2CGYRO.start();
+    if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
+
+    I2CGYRO.start();
+    if ( I2CGYRO.write(ITG3200_R) != I2C_ACK ) goto SGerror;
+    d = I2CGYRO.read(I2C_NACK);
+    I2CGYRO.stop();
+
+    return ( d );
+
+SGerror:
+    I2CGYRO.stop();
+    // GYRO FAILURE - FATAL
+    Stats[GyroFailS]++;
+    F.GyroFailure = true;
+    return (I2C_NACK);
+} // ReadByteITG3200
+
+void WriteByteITG3200(uint8 a, uint8 d) {
+    I2CGYRO.start();    // restart
+    if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(d) != I2C_ACK ) goto SGerror;
+    I2CGYRO.stop();
+    return;
+
+SGerror:
+    I2CGYRO.stop();
+    // GYRO FAILURE - FATAL
+    Stats[GyroFailS]++;
+    F.GyroFailure = true;
+    return;
+} // WriteByteITG3200
+
+void InitITG3200Gyro(void) {
+    F.GyroFailure = false; // reset optimistically!
+
+    WriteByteITG3200(ITG3200_PWR_M, 0x80);    // Reset to defaults
+    WriteByteITG3200(ITG3200_SMPL, 0x00);     // continuous update
+    WriteByteITG3200(ITG3200_DLPF, 0x19);     // 188Hz, 2000deg/S
+    WriteByteITG3200(ITG3200_INT_C, 0x00);    // no interrupts
+    WriteByteITG3200(ITG3200_PWR_M, 0x01);    // X Gyro as Clock Ref.
+
+    Delay1mS(50);
+
+    ReadITG3200Gyro();
+
+} // InitITG3200Gyro
+
+void ITG3200Test(void) {
+    static uint8 MyID, SMPLRT_DIV, DLPF_FS, PWR_MGM;
+    static int16 TEMP,GYRO_X, GYRO_Y, GYRO_Z;
+
+    MyID = ReadByteITG3200(ITG3200_WHO);
+
+    TxString("\tWHO_AM_I  \t0x");
+    TxValH(MyID);
+    TxNextLine();
+    Delay1mS(1);
+    SMPLRT_DIV = ReadByteITG3200(ITG3200_SMPL);
+    DLPF_FS = ReadByteITG3200(ITG3200_DLPF);
+    TEMP = (int16)ReadByteITG3200(ITG3200_TMP_H)<<8 | ReadByteITG3200(ITG3200_TMP_L);
+    GYRO_X = (int16)ReadByteITG3200(ITG3200_GX_H)<<8 | ReadByteITG3200(ITG3200_GX_L);
+    GYRO_Y = (int16)ReadByteITG3200(ITG3200_GY_H)<<8 | ReadByteITG3200(ITG3200_GY_L);
+    GYRO_Z = (int16)ReadByteITG3200(ITG3200_GZ_H)<<8 | ReadByteITG3200(ITG3200_GZ_L);
+    PWR_MGM = ReadByteITG3200(ITG3200_PWR_M);
+
+    ITG3200Temperature = 35.0 + ((TEMP + 13200.0 ) / 280.0);
+
+    TxString("\tSMPLRT_DIV\t");
+    TxVal32( SMPLRT_DIV,0,0);
+    TxNextLine();
+    TxString("\tDLPF      \t");
+    TxVal32( DLPF_FS & 7,0,0 );
+    TxString(" FS         \t");
+    TxVal32( (DLPF_FS>>3)&3, 0, 0);
+    TxNextLine();
+    TxString("\tTEMP      \t");
+    TxVal32( TEMP, 0, 0);
+    TxNextLine();
+    TxString("\tGYRO_X    \t");
+    TxVal32( GYRO_X, 0, 0);
+    TxNextLine();
+    TxString("\tGYRO_Y    \t");
+    TxVal32( GYRO_Y, 0, 0);
+    TxNextLine();
+    TxString("\tGYRO_Z    \t");
+    TxVal32( GYRO_Z, 0, 0);
+    TxNextLine();
+    TxString("\tPWR_MGM   \t0x");
+    TxValH( PWR_MGM );
+    TxNextLine();
+
+    TxNextLine();
+
+} // ITG3200Test
+
+boolean ITG3200GyroActive(void) {
+    I2CGYRO.start();
+    F.GyroFailure = I2CGYRO.write(ITG3200_ID) != I2C_ACK;
+    I2CGYRO.stop();
+    
+    if ( !F.GyroFailure )
+        TrackMinI2CRate(400000);
+
+    return ( !F.GyroFailure );
+
+} // ITG3200GyroActive