WebSVN - FlightCtrl - Diff - Rev 727 and 741 - /branches/V0.68d Code Redesign killagreg/mm3.c

 /*
-Copyright 2007, Niklas Nold
+Copyright 2008, by Killagreg
-This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify
+This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
 either version 3 of the License, or (at your option) any later version.
 This program 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 Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
-GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>.
 along with this program. If not, see <http://www.gnu.org/licenses/>.
 Please note: The original implementation was done by Niklas Nold.
-Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
+All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
 */
 #define MM3_PERIOD_2048 0x60
 #define MM3_PERIOD_4096 0x70
 MM3_calib_t MM3_calib;
+volatile MM3_working_t MM3;
-volatile MM3_working_t MM3;
+static volatile uint8_t MM3_Timeout = 0;
         MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
         MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
         MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
         MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
+        MM3_Timeout = 0;
         SREG = sreg;
+}
 /*********************************************/
 /*  Get Data from MM3                        */
 /*********************************************/
-void MM3_Update() // called every 102.4 ms by timer 0 ISR
+void MM3_Update() // called every 102.4 µs by timer 0 ISR
+{
         switch (MM3.STATE)
         case MM3_STATE_START_TRANSFER:
                 PORTC &= ~(1<<PORTC5);  // PC4 auf Low (was 102.4 µs at high level)
                 // write to SPDR triggers automatically the transfer MOSI MISO
+                // MM3 Period, + AXIS code
+                switch(MM3.AXIS)
+                {
-                // MM3 Period, + AXIS code
+                case MM3_X_AXIS:
+                        SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
+                        break;
-                if (MM3.AXIS == MM3_X_AXIS) SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
+                case MM3_Y_AXIS:
+                        SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
+                        break;
-                else if (MM3.AXIS == MM3_Y_AXIS) SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
+                case MM3_Z_AXIS:
+                        SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
+                        break;
+                default:
+                        MM3.AXIS = MM3_X_AXIS;
+                        MM3.STATE = MM3_STATE_RESET;
+                        return;
                 else SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; // MM3_Z_AXIS
                 // DRDY line is not connected, therefore
                 // wait before reading data back
                                 break;
+                        }
+                }
                 PORTC |= (1<<PORTC4); // deselect slave
                 MM3.STATE = MM3_STATE_RESET;
+                // Update timeout is called every 102.4 µs.
+                // It takes 2 cycles to write a measurement data request for one axis and
+                // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.
+                // I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
+                // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
+                // decremtent the MM3_Timeout every 100 ms.
+                // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
+                if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis))
+                {       // if all axis measurements give diffrent readings the data should be valid
+                        if(MM3_Timeout < 20) MM3_Timeout++;
+                }
+                else // something is very strange here
+                {
+                        if(MM3_Timeout ) MM3_Timeout--;
+                }
+                return;
+        default:
+                return;
+        }
+}
         GRN_ON;
         ROT_OFF;
         // get maximum and minimum reading of all axis
-        while (measurement)
+        while (measurement && !MM3_Timeout)
+        {
                 if (MM3.x_axis > x_max) x_max = MM3.x_axis;
                         GRN_FLASH;
                         BeepTime = 50;
                         beeper = 50;
+                }
                 beeper--;
                 // loop with period of 10 ms / 100 Hz
                 timer = SetDelay(10);
                 while(!CheckDelay(timer));
                 // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
                 if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
+        }
-        }
+        if(!MM3_Timeout)
+        {
-        // Rage of all axis
+                // Rage of all axis
-        MM3_calib.X_range = (x_max - x_min);
+                MM3_calib.X_range = (x_max - x_min);
-        MM3_calib.Y_range = (y_max - y_min);
+                MM3_calib.Y_range = (y_max - y_min);
-        SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
+                SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
-        SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
+                SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
-        SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
+                SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
-        SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
+                SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
+        }
+}
 /*********************************************/
         int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
         int16_t angle;
         uint16_t div_factor;
         int16_t heading;
+        if (MM3_Timeout)
+        {
-        // calibration factor for transforming Gyro Integrals to angular degrees
+                // calibration factor for transforming Gyro Integrals to angular degrees
-        div_factor = (uint16_t)ParamSet.UserParam3 * 8;
+                div_factor = (uint16_t)ParamSet.UserParam3 * 8;
-    // atan returns angular range from -180 deg to 180 deg in counter clockwise notation
+                // atan 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;
+                if (heading < 0) heading = -heading;
+                else heading = 360 - heading;
+        }
+        else // MM3_Timeout = 0 i.e now new data from external board
+        {
-        else heading = 360 - heading;
+                heading = -1;
+        }
 return heading;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/mm3.c @ 757 - Rev 727 → 741