WebSVN - FlightCtrl - Path Comparison - /branches/ Rev 225 and /branches/ Rev 226

Ignore whitespace Rev 225 → Rev 226

 /branches/v0.60_MicroMag3_Nick666/compass.c
 ,14 → 20,13
 void init_spi(void)
 //############################################################################
 {
-        SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0);
+        SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0);        //Interrupt an, Master, 156 kHz Oszillator
         //SPSR = (1<<SPI2X);
-        DDRB |= (1<<PB2)|(1<<PB7)|(1<<PB5); // J8, SCK, MOSI Ausgang (MicroMag3)
+    DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang
         MM3.AXIS = MM3_X;
         MM3.STATE = MM3_RESET;
-        PORTD &= ~(1<<PD3);     // J5 einmalig auf Low -> SSNOT
 }
 ,26 → 43,26
                 break;
         case MM3_START_TRANSFER:
-                PORTB &= ~(1<<PB2);     // J8 auf Low
+                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)
-                if (MM3.AXIS == MM3_X) SPDR = 0x41;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
-                else if (MM3.AXIS == MM3_Y) SPDR = 0x42;
-                else if (MM3.AXIS == MM3_Z) SPDR = 0x43;
+                if (MM3.AXIS == MM3_X) SPDR = 0x51;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
+                else if (MM3.AXIS == MM3_Y) SPDR = 0x52;                // Micromag Period Select ist auf 1024 (0x50)
+                else if (MM3.AXIS == MM3_Z) SPDR = 0x53;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
                 else {MM3.STATE == MM3_IDLE;break;}
-                MM3.DRDY = SetDelay(8);
+                MM3.DRDY = SetDelay(15);                // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
                 MM3.STATE = MM3_WAIT_DRDY;
                 break;
         case MM3_WAIT_DRDY:
-                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird
+                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
                 break;
         case MM3_IDLE:
                 break;
         }
 }
 //############################################################################
 //SPI byte ready
 ,28 → 71,59
 {
         switch (MM3.STATE)
         {
-        case MM3_DRDY:
+        case MM3_DRDY:
+                        // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken und Übertragung von 2. Byte auslösen
                 if (MM3.AXIS == MM3_X) {MM3.x_axis=SPDR; MM3.x_axis<<=8; SPDR=0x00; MM3.STATE=MM3_X_BYTE2; break;}
                 if (MM3.AXIS == MM3_Y) {MM3.y_axis=SPDR; MM3.y_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Y_BYTE2; break;}
                 if (MM3.AXIS == MM3_Z) {MM3.z_axis=SPDR; MM3.z_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Z_BYTE2; break;}
-        case MM3_X_BYTE2:
-                MM3.x_axis |= SPDR;
+        case MM3_X_BYTE2:       // 2. Byte der entsprechenden Achse ist da.
+                MM3.x_axis |= SPDR;
+                MM3.x_axis -= OFF_X;    // Sofort Offset aus der Kalibrierung berücksichtigen
+                //MM3.x_axis /= GAIN_X;
                 MM3.AXIS = MM3_Y;
                 MM3.STATE = MM3_RESET;
                 break;
         case MM3_Y_BYTE2:
-                MM3.y_axis |= SPDR;
+                MM3.y_axis |= SPDR;
+                MM3.y_axis -= OFF_Y;
+                //MM3.y_axis /= GAIN_Y;
                 MM3.AXIS = MM3_Z;
                 MM3.STATE = MM3_RESET;
                 break;
         case MM3_Z_BYTE2:
-                MM3.z_axis |= SPDR;
+                MM3.z_axis |= SPDR;
+                MM3.z_axis -= OFF_Z;
+                //MM3.z_axis /= GAIN_Z;
                 MM3.AXIS = MM3_X;
-                MM3.STATE = MM3_RESET;
+                MM3.STATE = MM3_RESET;
+                // Zeitnahe Berechnung der Nick-/Rollneigung in °
+                MM3.NickGrad = IntegralNick/Int2Grad_Faktor;
+                MM3.RollGrad = IntegralRoll/Int2Grad_Faktor;
                 break;
         }
+}
+signed int MM3_heading(void)
+{
+        float sin_nick, cos_nick, sin_roll, cos_roll;
+        signed int x_corr, y_corr;
+        signed int heading;
+        //Berechung von sinus und cosinus
+        sin_nick = (float)sin_i(MM3.NickGrad)/1000;
+        cos_nick = (float)cos_i(MM3.NickGrad)/1000;
+        sin_roll = (float)sin_i(MM3.RollGrad)/1000;
+        cos_roll = (float)cos_i(MM3.RollGrad)/1000;
+    //Neigungskompensation
+        y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
+    x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis);
+        //Winkelberechnung
+        heading = arctan_f(x_corr, y_corr);
+return (heading);
 }

 /branches/v0.60_MicroMag3_Nick666/compass.h
 ,12 → 2,14
 typedef struct
 {
-        unsigned char STATE;
+        uint8_t STATE;
         unsigned int DRDY;
-        unsigned char AXIS;
+        uint8_t AXIS;
         signed int x_axis;
         signed int y_axis;
         signed int z_axis;
+        signed int NickGrad;
+        signed int RollGrad;
 }MM3_struct;
 extern MM3_struct MM3;
 ,7 → 16,11
 void init_spi(void);
 void MM3_timer0(void);
+signed int MM3_heading(void);
+#define Int2Grad_Faktor         1024
+// Die Werte der Statemachine
 #define MM3_RESET                       0
 #define MM3_START_TRANSFER      1
 #define MM3_WAIT_DRDY           2
 ,8 → 33,16
 #define MM3_Z                           9
 #define MM3_IDLE                        255
-#define PS2             6
-#define PS1             5
-#define PS0             5
-#define AS1             1
-#define AS0             0
+// Die Werte der Kompasskalibrierung
+#define RANGE_X 1161
+#define RANGE_Y 1209
+#define RANGE_Z 1276
+#define OFF_X   -46
+#define OFF_Y   52
+#define OFF_Z   25
+#define GAIN_X  14
+#define GAIN_Y  15
+#define GAIN_Z  16

 /branches/v0.60_MicroMag3_Nick666/fc.c
 ,7 → 679,13
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
     DebugOut.Analog[7] = GasMischanteil;
-    DebugOut.Analog[8] = KompassValue;
+//    DebugOut.Analog[8] = KompassValue;
+DebugOut.Analog[8] = MM3_heading();
+DebugOut.Analog[9] = MM3.x_axis;
+DebugOut.Analog[10] = MM3.y_axis;
+DebugOut.Analog[11] = MM3.z_axis;
 //    DebugOut.Analog[9] = SollHoehe;
 //    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 //    DebugOut.Analog[11] = KompassStartwert;

 /branches/v0.60_MicroMag3_Nick666/math.c
 ,14 → 13,16
 #include "main.h"
 //-----------------------------------------------
-// Fast arctan2 with max error of .01 rads
+// Fast arctan2 with max error of .07 rads
 // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
 //-----------------------------------------------
-float arctan_f(signed int x, signed int y)
+signed char arctan_f(signed int x, signed int y)
 {
-   float rad,r;
-   signed int angle;
-   short int m;
+        float rad,r;
+        short int m;
+        #define coeff_1         0.7854
+        #define coeff_2         2.3562
+        #define rad2grad        57.2958
    if (!x && !y) return 0;
 ,15 → 31,17
    if (x>=0)
    {
-      r = (x - y) / (x + y);
-      rad = 0.1963*r*r*r - 0.9817*r + 0.7854;
+      r = (float)(x - y) / (float)(x + y);
+      rad = coeff_1 - coeff_1*r;
    }
    else
    {
-      r = (x + y) / (y - x);
-      rad = 0.1963*r*r*r - 0.9817*r + 2.3562;
+      r = (float)(x + y) / (float)(y - x);
+      rad = coeff_2 - coeff_1*r;
    }
+   rad *= rad2grad;
    return(rad*m);
 }
 ,9 → 51,8
 Stand 1.10.2007
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 // arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
-int8_t arctan_i( signed int x,  signed int y)
+signed int arctan_i(signed int x,  signed int y)
 {
         short int change_xy = 0;
         signed int i;
 ,7 → 70,8
         // Quadranten ermitteln
         // Wert durch lineare Interpolation ermitteln
-        if ((y == 0) && (x == 0))  wert =1; // Division durch 0 nicht erlaubt
+        if (!y && !x)  return 0; // Division durch 0 nicht erlaubt
         else wert= abs((x*1000)/y);
         if (wert <=268) //0...0.0,268  entsprechend 0..15 Grad
 ,16 → 94,16
 return return_value;
 }
+const unsigned int pgm_sinus[91] PROGMEM = {0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000};
 // cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
 signed int cos_i(signed int winkel)
-{
-  winkel = sin_i(90-winkel);
-  return winkel;
+{
+  return (sin_i(90-winkel));
 }
-const unsigned int pgm_sinus[91] PROGMEM = {0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000};
 // sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000
 signed int sin_i(signed int winkel)
 {
 ,3 → 130,51
 }
+const float pgm_sinus_f [91] PROGMEM = {0.00,0.02,0.03,0.05,0.07,0.09,0.10,0.12,0.14,0.16,0.17,0.19,0.21,0.23,0.24,0.26,0.28,0.29,0.31,0.33,0.34,0.36,0.38,0.39,0.41,0.42,0.44,0.45,0.47,0.49,0.50,0.52,0.53,0.55,0.56,0.57,0.59,0.60,0.62,0.63,0.64,0.66,0.67,0.68,0.70,0.71,0.72,0.73,0.74,0.76,0.77,0.78,0.79,0.80,0.81,0.82,0.83,0.84,0.85,0.86,0.87,0.88,0.88,0.89,0.90,0.91,0.91,0.92,0.93,0.93,0.94,0.95,0.95,0.96,0.96,0.97,0.97,0.97,0.98,0.98,0.98,0.99,0.99,0.99,0.99,1.00,1.00,1.00,1.00,1.00,1.00};
+inline float pgm_read_float(const float *addr)
+{
+        union
+        {
+                uint16_t i[2];  // 2 16-bit-Worte
+                float f;
+        } u;
+        u.i[0]=pgm_read_word((PGM_P)addr);
+        u.i[1]=pgm_read_word((PGM_P)addr+2);
+        return u.f;
+}
+// cosinus Funktion: Eingabewert Winkel in Grad
+float cos_f(signed int winkel)
+{
+ return (sin_f(90-winkel));
+}
+// sinus Funktion: Eingabewert Winkel in Grad
+float sin_f(signed int winkel)
+{
+ short int m,n;
+ float sinus;
+ //winkel = winkel % 360;
+ if (winkel < 0)
+ {
+        m = -1;
+        winkel = abs(winkel);
+ }
+ else m = +1;
+ // Quadranten auswerten
+ if ((winkel > 90 ) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;}
+ else if ((winkel > 180 ) && (winkel <= 270)) {winkel = winkel - 180; n = -1;}
+ else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;}
+ else n = 1; //0 - 90 Grad
+sinus = pgm_read_float(&pgm_sinus_f[winkel]);
+return (sinus*m*n);
+}

 /branches/v0.60_MicroMag3_Nick666/math.h
 ,12 → 5,12
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
+extern signed int arctan_i(signed int x, signed int y);
+extern signed char arctan_f(signed int x, signed int y);
 extern signed int sin_i(signed int winkel);
 extern signed int cos_i(signed int winkel);
-extern int8_t arctan_i(signed int x, signed int y);
-extern float arctan_f(signed int x, signed int y);
+extern float cos_f(signed int winkel);
+extern float sin_f(signed int winkel);
-#define Pi              3.1416

Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 225 → Rev 226