WebSVN - FlightCtrl - Diff - Rev 838 and 966 - /branches/KalmanFilter MikeW/mm3.c

 extern int Theta45;
 extern int Phi45;
 typedef struct
 {
-        uint8_t STATE;
+  uint8_t STATE;
-        uint16_t DRDY;
+  uint16_t DRDY;
-        uint8_t AXIS;
+  uint8_t AXIS;
-        int16_t x_axis;
+  int16_t x_axis;
-        int16_t y_axis;
+  int16_t y_axis;
-        int16_t z_axis;
+  int16_t z_axis;
 } MM3_working_t;
 // MM3 State Machine
-#define MM3_STATE_RESET                         0
+#define MM3_STATE_RESET           0
-#define MM3_STATE_START_TRANSFER        1
+#define MM3_STATE_START_TRANSFER  1
-#define MM3_STATE_WAIT_DRDY                     2
+#define MM3_STATE_WAIT_DRDY       2
-#define MM3_STATE_DRDY                          3
+#define MM3_STATE_DRDY            3
-#define MM3_STATE_BYTE2                         4
+#define MM3_STATE_BYTE2           4
-#define MM3_X_AXIS              0x01
+#define MM3_X_AXIS  0x01
-#define MM3_Y_AXIS              0x02
+#define MM3_Y_AXIS  0x02
-#define MM3_Z_AXIS              0x03
+#define MM3_Z_AXIS  0x03
 #define MM3_PERIOD_32   0x00
 #define MM3_PERIOD_64   0x10
-#define MM3_PERIOD_128  0x20
+#define MM3_PERIOD_128  0x20
-#define MM3_PERIOD_256  0x30
+#define MM3_PERIOD_256  0x30
-#define MM3_PERIOD_512  0x40
+#define MM3_PERIOD_512  0x40
 #define MM3_PERIOD_1024 0x50
 /***************************************************/
 /*       Read Parameter from EEPROM as byte        */
 /***************************************************/
 uint8_t GetParamByte(uint8_t param_id)
+{
-        return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
+  return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
+}
 /***************************************************/
 /*       Write Parameter to EEPROM as byte         */
 /***************************************************/
 void SetParamByte(uint8_t param_id, uint8_t value)
 {
-        eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
+  eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
+}
 /***************************************************/
 /*       Read Parameter from EEPROM as word        */
 /***************************************************/
 uint16_t GetParamWord(uint8_t param_id)
+{
-        return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
+  return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
+}
 /***************************************************/
 /*       Write Parameter to EEPROM as word         */
 /***************************************************/
 void SetParamWord(uint8_t param_id, uint16_t value)
+{
-        eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
+  eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
+}
 /*********************************************/
 /*  Initialize Interface to MM3 Compass      */
 /*********************************************/
 void MM3_Init(void)
 {
-        uint8_t sreg = SREG;
+  uint8_t sreg = SREG;
-        cli();
+  cli();
-        // Configure Pins for SPI
+  // Configure Pins for SPI
-        // set SCK (PB7), MOSI (PB5) as output
+  // set SCK (PB7), MOSI (PB5) as output
-        DDRB |= (1<<DDB7)|(1<<DDB5);
+  DDRB |= (1<<DDB7)|(1<<DDB5);
-        // set MISO (PB6) as input
+  // set MISO (PB6) as input
-        DDRB &= ~(1<<DDB6);
+  DDRB &= ~(1<<DDB6);
-        // Output Pins PC4->MM3_SS ,PC5->MM3_RESET
+  // Output Pins PC4->MM3_SS ,PC5->MM3_RESET
-        DDRC |= (1<<DDC4)|(1<<DDC5);
+  DDRC |= (1<<DDC4)|(1<<DDC5);
-        // set pins permanent to low
+  // set pins permanent to low
-        PORTC &= ~((1<<PORTC4)|(1<<PORTC5));
+  PORTC &= ~((1<<PORTC4)|(1<<PORTC5));
-        // Initialize SPI-Interface
+  // Initialize SPI-Interface
-        // Enable interrupt (SPIE=1)
+  // Enable interrupt (SPIE=1)
-        // Enable SPI bus (SPE=1)
+  // Enable SPI bus (SPE=1)
-        // MSB transmitted first (DORD = 0)
+  // MSB transmitted first (DORD = 0)
-        // Master SPI Mode (MSTR=1)
+  // Master SPI Mode (MSTR=1)
-        // Clock polarity low when idle (CPOL=0)
+  // Clock polarity low when idle (CPOL=0)
-        // Clock phase sample at leading edge (CPHA=0)
+  // Clock phase sample at leading edge (CPHA=0)
-        // Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
+  // Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
-        SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
+  SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
-        SPSR &= ~(1<<SPI2X);
+  SPSR &= ~(1<<SPI2X);
-    // Init Statemachine
+  // Init Statemachine
-        MM3.AXIS = MM3_X_AXIS;
+  MM3.AXIS = MM3_X_AXIS;
-        MM3.STATE = MM3_STATE_RESET;
+  MM3.STATE = MM3_STATE_RESET;
-        // Read calibration from EEprom
+  // Read calibration from EEprom
-        MM3_calib.X_off = (int16_t)GetParamWord(PID_MM3_X_OFF);
+  MM3_calib.X_off = (int16_t)GetParamWord(PID_MM3_X_OFF);
-        MM3_calib.Y_off = (int16_t)GetParamWord(PID_MM3_Y_OFF);
+  MM3_calib.Y_off = (int16_t)GetParamWord(PID_MM3_Y_OFF);
-        MM3_calib.Z_off = (int16_t)GetParamWord(PID_MM3_Z_OFF);
+  MM3_calib.Z_off = (int16_t)GetParamWord(PID_MM3_Z_OFF);
-        MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
+  MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
-        MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
+  MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
-        MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
+  MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
-        MM3_Timeout = 0;
+  MM3_Timeout = 0;
-        SREG = sreg;
+  SREG = sreg;
+}
 /*********************************************/
 /*  Get Data from MM3                        */
 /*********************************************/
 void MM3_Update(void) // called every 102.4 µs by timer 0 ISR
 {
-        switch (MM3.STATE)
+  switch (MM3.STATE)
-        {
+  {
-        case MM3_STATE_RESET:
+  case MM3_STATE_RESET:
-                PORTC &= ~(1<<PORTC4); // select slave
+    PORTC &= ~(1<<PORTC4); // select slave
-                PORTC |= (1<<PORTC5);   // PC5 to High, MM3 Reset
+    PORTC |= (1<<PORTC5);       // PC5 to High, MM3 Reset
-                MM3.STATE = MM3_STATE_START_TRANSFER;
+    MM3.STATE = MM3_STATE_START_TRANSFER;
-                return;
+    return;
-        case MM3_STATE_START_TRANSFER:
+  case MM3_STATE_START_TRANSFER:
-                PORTC &= ~(1<<PORTC5);  // PC4 auf Low (was 102.4 µs at high level)
+    PORTC &= ~(1<<PORTC5);      // PC4 auf Low (was 102.4 µs at high level)
-                // write to SPDR triggers automatically the transfer MOSI MISO
+    // write to SPDR triggers automatically the transfer MOSI MISO
-                // MM3 Period, + AXIS code
+    // MM3 Period, + AXIS code
-                switch(MM3.AXIS)
+    switch(MM3.AXIS)
-                {
+    {
-                case MM3_X_AXIS:
+    case MM3_X_AXIS:
-                        SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
+      SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
-                        break;
+      break;
-                case MM3_Y_AXIS:
+    case MM3_Y_AXIS:
-                        SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
+      SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
-                        break;
+      break;
-                case MM3_Z_AXIS:
+    case MM3_Z_AXIS:
-            SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
+      SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
-                        break;
+      break;
-                default:
+    default:
-                        MM3.AXIS = MM3_X_AXIS;
+      MM3.AXIS = MM3_X_AXIS;
-                        MM3.STATE = MM3_STATE_RESET;
+      MM3.STATE = MM3_STATE_RESET;
-                        return;
+      return;
-                }
+    }
-                // DRDY line is not connected, therefore
+    // DRDY line is not connected, therefore
-                // wait before reading data back
+    // wait before reading data back
-                MM3.DRDY = SetDelay(8); // wait 8ms for data ready
+    MM3.DRDY = SetDelay(8); // wait 8ms for data ready
-                MM3.STATE = MM3_STATE_WAIT_DRDY;
+    MM3.STATE = MM3_STATE_WAIT_DRDY;
-                return;
+    return;
-        case MM3_STATE_WAIT_DRDY:
+    case MM3_STATE_WAIT_DRDY:
-                if (CheckDelay(MM3.DRDY))
+      if (CheckDelay(MM3.DRDY))
-                {
+      {
-                        // write something into SPDR to trigger data reading
+        // write something into SPDR to trigger data reading
-                        SPDR = 0x00;
+        SPDR = 0x00;
-                        MM3.STATE = MM3_STATE_DRDY;
+        MM3.STATE = MM3_STATE_DRDY;
+      }
-                return;
+      return;
+  }
+}
 /*********************************************/
 /*  Interrupt SPI transfer complete          */
 /*********************************************/
 ISR(SPI_STC_vect)
 {
-        static int8_t tmp;
+  static int8_t tmp;
-        int16_t value;
+  int16_t value;
-        switch (MM3.STATE)
+  switch (MM3.STATE)
-        {
+  {
-        // 1st byte received
+    // 1st byte received
-        case MM3_STATE_DRDY:
+  case MM3_STATE_DRDY:
-                tmp = SPDR;     // store 1st byte
+    tmp = SPDR;     // store 1st byte
-                SPDR = 0x00;    // trigger transfer of 2nd byte
+    SPDR = 0x00;        // trigger transfer of 2nd byte
-                MM3.STATE = MM3_STATE_BYTE2;
+    MM3.STATE = MM3_STATE_BYTE2;
-                return;
+    return;
-        case MM3_STATE_BYTE2:           // 2nd byte received
+  case MM3_STATE_BYTE2:         // 2nd byte received
-                value = (int16_t)tmp;   // combine the 1st and 2nd byte to a word
+    value = (int16_t)tmp;       // combine the 1st and 2nd byte to a word
-                value <<= 8;            // shift 1st byte to MSB-Position
+    value <<= 8;                // shift 1st byte to MSB-Position
-                value |= (int16_t)SPDR; // add 2nd byte
+    value |= (int16_t)SPDR;     // add 2nd byte
-                if(abs(value) < MAX_AXIS_VALUE)         // ignore spikes
+    if(abs(value) < MAX_AXIS_VALUE)             // ignore spikes
-                {
+    {
-                        switch (MM3.AXIS)
+      switch (MM3.AXIS)
-                        {
+      {
-                        case MM3_X_AXIS:
+      case MM3_X_AXIS:
-                                MM3.x_axis = value;
+        MM3.x_axis = value;
-                                MM3.AXIS = MM3_Y_AXIS;
+        MM3.AXIS = MM3_Y_AXIS;
-                                break;
+        break;
-                        case MM3_Y_AXIS:
+      case MM3_Y_AXIS:
-                                MM3.y_axis = value;
+        MM3.y_axis = value;
-                                MM3.AXIS = MM3_Z_AXIS;
+        MM3.AXIS = MM3_Z_AXIS;
-                                break;
+        break;
-                        case MM3_Z_AXIS:
+      case MM3_Z_AXIS:
-                                MM3.z_axis = value;
+        MM3.z_axis = value;
-                                MM3.AXIS = MM3_X_AXIS;
+        MM3.AXIS = MM3_X_AXIS;
-                                break;
+        break;
-                        default:
+      default:
-                                MM3.AXIS = MM3_X_AXIS;
+        MM3.AXIS = MM3_X_AXIS;
-                                break;
+        break;
-                        }
+      }
-                }
+    }
-                PORTC |= (1<<PORTC4); // deselect slave
+    PORTC |= (1<<PORTC4); // deselect slave
-                MM3.STATE = MM3_STATE_RESET;
+    MM3.STATE = MM3_STATE_RESET;
-                // Update timeout is called every 102.4 µs.
+    // Update timeout is called every 102.4 µs.
-                // It takes 2 cycles to write a measurement data request for one axis and
+    // 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.
+    // 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.
+    // I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
-                // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
+    // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
-                // decremtent the MM3_Timeout every 100 ms.
+    // decremtent the MM3_Timeout every 100 ms.
-                // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
+    // 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 ((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 all axis measurements give diffrent readings the data should be valid
-                        if(MM3_Timeout < 20) MM3_Timeout++;
+      if(MM3_Timeout < 20) MM3_Timeout++;
-                }
+    }
-                else // something is very strange here
+    else // something is very strange here
-                {
+    {
-                        if(MM3_Timeout ) MM3_Timeout--;
+      if(MM3_Timeout ) MM3_Timeout--;
+    }
-                return;
+    return;
-        default:
+  default:
-                return;
+    return;
+  }
 }
 /*********************************************/
 /*  Calibrate Compass                        */
 /*********************************************/
 void MM3_Calibrate(void)
 {
-        static uint8_t debugcounter = 0;
+  static uint8_t debugcounter = 0;
-        int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
+  int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
-        uint8_t measurement = 50, beeper = 0;
+  uint8_t measurement = 50, beeper = 0;
-        uint16_t timer;
+  uint16_t timer;
-        GRN_ON;
+  GRN_ON;
-        ROT_OFF;
+  ROT_OFF;
-        x_max = -16000;
+  x_max = -16000;
-        x_min =  16000;
+  x_min =  16000;
-        y_max = -16000;
+  y_max = -16000;
-        y_min =  16000;
+  y_min =  16000;
-        z_max = -16000;
+  z_max = -16000;
-        z_min =  16000;
+  z_min =  16000;
-        // get maximum and minimum reading of all axis
+  // get maximum and minimum reading of all axis
-        while (measurement)
+  while (measurement)
-        {
+  {
-                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
+    if (MM3.x_axis > x_max) x_max = MM3.x_axis;
-                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
+    else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
-                if (MM3.y_axis > y_max) y_max = MM3.y_axis;
+    if (MM3.y_axis > y_max) y_max = MM3.y_axis;
-                else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
+    else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
-                if (MM3.z_axis > z_max) z_max = MM3.z_axis;
+    if (MM3.z_axis > z_max) z_max = MM3.z_axis;
-                else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
+    else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
-                if (!beeper)
+    if (!beeper)
-                {
+    {
-                        ROT_FLASH;
+      ROT_FLASH;
-                        GRN_FLASH;
+      GRN_FLASH;
-                        beeper = 50;
+      beeper = 50;
-                }
+    }
-                beeper--;
+    beeper--;
-                // loop with period of 10 ms / 100 Hz
+    // loop with period of 10 ms / 100 Hz
-                timer = SetDelay(10);
+    timer = SetDelay(10);
-                while(!CheckDelay(timer));
+    while(!CheckDelay(timer));
-                if(debugcounter++ > 30)
+    if(debugcounter++ > 30)
-                {
+    {
-                        printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max);
+      printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max);
-                        debugcounter = 0;
+      debugcounter = 0;
-                }
+    }
-                // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
+    // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
-                if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
+    if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
-        }
+  }
-        // 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);
-        MM3_calib.Z_range = (z_max - z_min);
+  MM3_calib.Z_range = (z_max - z_min);
-        // Offset of all axis
+  // Offset of all axis
-        MM3_calib.X_off = (x_max + x_min) / 2;
+  MM3_calib.X_off = (x_max + x_min) / 2;
-        MM3_calib.Y_off = (y_max + y_min) / 2;
+  MM3_calib.Y_off = (y_max + y_min) / 2;
-        MM3_calib.Z_off = (z_max + z_min) / 2;
+  MM3_calib.Z_off = (z_max + z_min) / 2;
-        // save to EEProm
+  // save to EEProm
-        SetParamWord(PID_MM3_X_OFF,   (uint16_t)MM3_calib.X_off);
+  SetParamWord(PID_MM3_X_OFF,   (uint16_t)MM3_calib.X_off);
-        SetParamWord(PID_MM3_Y_OFF,   (uint16_t)MM3_calib.Y_off);
+  SetParamWord(PID_MM3_Y_OFF,   (uint16_t)MM3_calib.Y_off);
-        SetParamWord(PID_MM3_Z_OFF,   (uint16_t)MM3_calib.Z_off);
+  SetParamWord(PID_MM3_Z_OFF,   (uint16_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);
 }
 /*********************************************/
 /*  Calculate north direction (heading)      */
 /*********************************************/
 int16_t MM3_Heading(void)
 {
-        int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
+  int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
-        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
+  int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
-        int16_t angle;
+  int16_t angle;
-    int16_t heading;
+  int16_t heading;
-        if (MM3_Timeout)
+  if (MM3_Timeout)
+  {
-        // Offset correction and normalization (values of H are +/- 512)
+    // Offset correction and normalization (values of H are +/- 512)
-        Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
+    Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
-        Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
+    Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
-        Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
+    Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
-        // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
+    // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
     // assuming the MM3 board is mounted parallel to the frame.
     // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
     // in a top view counter clockwise direction.
     // North is in opposite direction of the small arrow on the MM3 board.
     // Therefore 180 deg must be added to that angle.
-        angle = ((int16_t)180);
+    angle = ((int16_t)180);
-        // wrap angle to interval of 0°- 359°
+    // wrap angle to interval of 0°- 359°
-        angle += 360;
+    angle += 360;
-        angle %= 360;
+    angle %= 360;
-        sin_yaw = (int32_t)(c_sin_8192(angle));
+    sin_yaw = (int32_t)(c_sin_8192(angle));
-        cos_yaw = (int32_t)(c_cos_8192(angle));
+    cos_yaw = (int32_t)(c_cos_8192(angle));
-        Hx_corr = Hx;
+    Hx_corr = Hx;
-        Hy_corr = Hy;
+    Hy_corr = Hy;
-        // rotate
+    // rotate
-        Hx = (Hx_corr * cos_yaw - Hy_corr  * sin_yaw) / 8192;
+    Hx = (Hx_corr * cos_yaw - Hy_corr  * sin_yaw) / 8192;
-        Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
+    Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
 #ifdef USE_Extended_MM3_Measurement_Model
-        /* Normalize the values to be in the same range as the accelerations  */
+    /* Normalize the values to be in the same range as the accelerations  */
-        MM3_Hx = Hx / 51.2F;
+    MM3_Hx = Hx / 51.2F;
-        MM3_Hy = Hy / 51.2F;
+    MM3_Hy = Hy / 51.2F;
-        MM3_Hz = Hz / 51.2F;
+    MM3_Hz = Hz / 51.2F;
 #endif
     // tilt compensation
-        // calibration factor for transforming Gyro Integrals to angular degrees
+    // calibration factor for transforming Gyro Integrals to angular degrees
-        // calculate sinus cosinus of pitch and tilt angle
+    // calculate sinus cosinus of pitch and tilt angle
-        //angle = (status. IntegralPitch/div_factor);
+    //angle = (status. IntegralPitch/div_factor);
-        angle = (status.iTheta10 / 10);
+    angle = (status.iTheta10 / 10);
-        angle = 0;
+    angle = 0;
-        sin_pitch = (int32_t)(c_sin_8192(angle));
+    sin_pitch = (int32_t)(c_sin_8192(angle));
-        cos_pitch = (int32_t)(c_cos_8192(angle));
+    cos_pitch = (int32_t)(c_cos_8192(angle));
+    //angle = (IntegralRoll/div_factor);
+    angle = (status.iPhi10/10);
+    angle = 0;
+    sin_roll = (int32_t)(c_sin_8192(angle));
+    cos_roll = (int32_t)(c_cos_8192(angle));
-        //angle = (IntegralRoll/div_factor);
+    Hx_corr = (Hx * cos_pitch -  Hz * sin_pitch) / 8192;
-        angle = (status.iPhi10/10);
+    Hy_corr = (Hy * cos_roll +  Hz * sin_roll) / 8192;
         angle = 0;
-        sin_roll = (int32_t)(c_sin_8192(angle));
+    // calculate Heading
-        cos_roll = (int32_t)(c_cos_8192(angle));
+    heading = c_atan2(Hy_corr, Hx_corr);
-        Hx_corr = (Hx * cos_pitch -  Hz * sin_pitch) / 8192;
+#if 0
-        Hy_corr = (Hy * cos_roll +  Hz * sin_roll) / 8192;
+    DebugOut.Analog[3] =  Hx;
+    DebugOut.Analog[4]  = Hy;
-        // calculate Heading
+    DebugOut.Analog[5]  = Hz;
-        heading = c_atan2(Hy_corr, Hx_corr);
+#endif
     // atan returns angular range from -180 deg to 180 deg in counter clockwise notation

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/mm3.c - Rev 838 → 966