WebSVN - FlightCtrl - Diff - Rev 2097 and 2189 - /branches/dongfang_FC

 #ifndef _ANALOG_H
 #define _ANALOG_H
 #include <inttypes.h>
 #include "configuration.h"
-/*
- 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 outputs a voltage > the no-rotation voltage.
- A gyro is considered, in this code, to be "forward" if its positive
- direction is:
- - Nose down for pitch
+#include "Vector3.h"
- - Left hand side down for roll
- - Clockwise seen from above for yaw.
- Setting gyro gain correctly: All sensor measurements in analog.c take
- place in a cycle, each cycle comprising all sensors. Some sensors are
- sampled more than once (oversampled), and the results added.
- In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
- 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. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
-*/
-#define GYRO_FACTOR_PITCHROLL 1
 /*
  GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
  ADCValue [units] =
- rotational speed [deg/s] *
+ rotational speed [rad/s] *
- gyro sensitivity [V / deg/s] *
+ gyro sensitivity [V/rad/s] *
  amplifier gain [units] *
 [units] /
 V full range [V]
  GYRO_HW_FACTOR is:
- gyro sensitivity [V / deg/s] *
+ gyro sensitivity [V/rad/s] *
  amplifier gain [units] *
 [units] /
 V full range [V]
  Examples:
- FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
+ FC1.3 has 38.39 mV/rad/s gyros and amplifiers with a gain of 5.7:
- GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
+ GYRO_HW_FACTOR = 0.03839 V/deg/s*5.7*1024/3V = 74.688 units/rad/s.
- FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
+ FC2.0 has 6*(3/5) mV/rad/s gyros (they are ratiometric) and no amplifiers:
- GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
  GYRO_HW_FACTOR = 0.006 V/rad/s*1*1024*3V/5V/3V = 70.405 units/(rad/s).
  My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
+ My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
+ GYRO_HW_FACTOR = 0.002 V/rad/s*1*1024/3V = 39.1139 units/(rad/s)
+ (only about half as sensitive as V1.3. But it will take about twice the rotation rate!)
+ GYRO_HW_FACTOR is given in the makefile.
- GYRO_HW_FACTOR = 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!)
+#define ACCEL_HW_FACTOR 204.8f
- GYRO_HW_FACTOR is given in the makefile.
+// If defined, acceleration is scaled to m/s^2. Else it is scaled to g's.
-*/
+#define USE_MSSQUARED 1
+/*
+ * How many samples are added in one ADC loop, for pitch&roll and yaw,
+ * respectively. This is = the number of occurences of each channel in the
+ * channelsForStates array in analog.c.
+ */
-/*
+#define GYRO_OVERSAMPLING_XY 8
- * How many samples are added in one ADC loop, for pitch&roll and yaw,
+#define GYRO_RATE_FACTOR_XY (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_XY * GYRO_XY_CORRECTION)
- * respectively. This is = the number of occurences of each channel in the
+#define GYRO_RATE_FACTOR_XY_MS (GYRO_RATE_FACTOR_XY * 1000.0)
- * channelsForStates array in analog.c.
+#define GYRO_OVERSAMPLING_Z 4
+#define GYRO_RATE_FACTOR_Z (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_Z * GYRO_Z_CORRECTION)
- */
+#define GYRO_RATE_FACTOR_Z_MS (GYRO_RATE_FACTOR_Z * 1000.0)
-#define GYRO_OVERSAMPLING_PITCHROLL 4
-#define GYRO_OVERSAMPLING_YAW 2
+#define ACCEL_OVERSAMPLING_XY 4
+#define ACCEL_G_FACTOR_XY (ACCEL_HW_FACTOR * ACCEL_OVERSAMPLING_XY)
-#define ACC_OVERSAMPLING_XY 2
+#define ACCEL_M_SSQUARED_FACTOR_XY (ACCEL_G_FACTOR_XY / 9.82f)
-#define ACC_OVERSAMPLING_Z 1
+#define ACCEL_OVERSAMPLING_Z 2
-/*
- * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
+// number of counts per g
- */
+#define ACCEL_G_FACTOR_Z (ACCEL_HW_FACTOR * ACCEL_OVERSAMPLING_Z)
+// number of counts per m/s^2
+#define ACCEL_M_SSQUARED_FACTOR_Z (ACCEL_G_FACTOR_Z / 9.82f)
-#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
-#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
+#ifdef USE_MSSQUARED
 #define ACCEL_FACTOR_XY ACCEL_M_SSQUARED_FACTOR_XY
-/*
+#define ACCEL_FACTOR_Z ACCEL_M_SSQUARED_FACTOR_Z
- * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
+#else
- * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
+#define ACCEL_FACTOR_XY ACCEL_G_FACTOR_XY
- */
+#define ACCEL_FACTOR_Z ACCEL_G_FACTOR_Z
+#endif
 /*
  * 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_OVERSAMPLING_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)
+#define SENSOR_MIN_XY (32 * GYRO_OVERSAMPLING_XY)
-/*
+// Other end of the range (calculated)
- * This value is subtracted from the gyro noise measurement in each iteration,
+#define SENSOR_MAX_XY (GYRO_OVERSAMPLING_XY * 1023 - SENSOR_MIN_XY)
+// 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_XY)
+#define X 0
+#define Y 1
+#define Z 2
+// ADC channels
+#define AD_GYRO_Z          0
- * making it return towards zero.
+#define AD_GYRO_X          1
- */
+#define AD_GYRO_Y          2
-#define GYRO_NOISE_MEASUREMENT_DAMPING 5
+#define AD_AIRPRESSURE     3
 #define AD_UBAT            4
-#define PITCH 0
+#define AD_ACCEL_Z         5
-#define ROLL 1
+#define AD_ACCEL_Y         6
-#define YAW 2
+#define AD_ACCEL_X         7
 #define Z 2
 /*
  * 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
- * These first 2 exported arrays are zero-offset. The "PID" ones are used
+ * in filtering, and when a gyro becomes near saturated.
+ * Maybe this distinction is not really necessary.
- * in the attitude control as rotation rates. The "ATT" ones are for
+ */
+extern Vector3f gyro_attitude;
- * integration to angles. For the same axis, the PID and ATT variables
+/*
- * generally have about the same values. There are just some differences
+ * The acceleration values that this module outputs. They are zero based.
- * in filtering, and when a gyro becomes near saturated.
+ */
- * Maybe this distinction is not really necessary.
+extern Vector3f accel;
  */
-extern int16_t gyro_PID[2];
+#define GYRO_D_WINDOW_LENGTH 8
-extern int16_t gyro_ATT[2];
-#define GYRO_D_WINDOW_LENGTH 8
+extern int16_t gyro_control[3];
 extern int16_t gyroD[2];
+extern int16_t UBat;
-extern int16_t yawGyro;
-extern int16_t UBat;
+// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
+#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
+extern sensorOffset_t gyroOffset;
+extern sensorOffset_t accelOffset;
+extern sensorOffset_t gyroAmplifierOffset;
-// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
-#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
-extern sensorOffset_t gyroOffset;
-extern sensorOffset_t accOffset;
-extern sensorOffset_t gyroAmplifierOffset;
-/*
- * This is not really for external use - but the ENC-03 gyro modules needs it.
- */
-//extern volatile int16_t rawGyroSum[3];
-/*
- * The acceleration values that this module outputs. They are zero based.
+typedef enum {
- */
+    CONTROL_SENSOR_SAMPLING_DATA,
-extern int16_t acc[3];
+    CONTROL_SENSOR_DATA_READY,
-extern int16_t filteredAcc[3];
+} ControlSensorDataStatus;
 // extern volatile int32_t stronglyFilteredAcc[3];
 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
 Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
 This is OCR2 = 143.15 at 1.5V in --> simple pressure =
 */
-#define AIRPRESSURE_OVERSAMPLING 14
+#define AIRPRESSURE_OVERSAMPLING 28
 #define AIRPRESSURE_FILTER 8
 // Minimum A/D value before a range change is performed.
 #define MIN_RAWPRESSURE (200 * 2)
 // Maximum A/D value before a range change is performed.
 #define PRESSURE_EXTRAPOLATION_COEFF 25L
 #define AUTORANGE_WAIT_FACTOR 1
-#define ABS_ALTITUDE_OFFSET 108205
-extern uint16_t simpleAirPressure;
-/*
- * At saturation, the filteredAirPressure may actually be (simulated) negative.
- */
-extern int32_t filteredAirPressure;
-extern int16_t magneticHeading;
-extern uint32_t gyroActivity;
-/*
- * Flag: Interrupt handler has done all A/D conversion and processing.
- */
-extern volatile uint8_t analogDataReady;
 #define ABS_ALTITUDE_OFFSET 108205
 void analog_init(void);
 /*
  * This is really only for use for the ENC-03 code module, which needs to get the raw value
  * for its calibration. The raw value should not be used for anything else.
  */
-uint16_t rawGyroValue(uint8_t axis);
+uint16_t gyroValueForFC13DACCalibration(uint8_t axis);
+/*
-/*
+ * Start the conversion cycle. It will stop automatically.
- * Start the conversion cycle. It will stop automatically.
  */
-void startAnalogConversionCycle(void);
+void startADCCycle(void);
+void waitADCCycle(uint16_t delay);
 /*
-/*
+ */
- * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
+void analog_updateControlData(void);
- */
+void analog_sumAttitudeData(void);
  */
 void analog_calibrateAcc(void);
 void analog_setGround(void);
 void analog_setGround(void);
 int32_t analog_getHeight(void);

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.h - Rev 2097 → 2189