WebSVN - MK3Mag - Diff - Rev 28 and 29 - /branches/MK3Mag V0.14 Code Redesign Killagreg/main.c

 #include "analog.h"
 #include "uart.h"
 AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
-AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
+Orientation_t Orientation = ORIENTATION_FC;
         struct Scaling_t MagY;
         struct Scaling_t MagZ;
         struct Scaling_t AccX;
         struct Scaling_t AccY;
         struct Scaling_t AccZ;
-} ;
+};
 struct Calibration_t eeCalibration EEMEM;       // calibration data in EEProm
 struct Calibration_t Calibration;               // calibration data in RAM
 // magnet sensor variable
 int16_t RawMagnet1a, RawMagnet1b;                       // raw magnet sensor data
 int16_t RawMagnet2a, RawMagnet2b;
 int16_t RawMagnet3a, RawMagnet3b;
+int16_t UncalMagX, UncalMagY, UncalMagZ;        // sensor signal difference without Scaling
-int16_t UncalMagX, UncalMagY, UncalMagZ;        // sensor signal difference without Scaling
+int16_t MagX = 0, MagY = 0, MagZ = 0;           // rescaled magnetic field readings
-int16_t MagX, MagY, MagZ;                                       // rescaled magnetic field readings
-// acc sensor variables
+// acceleration sensor variables
-int16_t RawAccX, RawAccY, RawAccZ;                      // raw acceleration readings
-int16_t AccX, AccY, AccZ;                                       // rescaled acceleration readings
+int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0;                  // raw acceleration readings
-uint8_t AccPresent = 0;
+int16_t AccX = 0, AccY = 0, AccZ = 0;                                   // rescaled acceleration readings
-uint8_t PC_Connected = 0;
+int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0;               // nick and roll angle from acc
-int16_t Heading = -1;
+int16_t Heading = -1;                                           // the current compass heading in deg
         else MagX = 0;
         if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range);
         else MagY = 0;
         if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
         else MagZ = 0;
+        if(AccPresent)
+        {
+                AccX = (RawAccX - Calibration.AccX.Offset);
+                AccY = (RawAccY - Calibration.AccY.Offset);
+                AccZ = (Calibration.AccZ.Offset - RawAccZ);
+                #if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL
+                // acc mode assumes orientation like FC
+                if(AccX >  136) AccAttitudeNick = -800;
+                else
+                if(AccX < -136) AccAttitudeNick = 800;
+                else                    AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI);
+                if(AccY >  136) AccAttitudeRoll = 800;
+                else
+                if(AccY < -136) AccAttitudeRoll = -800;
+                else                    AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
+                #else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z)
+                // acc mode assumes orientation like FC
+                if(AccY >  136) AccAttitudeNick = 800;
+                else
+                if(AccY < -136) AccAttitudeNick = -800;
+                else                    AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
+                if(AccX >  136) AccAttitudeRoll = 800;
+                else
+                if(AccX < -136) AccAttitudeRoll = -800;
+                else                    AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI);
+                #endif
+        }
+}
 void CalcHeading(void)
+{
-        double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
+        double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
         int16_t heading = -1;
         // blink code for normal operation
         if(CheckDelay(Led_Timer))
+        {
+                LED_GRN_TOGGLE;
+                Led_Timer = SetDelay(500);
+        }
-                LED_GRN_TOGGLE;
-                Led_Timer = SetDelay(500);
+        switch(Orientation)
-        }
+        {
+                case ORIENTATION_NC:
+                        Cx = MagX;
+                        Cy = MagY;
-        Cx = MagX;
+                        Cz = MagZ;
-        Cy = MagY;
-        Cz = MagZ;
+                        break;
-        if(ExternData.Orientation == 1)
+                case ORIENTATION_FC:
+                        // rotation of 90 deg compared to NC setup
                         Cx = MagY;
-                Cx = MagX;
+                        Cy = -MagX;
-                Cy = -MagY;
+                        Cz = MagZ;
-                Cz = MagZ;
+                        break;
                         nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
                         roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
                         break;
-                case ATTITUDE_SOURCE_ACC:
-                        if(AccX >  125) nick_rad = M_PI / 2;
-                        else
-                        if(AccX < -125) nick_rad = -M_PI / 2;
-                        else
-                        {
+                case ATTITUDE_SOURCE_ACC:
-                                nick_rad = asin((double) AccX / 125.0);
-                        }
-                        if(AccY >  125) roll_rad = M_PI / 2;
-                        else
-                        if(AccY < -125) roll_rad = -M_PI / 2;
-                        else
-                        {
+                        nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
-                        roll_rad = asin((double) AccY / 125.0);
                         roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
+                        break;
+                default:
+                        nick_rad = 0;
+                        roll_rad = 0;
-                        break;
+                break;
+        }
         // calculate attitude correction
+        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
+        Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
+        DebugOut.Analog[27] = (int16_t)Hx;
-        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
+        DebugOut.Analog[28] = (int16_t)Hy;
         Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
         // calculate Heading
         heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
                 case 5:
                         // Save values
                         if(cal != calold) // avoid continously writing of eeprom!
                         {
-                                Calibration.MagY.Range = Xmax - Xmin;
+                                Calibration.MagX.Range = Xmax - Xmin;
                                 Calibration.MagX.Offset = (Xmin + Xmax) / 2;
                                 Calibration.MagY.Range = Ymax - Ymin;
                                 Calibration.MagY.Offset = (Ymin + Ymax) / 2;
                                 Calibration.MagZ.Range = Zmax - Zmin;
                                 Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
                                 if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150))
+                                {
                                         // indicate write process by setting the led
                                         LED_GRN_ON;
                                         eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
-                                        Delay_ms(2000);
-                                        // reset led state
-                                        LED_GRN_OFF;
+                                        Led_Timer = SetDelay(2000);
                                         // reset  blinkcode
-                                        blinkcount = 0;
-                                        Led_Timer = SetDelay(1000);
+                                        blinkcount = 0;
+                                }
+                        }
                         break;
         DebugOut.Analog[4] =  UncalMagY;
         DebugOut.Analog[5] =  UncalMagZ;
         switch(AttitudeSource)
+        {
                 case ATTITUDE_SOURCE_ACC:
+                        DebugOut.Analog[6] =  AccAttitudeNick;
+                        DebugOut.Analog[7] =  AccAttitudeRoll;
                         break;
                 case ATTITUDE_SOURCE_UART:
                         DebugOut.Analog[6] =  ExternData.Attitude[NICK];
         DebugOut.Analog[22] = RawAccY;
         DebugOut.Analog[23] = RawAccZ;
         DebugOut.Analog[24] = Calibration.AccX.Offset;
         DebugOut.Analog[25] = Calibration.AccY.Offset;
     DebugOut.Analog[26] = Calibration.AccZ.Offset;
+    DebugOut.Analog[29] = AttitudeSource;
+}
 void AccMeasurement(void)
+{
         if(AccPresent)
         {
-                RawAccX = (RawAccX + (int16_t)ADC_GetValue(ADC2))/2;
+                RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2;
-                RawAccY = (RawAccY + (int16_t)ADC_GetValue(ADC3))/2;
+                RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2;
-                RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ADC6))/2;
+                RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2;
+        }
         else
+        {
                 RawAccX = 0;
                 RawAccY = 0;
                 RawAccZ = 0;
-        }
-                AccX = ((RawAccX - Calibration.AccX.Offset) + AccX * 7) / 8;
-                AccY = ((RawAccY - Calibration.AccY.Offset) + AccY * 7) / 8;
                 AccZ = ((Calibration.AccZ.Offset - RawAccZ) + AccZ * 7) / 8;
-}
+}
 int main (void)
+{
         // reset input pullup
     ADC_Init();
         I2C_Init();
+    sei(); // enable globale interrupts
+    if(AccPresent)
+    {
+                USART0_Print("ACC present\n");
     sei(); // enable globale interrupts
     LED_GRN_ON;
     Debug_Timer = SetDelay(200);
-    Led_Timer = SetDelay(200);
     Led_Timer = SetDelay(200);
         // read calibration info from eeprom
-        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
+        // read calibration info from eeprom
         eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
     ExternData.Orientation = 0;
     ExternData.CalState = 0;
     I2C_WriteCal.CalByte = 0;
         // main loop
     while (1)
+    {
                 FLIP_LOW;
                 Delay_ms(2);
-                RawMagnet1a = ADC_GetValue(ADC0);
+                RawMagnet1a = ADC_GetValue(MAG_X);
-                RawMagnet2a = -ADC_GetValue(ADC1);
+                RawMagnet2a = -ADC_GetValue(MAG_Y);
-                RawMagnet3a = ADC_GetValue(ADC7);
+                RawMagnet3a = ADC_GetValue(MAG_Z);
                 AccMeasurement();
                 Delay_ms(1);
                 FLIP_HIGH;
                 Delay_ms(2);
-                RawMagnet1b = ADC_GetValue(ADC0);
+                RawMagnet1b = ADC_GetValue(MAG_X);
-                RawMagnet2b = -ADC_GetValue(ADC1);
+                RawMagnet2b = -ADC_GetValue(MAG_Y);
+                RawMagnet3b = ADC_GetValue(MAG_Z);
+                AccMeasurement();
+                Delay_ms(1);
+                CalcFields();
+                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
-                RawMagnet3b = ADC_GetValue(ADC7);
+                else CalcHeading();
-                AccMeasurement();
+                // check data from USART
-                Delay_ms(1);
+        USART0_ProcessRxData();
-                CalcFields();
+                if(NC_Connected) NC_Connected--;

Subversion Repositories MK3Mag

(root)/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c - Rev 28 → 29