WebSVN - FlightCtrl - Diff - Rev 1927 and 2022 - /branches/dongfang_FC

-#include <avr/io.h>
 #include <avr/interrupt.h>
 #include <avr/pgmspace.h>
 #include "analog.h"
 volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512
                 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z };
-/*
- * This allows some experimentation with the gyro filters.
- * Should be replaced by #define's later on...
- */
-volatile uint8_t GYROS_PID_FILTER;
-volatile uint8_t GYROS_ATT_FILTER;
-volatile uint8_t GYROS_D_FILTER;
-volatile uint8_t ACC_FILTER;
 /*
  * Air pressure
  */
  */
 uint16_t getSimplePressure(int advalue) {
   return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
+}
+/*
+ * In the MK coordinate system, nose-down is positive and left-roll is positive.
+ * If a sensor is used in an orientation where one but not both of the axes has
+ * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
  */
 void transformPRGyro(int16_t* result) {
+  static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1};
-  static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1};
+  // Pitch to Pitch part
+  int8_t pp = PR_GYROS_ORIENTATION_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT];
-  int8_t pp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT];
+  // Pitch to Roll part
+  int8_t pr = tab[(GYRO_QUADRANT+2)%8];
-  int8_t pr = tab[(GYRO_QUADRANT+2)%8];
+  // Roll to Roll part
+  int8_t rp = PR_GYROS_ORIENTATION_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8];
-  int8_t rp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8];
+  // Roll to Roll part
   int8_t rr = tab[GYRO_QUADRANT];
+  int16_t pitchIn = result[PITCH];
-  int16_t temp = result[0];
-  result[0] = pp*result[0] + pr*result[1];
+  result[PITCH] = pp*result[PITCH] + pr*result[ROLL];
-  result[1] = rp*temp + rr*result[1];
+  result[ROLL] = rp*pitchIn + rr*result[ROLL];
+}
 /*****************************************************
  * Interrupt Service Routine for ADC
- * Runs at 312.5 kHz or 3.2 µs. When all states are
+ * Runs at 312.5 kHz or 3.2 us. When all states are
  * processed the interrupt is disabled and further
  * AD conversions are stopped.
                 break;
         case 11: // yaw gyro
                 rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
-                if (YAW_REVERSED)
+                if (YAW_GYRO_REVERSED)
                   tempOffsetGyro[0] = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
                 else
                   tempOffsetGyro[0] = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
-                gyroD[YAW] = (gyroD[YAW] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[0] - yawGyro)) / GYROS_D_FILTER;
+                gyroD[YAW] = (gyroD[YAW] * (staticParams.DGyroFilter - 1) + (tempOffsetGyro[0] - yawGyro)) / staticParams.DGyroFilter;
                 yawGyro = tempOffsetGyro[0];
-                break;
-    case 13: // roll axis acc.
-            /*
-            for (axis=0; axis<2; axis++) {
-                if (ACC_REVERSED[axis])
-                        tempSensor[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis];
-                else
-                        tempSensor[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis];
-            }
-            if (AXIS_TRANSFORM) {
-                acc[PITCH] = tempSensor[PITCH] + tempSensor[ROLL];
-                acc[ROLL] = tempSensor[ROLL] - tempSensor[PITCH];
-            } else {
-                acc[PITCH] = tempSensor[PITCH];
-                acc[ROLL] = tempSensor[ROLL];
                 break;
-            */
+    case 13: // roll axis acc.
             // We have no sensor installed...
             acc[PITCH] = acc[ROLL] = 0;
             for (axis=0; axis<2; axis++) {
         filteredAcc[axis] =
-                    (filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER;
+                    (filteredAcc[axis] * (staticParams.accFilter - 1) + acc[axis]) / staticParams.accFilter;
         // 2.1: Transform axis if configured to.
             transformPRGyro(tempOffsetGyro);
                 // 3) Scale and filter.
             for (axis=0; axis<2; axis++) {
-                tempOffsetGyro[axis] = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro[axis]) / GYROS_PID_FILTER;
+                tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.PIDGyroFilter - 1) + tempOffsetGyro[axis]) / staticParams.PIDGyroFilter;
                 // 4) Measure noise.
                 measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
                 // 5) Differential measurement.
-                gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / GYROS_D_FILTER;
+                gyroD[axis] = (gyroD[axis] * (staticParams.DGyroFilter - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.DGyroFilter;
+            }
             transformPRGyro(tempOffsetGyro);
             // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
-            gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[PITCH]) / GYROS_ATT_FILTER;
+            gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter;
-            gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[ROLL]) / GYROS_ATT_FILTER;
+            gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter;
             break;
 void analog_calibrate(void) {
 #define GYRO_OFFSET_CYCLES 32
         uint8_t i, axis;
         int32_t deltaOffsets[3] = { 0, 0, 0 };
-        // Set the filters... to be removed again, once some good settings are found.
-        GYROS_PID_FILTER = (staticParams.sensorFilterSettings & (0x7 & (1<<0))) + 1;
-        GYROS_ATT_FILTER = 1;
-        GYROS_D_FILTER =   (staticParams.sensorFilterSettings & (0x3 & (1<<4))) + 1;
-        ACC_FILTER =       (staticParams.sensorFilterSettings & (0x3 & (1<<6))) + 1;
         gyro_calibrate();
         // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/analog.c - Rev 1927 → 2022