WebSVN - FlightCtrl - Diff - Rev 1634 and 1645 - /branches/dongfang_FC

 #ifndef _ANALOG_H
 #define _ANALOG_H
 #include <inttypes.h>
-//#include "invenSense.h"
+// #include "invenSense.h"
-#include "ENC-03_FC1.3.h"
 #include "ENC-03_FC1.3.h"
 /*
- * How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro.
+ * How much low pass filtering to apply for gyro_PID.
  * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
  * Temporarily replaced by userparam-configurable variable.
  */
-//#define GYROS_FIRSTORDERFILTER 2
+#define GYROS_PIDFILTER 1
 /*
- * How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro.
+ * How much low pass filtering to apply for gyro_ATT.
  * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
  * Temporarily replaced by userparam-configurable variable.
  */
-//#define GYROS_SECONDORDERFILTER 2
+#define GYROS_INTEGRALFILTER 1
 // Temporarily replaced by userparam-configurable variable.
 //#define ACC_FILTER 4
 /*
-  About setting constants right for different gyros:
+  About setting constants for different gyros:
   Main parameters are positive directions and voltage/angular speed gain.
   The "Positive direction" is the rotation direction around an axis where
-  the corresponding gyro gives a voltage > the no-rotation voltage.
+  the corresponding gyro outputs a voltage > the no-rotation voltage.
   A gyro is considered, in this code, to be "forward" if its positive
   or 4 (other versions), offset to zero, low pass filtered and then assigned
   to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
   roll.
   So:
-  HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
+  gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
     where V is 2 for FC1.0 and 4 for all others.
   Assuming constant ADCValue, in the H&I code:
-  HiResXXXX = I * ADCValue.
+  gyro = I * ADCValue.
   My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
     H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
     (only about half as sensitive as V1.3. But it will take about twice the
      rotation rate!)
-  All together: HiResXXXX = I * H * rotation rate [units / (deg/s)].
+  All together: gyro = I * H * rotation rate [units / (deg/s)].
 */
 /*
  * A factor that the raw gyro values are multiplied by,
- * before being zero-offset, filtered and passed to the attitude module.
+ * before being filtered and passed to the attitude module.
  * A value of 1 would cause a little bit of loss of precision in the
  * filtering (on the other hand the values are so noisy in flight that
+ * it will not really matter - but when testing on the desk it might be
- * it will not really matter - but when testing on the desk it might be
+ * noticeable). 4 is fine for the default filtering.
- * noticeable). 4 is fine for the default filtering.
+ * Experiment: Set it to 1.
  */
 #define GYRO_FACTOR_PITCHROLL 4
 degree flip-over detection limits) are corrected to it. Because the
   division by the constant takes place in the flight attitude code, the
   constant is there.
   The control loop executes every 2ms, and for each iteration
-  HiResXXXX is added to gyroIntegralXXXX.
+  gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
-  Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this
+  Assuming a constant rotation rate v and a zero initial gyroIntegral
-  explanation), we get:
+  (for this explanation), we get:
-  gyroIntegralXXXX =
+  gyroIntegral =
-    N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR =
+    N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
     N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
   FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260
   My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
 */
 /*
- * The value of hiResXXXX for one deg/s = The hardware factor H * the number of samples * multiplier factor.
+ * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
  * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
  */
 #define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
 #define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
+/*
+ * Gyro saturation prevention.
+ */
+// How far from the end of its range a gyro is considered near-saturated.
+#define SENSOR_MIN_PITCHROLL 32
+// Other end of the range (calculated)
+#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
+// Max. boost to add "virtually" to gyro signal at total saturation.
+#define EXTRAPOLATION_LIMIT 2500
+// Slope of the boost (calculated)
+#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
 /*
  * This value is subtracted from the gyro noise measurement in each iteration,
  * making it return towards zero.
+ */
+#define GYRO_NOISE_MEASUREMENT_DAMPING 5
  */
-#define GYRO_NOISE_MEASUREMENT_DAMPING 5
+#define PITCH 0
+#define ROLL 1
+/*
+ * The values that this module outputs
+ * These first 2 exported arrays are zero-offset. The "PID" ones are used
+ * in the attitude control as rotation rates. The "ATT" ones are for
+ * integration to angles. For the same axis, the PID and ATT variables
+ * generally have about the same values. There are just some differences
-/*
+ * in filtering, and when a gyro becomes near saturated.
- * The values that this module outputs
+ * Maybe this distinction is not really necessary.
  */
+extern volatile int16_t gyro_PID[2];
-extern volatile int16_t hiResPitchGyro, hiResRollGyro;
+extern volatile int16_t gyro_ATT[2];
-extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro;
+extern volatile int16_t gyroD[2];
 extern volatile int16_t pitchGyroD, rollGyroD;
 extern volatile uint16_t ADCycleCount;
 extern volatile int16_t UBat;
 extern volatile int16_t yawGyro;
 /*
  * This is not really for external use - but the ENC-03 gyro modules needs it.
  */
-extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum;
-/*
- * The acceleration values that this module outputs
- */
-extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc;
-extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc;
-// Only for debugging! Not to be exported! Remove when finished.
+extern volatile int16_t rawGyroSum[2], rawYawGyroSum;
-// extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset;
-// Air pressure measurement not supported right now.
-// extern volatile int32_t AirPressure;
-// extern volatile int16_t HeightD;
+/*
-// extern volatile uint16_t ReadingAirPressure;
+ * The acceleration values that this module outputs. They are zero based.
-// extern volatile int16_t StartAirPressure;
+ */
-// extern uint8_t PressureSensorOffset;
+extern volatile int16_t acc[2], ZAcc;
-// extern int8_t ExpandBaro;
+extern volatile int16_t filteredAcc[2];
 /*
  * Flag: Interrupt handler has done all A/D conversion and processing.
  */
 extern volatile uint8_t analogDataReady;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.h - Rev 1634 → 1645