WebSVN - MK3Mag - Diff - Rev 36 and 37

 Calibration_t eeCalibration EEMEM;      // calibration data in EEProm
 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 heading = -1;
         // blink code for normal operation
         if(CheckDelay(Led_Timer))
         {
-        if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE;
+                if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE;
                 else LED_GRN_ON;
                 Led_Timer = SetDelay(150);
+        }
         switch(Orientation)
         // calculate nick and roll angle in rad
         switch(AttitudeSource)
+        {
                 case ATTITUDE_SOURCE_I2C:
-           cli(); // stop interrupts
+                        cli(); // stop interrupts
-            nick = I2C_WriteAttitude.Nick;
+                        nick = I2C_WriteAttitude.Nick;
                         roll = I2C_WriteAttitude.Roll;
-           sei(); // start interrupts
+                        sei(); // start interrupts
                         break;
                 case ATTITUDE_SOURCE_UART:
-           cli(); // stop interrupts
+                        cli(); // stop interrupts
-            nick = ExternData.Attitude[NICK];
+                        nick = ExternData.Attitude[NICK];
-            roll = ExternData.Attitude[ROLL];
+                        roll = ExternData.Attitude[ROLL];
-           sei(); // start interrupts
+                        sei(); // start interrupts
                         break;
                 case ATTITUDE_SOURCE_ACC:
-            nick = AccAttitudeNick;
+                        nick = AccAttitudeNick;
-            roll = AccAttitudeRoll;
+                        roll = AccAttitudeRoll;
                         break;
                 default:
                         nick = 0;
                         roll = 0;
                 break;
         }
-    nick_rad = ((double)nick) * M_PI / (double)(1800.0);
+        nick_rad = ((double)nick) * M_PI / (double)(1800.0);
-    roll_rad = ((double)roll) * M_PI / (double)(1800.0);
+        roll_rad = ((double)roll) * M_PI / (double)(1800.0);
         static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
         static uint8_t blinkcount = 0;
         static uint8_t invert_blinking = 0;
         // check both sources of communication for calibration request
-        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
+        if(I2C_WriteCal.CalByte)        cal = I2C_WriteCal.CalByte;
-        else                     cal = ExternData.CalState;
+        else                                            cal = ExternData.CalState;
                         Ymax = -10000;
                         Zmin =  10000;
                         Zmax = -10000;
                         Calibration.AccX.Offset = RawAccX;
                         Calibration.AccY.Offset = RawAccY;
-                Calibration.AccZ.Offset = RawAccZ;
+                        Calibration.AccZ.Offset = RawAccZ;
-            invert_blinking = 0;
+                        invert_blinking = 0;
                         break;
                 case 2: // 2nd step of calibration
                         // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
                         if(UncalMagX < Xmin) Xmin = UncalMagX;
                         if(UncalMagX > Xmax) Xmax = UncalMagX;
                         if(UncalMagY < Ymin) Ymin = UncalMagY;
                         if(UncalMagY > Ymax) Ymax = UncalMagY;
-            invert_blinking = 1;
+                        invert_blinking = 1;
                         break;
                 case 3: // 3rd step of calibration
                         // used to change the orientation of the MK3MAG vertical to the horizontal plane
-            invert_blinking = 0;
+                        invert_blinking = 0;
                         break;
                 case 4:
                         // find Min and Max of the Z-Sensor
                         if(UncalMagZ < Zmin) Zmin = UncalMagZ;
                         if(UncalMagZ > Zmax) Zmax = UncalMagZ;
-            invert_blinking = 1;
+                        invert_blinking = 1;
                         break;
                                         Led_Timer = SetDelay(2000);
                                         // reset  blinkcode
                                         blinkcount = 0;
+                                }
+                        }
-            invert_blinking = 0;
+                        invert_blinking = 0;
                         break;
                 default:
                         break;
         DebugOut.Analog[21] = RawAccX;
         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[26] = Calibration.AccZ.Offset;
-    DebugOut.Analog[29] = AttitudeSource;
+        DebugOut.Analog[29] = AttitudeSource;
+}
 void AccMeasurement(void)
+{
+{
         // reset input pullup
         DDRC &=~((1<<DDC6));
         PORTC |= (1<<PORTC6);
-    LED_Init();
+        LED_Init();
-    TIMER0_Init();
+        TIMER0_Init();
-    USART0_Init();
+        USART0_Init();
-    ADC_Init();
+        ADC_Init();
         I2C_Init();
-    sei(); // enable globale interrupts
+        sei(); // enable globale interrupts
-    if(AccPresent)
+        if(AccPresent)
-    {
+        {
                 USART0_Print("ACC present\n");
         }
-    LED_GRN_ON;
+        LED_GRN_ON;
-    Debug_Timer = SetDelay(200);
+        Debug_Timer = SetDelay(200);
-    Led_Timer = SetDelay(200);
+        Led_Timer = SetDelay(200);
         // read calibration info from eeprom
         eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
-    ExternData.CalState = 0;
+        ExternData.CalState = 0;
-    I2C_WriteCal.CalByte = 0;
+        I2C_WriteCal.CalByte = 0;
                 if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
                 else CalcHeading();
                 // check data from USART
-        USART0_ProcessRxData();
+                USART0_ProcessRxData();
                 if(NC_Connected) NC_Connected--;
                 if(FC_Connected) FC_Connected--;
                 // fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data
                 if(!FC_Connected && ! NC_Connected)
+                {
                         AttitudeSource = ATTITUDE_SOURCE_ACC;
                         Orientation = ORIENTATION_FC;
                 }
-        if(PC_Connected)
+                if(PC_Connected)
+                {
-            USART0_EnableTXD();
+                        USART0_EnableTXD();
-            USART0_TransmitTxData();
+                        USART0_TransmitTxData();
-            PC_Connected--;
+                        PC_Connected--;
+                }

Subversion Repositories MK3Mag

(root)/tags/V0.21b/main.c @ 62 - Rev 36 → 37