| 2,102 → 2,99 |
|---|
| #define _ANALOG_H |
| #include <inttypes.h> |
| #include "configuration.h" |
| #include "Vector3.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 |
| - 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] * |
| gyro sensitivity [V / deg/s] * |
| rotational speed [rad/s] * |
| gyro sensitivity [V/rad/s] * |
| amplifier gain [units] * |
| 1024 [units] / |
| 3V full range [V] |
| |
| GYRO_HW_FACTOR is: |
| gyro sensitivity [V / deg/s] * |
| gyro sensitivity [V/rad/s] * |
| amplifier gain [units] * |
| 1024 [units] / |
| 3V full range [V] |
| |
| Examples: |
| 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). |
| FC1.3 has 38.39 mV/rad/s gyros and amplifiers with a gain of 5.7: |
| 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: |
| GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
| FC2.0 has 6*(3/5) mV/rad/s gyros (they are ratiometric) and no amplifiers: |
| 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: |
| 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!) |
| 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. |
| */ |
| |
| #define ACCEL_HW_FACTOR 204.8f |
| |
| // 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_PITCHROLL 4 |
| #define GYRO_OVERSAMPLING_YAW 2 |
| #define GYRO_OVERSAMPLING_XY 8 |
| #define GYRO_RATE_FACTOR_XY (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_XY * GYRO_XY_CORRECTION) |
| #define GYRO_RATE_FACTOR_XY_MS (GYRO_RATE_FACTOR_XY * 1000.0) |
| |
| #define ACC_OVERSAMPLING_XY 2 |
| #define ACC_OVERSAMPLING_Z 1 |
| #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) |
| |
| /* |
| * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate. |
| */ |
| #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) |
| #define ACCEL_OVERSAMPLING_XY 4 |
| #define ACCEL_G_FACTOR_XY (ACCEL_HW_FACTOR * ACCEL_OVERSAMPLING_XY) |
| #define ACCEL_M_SSQUARED_FACTOR_XY (ACCEL_G_FACTOR_XY / 9.82f) |
| |
| /* |
| * 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 ACCEL_OVERSAMPLING_Z 2 |
| |
| // 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) |
| |
| #ifdef USE_MSSQUARED |
| #define ACCEL_FACTOR_XY ACCEL_M_SSQUARED_FACTOR_XY |
| #define ACCEL_FACTOR_Z ACCEL_M_SSQUARED_FACTOR_Z |
| #else |
| #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 |
| #define SENSOR_MIN_XY (32 * GYRO_OVERSAMPLING_XY) |
| // Other end of the range (calculated) |
| #define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
| #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_PITCHROLL) |
| #define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_XY) |
| |
| /* |
| * This value is subtracted from the gyro noise measurement in each iteration, |
| * making it return towards zero. |
| */ |
| #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
| #define X 0 |
| #define Y 1 |
| #define Z 2 |
| |
| #define PITCH 0 |
| #define ROLL 1 |
| #define YAW 2 |
| #define Z 2 |
| // ADC channels |
| #define AD_GYRO_Z 0 |
| #define AD_GYRO_X 1 |
| #define AD_GYRO_Y 2 |
| #define AD_AIRPRESSURE 3 |
| #define AD_UBAT 4 |
| #define AD_ACCEL_Z 5 |
| #define AD_ACCEL_Y 6 |
| #define AD_ACCEL_X 7 |
| |
| /* |
| * The values that this module outputs |
| * These first 2 exported arrays are zero-offset. The "PID" ones are used |
| 107,11 → 104,18 |
|---|
| * in filtering, and when a gyro becomes near saturated. |
| * Maybe this distinction is not really necessary. |
| */ |
| extern int16_t gyro_PID[2]; |
| extern int16_t gyro_ATT[2]; |
| extern Vector3f gyro_attitude; |
| |
| /* |
| * The acceleration values that this module outputs. They are zero based. |
| */ |
| extern Vector3f accel; |
| |
| #define GYRO_D_WINDOW_LENGTH 8 |
| |
| extern int16_t gyro_control[3]; |
| extern int16_t gyroD[2]; |
| extern int16_t yawGyro; |
| |
| extern int16_t UBat; |
| |
| // 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
| 118,28 → 122,22 |
|---|
| #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 accelOffset; |
| 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]; |
| typedef enum { |
| CONTROL_SENSOR_SAMPLING_DATA, |
| CONTROL_SENSOR_DATA_READY, |
| } ControlSensorDataStatus; |
| |
| /* |
| * The acceleration values that this module outputs. They are zero based. |
| */ |
| extern int16_t acc[3]; |
| extern int16_t filteredAcc[3]; |
| // extern volatile int32_t stronglyFilteredAcc[3]; |
| typedef enum { |
| ATTITUDE_SENSOR_NO_DATA, |
| ATTITUDE_SENSOR_DATA_READY, |
| ATTITUDE_SENSOR_READING_DATA |
| } AttitudeSensorDataStatus; |
| |
| /* |
| * 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 uint16_t gyroNoisePeak[3]; |
| extern uint16_t accNoisePeak[3]; |
| extern volatile uint8_t analog_controlDataStatus; |
| extern volatile uint8_t analog_attitudeDataStatus; |
| |
| /* |
| * Air pressure. |
| 171,7 → 169,7 |
|---|
| 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) |
| 186,22 → 184,6 |
|---|
| |
| #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; |
| |
| |
| void analog_init(void); |
| |
| /* |
| 208,17 → 190,19 |
|---|
| * 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. |
| */ |
| 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_update(void); |
| void analog_updateControlData(void); |
| void analog_sumAttitudeData(void); |
| void analog_updateAttitudeData(void); |
| |
| /* |
| * Read gyro and acc.meter calibration from EEPROM. |
| 237,7 → 221,6 |
|---|
| |
| |
| void analog_setGround(void); |
| |
| int32_t analog_getHeight(void); |
| int16_t analog_getDHeight(void); |
| |