WebSVN - FlightCtrl - Diff - Rev 2023 and 2096 - /branches/dongfang_FC

 #ifndef _ANALOG_H
 #define _ANALOG_H
 #include <inttypes.h>
-//#include "invenSense.h"
+#include "configuration.h"
-//#include "ENC-03_FC1.3.h"
-//#include "ADXRS610_FC2.0.h"
 /*
  About setting constants for different gyros:
  Main parameters are positive directions and voltage/angular speed gain.
  A gyro is considered, in this code, to be "forward" if its positive
  direction is:
  - Nose down for pitch
  - 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 ones, and the results added. The pitch and roll gyros
+ sampled more than once (oversampled), and the results added.
- are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
- code.
  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.
- So:
- gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
+ roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
- where V is 2 for FC1.0 and 4 for all others.
+*/
- Assuming constant ADCValue, in the H&I code:
+#define GYRO_FACTOR_PITCHROLL 1
- gyro = I * ADCValue.
- where I is 8 for FC1.0 and 16 for all others.
+/*
- The relation between rotation rate and ADCValue:
+ GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
  ADCValue [units] =
  rotational speed [deg/s] *
  gyro sensitivity [V / deg/s] *
  amplifier gain [units] *
 [units] /
-V full range [V]
+V full range [V]
- or: H is the number of steps the ADC value changes with,
  for a 1 deg/s change in rotational velocity:
- H = ADCValue [units] / rotation rate [deg/s] =
+ GYRO_HW_FACTOR is:
  gyro sensitivity [V / deg/s] *
  amplifier gain [units] *
 [units] /
 V full range [V]
+ Examples:
- Examples:
+ FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
- FC1.3 has 0.67 mV/deg/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).
- H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
+ FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
- FC2.0 has 6*(3/5) mV/deg/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).
  H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
  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)
+ 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!)
- All together: gyro = I * H * rotation rate [units / (deg/s)].
- */
-/*
- * A factor that the raw gyro values are multiplied by,
- * 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
- * noticeable). 4 is fine for the default filtering.
- * Experiment: Set it to 1.
- */
+ GYRO_HW_FACTOR is given in the makefile.
-#define GYRO_FACTOR_PITCHROLL 1
+*/
 /*
- * How many samples are summed in one ADC loop, for pitch&roll and yaw,
+ * 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_SUMMATION_FACTOR_PITCHROLL 4
-#define GYRO_SUMMATION_FACTOR_YAW 2
-#define ACC_SUMMATION_FACTOR_PITCHROLL 2
-#define ACC_SUMMATION_FACTOR_Z 1
-/*
+#define GYRO_OVERSAMPLING_PITCHROLL 4
- Integration:
- The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
- In the Killagreg rewrite of the H&I firmware, the factor 8 is called
- HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,
- and care has been taken that all other constants (gyro to degree factor, and
-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
- gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
- Assuming a constant rotation rate v and a zero initial gyroIntegral
- (for this explanation), we get:
- gyroIntegral =
- N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
- N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
- where N is the number of summations; N = t/2ms.
- For one degree of rotation: t*v = 1:
- gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
 #define GYRO_OVERSAMPLING_YAW 2
+#define ACC_OVERSAMPLING_XY 2
- This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
+#define ACC_OVERSAMPLING_Z 1
- Examples:
+/*
- FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304
+ * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
- 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
  */
 #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)
  * 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.
+/*
- */
+ * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
-#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
+ * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
-#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
+ */
 /*
  * Gyro saturation prevention.
  * 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.
  * Maybe this distinction is not really necessary.
  */
-extern volatile int16_t gyro_PID[2];
+extern int16_t gyro_PID[2];
-extern volatile int16_t gyro_ATT[2];
+extern int16_t gyro_ATT[2];
-extern volatile int16_t gyroD[3];
+#define GYRO_D_WINDOW_LENGTH 8
-extern volatile int16_t yawGyro;
+extern int16_t gyroD[3];
-extern volatile uint16_t ADCycleCount;
+extern int16_t yawGyro;
-extern volatile int16_t UBat;
+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 accOffset;
-#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
+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];
+//extern volatile int16_t rawGyroSum[3];
 /*
  * The acceleration values that this module outputs. They are zero based.
  */
-extern volatile int16_t acc[3];
+extern int16_t acc[3];
-extern volatile int16_t filteredAcc[2];
+extern int16_t filteredAcc[3];
 // extern volatile int32_t stronglyFilteredAcc[3];
 /*
  * Diagnostics: Gyro noise level because of motor vibrations. The variables
  * only really reflect the noise level when the copter stands still but with
  * its motors running.
  */
-extern volatile uint16_t gyroNoisePeak[2];
+extern uint16_t gyroNoisePeak[3];
-extern volatile uint16_t accNoisePeak[2];
+extern uint16_t accNoisePeak[3];
+/*
+ * Air pressure.
+ * The sensor has a sensitivity of 45 mV/kPa.
-/*
+ * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h
+ * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h
+ * 11.95 * 10^-3 * h = 101.3 - p[kPa]
+ * h = (101.3 - p[kPa])/0.01195
+ * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.
+ * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m
+Display pressures
+mV-->1084.7
+mV-->1602.4   517.7
+mV-->3107.8  1503.4
+mV-->1419.1
+mV-->208.1
+Diff.:   1211.0
+Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)
+mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
+mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m
+mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m
- * Air pressure.
+mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
- * The sensor has a sensitivity of 46 mV/kPa.
+mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
- * An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 11.95 * 10^-3 * h
+Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
- * That is: dV = 46 mV * 11.95 * 10^-3 dh = 0.5497 mV / m.
+This is OCR2 = 143.15 at 1.5V in --> simple pressure =
- * That is, with 2 * 1.024 / 3 steps per mV: 0.037 steps / m
+*/
  */
-#define AIRPRESSURE_SUMMATION_FACTOR 16
+#define AIRPRESSURE_OVERSAMPLING 14
 #define MAX_RANGES_EXTRAPOLATION 240
 #define PRESSURE_EXTRAPOLATION_COEFF 25L
+#define AUTORANGE_WAIT_FACTOR 1
-#define AUTORANGE_WAIT_FACTOR 1
+#define ABS_ALTITUDE_OFFSET 108205
-extern volatile uint16_t simpleAirPressure;
+extern uint16_t simpleAirPressure;
 /*
  * At saturation, the filteredAirPressure may actually be (simulated) negative.
+ */
+extern int32_t filteredAirPressure;
+extern int16_t magneticHeading;
  */
-extern volatile int32_t filteredAirPressure;
+extern uint32_t gyroActivity;
 /*
+ * Flag: Interrupt handler has done all A/D conversion and processing.
- * Flag: Interrupt handler has done all A/D conversion and processing.
+ */
+extern volatile uint8_t analogDataReady;
- */
+void analog_init(void);
+/*
+ * This is really only for use for the ENC-03 code module, which needs to get the raw value
-extern volatile uint8_t analogDataReady;
+ * for its calibration. The raw value should not be used for anything else.
+ */
+uint16_t rawGyroValue(uint8_t axis);
+/*
+ * Start the conversion cycle. It will stop automatically.
-void analog_init(void);
+ */
 void startAnalogConversionCycle(void);
-// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
-#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
+/*
+ * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
-// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
+ */
+void analog_update(void);
+/*
+ * Read gyro and acc.meter calibration from EEPROM.
+ */
-#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
+void analog_setNeutral(void);
 /*
+ * Zero-offset gyros and write the calibration data to EEPROM.
+ */
+void analog_calibrateGyros(void);
+/*
+ * Zero-offset accelerometers and write the calibration data to EEPROM.
+ */
- * "Warm" calibration: Zero-offset gyros.
+void analog_calibrateAcc(void);

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/analog.h @ 2115 - Rev 2023 → 2096