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

 void CalcHeading(void)
+{
         double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
-        double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
+    int16_t nick, roll;
         int16_t heading = -1;
         // calculate nick and roll angle in rad
         switch(AttitudeSource)
+        {
+                case ATTITUDE_SOURCE_I2C:
-                case ATTITUDE_SOURCE_I2C:
+           cli(); // stop interrupts
-                        nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0);
+            nick = I2C_WriteAttitude.Nick;
+                        roll = I2C_WriteAttitude.Roll;
-                        roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0);
+           sei(); // start interrupts
-                        break;
                         break;
+                case ATTITUDE_SOURCE_UART:
-                case ATTITUDE_SOURCE_UART:
+           cli(); // stop interrupts
-                        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
+            nick = ExternData.Attitude[NICK];
+            roll = ExternData.Attitude[ROLL];
-                        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
+           sei(); // start interrupts
-                        break;
                         break;
                 case ATTITUDE_SOURCE_ACC:
-                        nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
+            nick = AccAttitudeNick;
-                        roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
+            roll = AccAttitudeRoll;
-                        break;
                         break;
                 default:
                         nick_rad = 0;
                         roll_rad = 0;
                 break;
+        }
+    nick_rad = ((double)nick) * M_PI / (double)(1800.0);
     roll_rad = ((double)roll) * M_PI / (double)(1800.0);
         // 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;
-        DebugOut.Analog[28] = (int16_t)Hy;
+        DebugOut.Analog[27] = (int16_t)Hx;
         DebugOut.Analog[28] = (int16_t)Hy;
         // calculate Heading
         heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
+        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
-        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
+        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
-        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
+        if (heading < 0) heading = -heading;
+        else heading = 360 - heading;
-        if (heading < 0) heading = -heading;
+   cli(); // stop interrupts
-        else heading = 360 - heading;
+        if(abs(heading) < 361) Heading = heading;
         else (Heading = -1);
-        if(abs(heading) < 361) Heading = heading;
+   sei(); // start interrupts
         else (Heading = -1);
+}
+void Calibrate(void)
 void Calibrate(void)
         uint8_t cal;
-        uint8_t cal;
+        static uint8_t calold = 0;
         // blink code for current calibration state
         if(cal)
+        {
                 if(CheckDelay(Led_Timer) || (cal != calold))
+                {
-                        if(blinkcount & 0x01) LED_GRN_OFF;
+                        if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
-                        else LED_GRN_ON;
+                        else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;
                         // end of blinkcount sequence
-                        if( (blinkcount + 1 ) >= (2 * cal) )
+                        if((blinkcount + 1 ) >= (2 * cal))
+                        {
                                 blinkcount = 0;
                                 Led_Timer = SetDelay(1000);
+                        }
+                        }
+                }
+        }
         else
+        {
-                LED_GRN_OFF;
+                if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
+        }
         // calibration state machine
         switch(cal)
                         Zmin =  10000;
                         Zmax = -10000;
                         Calibration.AccX.Offset = RawAccX;
                         Calibration.AccY.Offset = RawAccY;
                 Calibration.AccZ.Offset = RawAccZ;
+            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;
-                        if(UncalMagY > Ymax) Ymax = UncalMagY;
+            invert_blinking = 1;
                         break;
+                case 3: // 3rd step of calibration
-                case 3: // 3rd step of calibration
+                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
-                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
+            invert_blinking = 0;
                         break;
                 case 4:
+                        // find Min and Max of the Z-Sensor
-                        // find Min and Max of the Z-Sensor
+                        if(UncalMagZ < Zmin) Zmin = UncalMagZ;
-                        if(UncalMagZ < Zmin) Zmin = UncalMagZ;
+                        if(UncalMagZ > Zmax) Zmax = UncalMagZ;
-                        if(UncalMagZ > Zmax) Zmax = UncalMagZ;
+            invert_blinking = 1;
                         break;
                                         Led_Timer = SetDelay(2000);
                                         // reset  blinkcode
                                         blinkcount = 0;
+                                }
+                        }
+            invert_blinking = 0;
                         break;
                 default:
                         break;

Subversion Repositories MK3Mag

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