| 2,34 → 2,33 |
|---|
| #define _ANALOG_H |
| #include <inttypes.h> |
| |
| //#include "invenSense.h" |
| // #include "invenSense.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 |
| direction is the same as in FC1.0/1.1/1.2/1.3, and reverse otherwise. |
| direction is the same as in FC1.0-1.3, and reverse otherwise. |
| Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
| GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
| |
| 44,12 → 43,12 |
|---|
| 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. |
| |
| where I is 8 for FC1.0 and 16 for all others. |
| |
| 79,16 → 78,17 |
|---|
| (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 |
| * noticeable). 4 is fine for the default filtering. |
| * Experiment: Set it to 1. |
| */ |
| #define GYRO_FACTOR_PITCHROLL 4 |
| |
| 111,12 → 111,12 |
|---|
| constant is there. |
| |
| The control loop executes every 2ms, and for each iteration |
| HiResXXXX is added to gyroIntegralXXXX. |
| Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this |
| explanation), we get: |
| 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: |
| |
| gyroIntegralXXXX = |
| N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR = |
| 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. |
| 134,7 → 134,7 |
|---|
| */ |
| |
| /* |
| * 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) |
| 141,17 → 141,38 |
|---|
| #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 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. |
| * Maybe this distinction is not really necessary. |
| */ |
| extern volatile int16_t hiResPitchGyro, hiResRollGyro; |
| extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro; |
| extern volatile int16_t pitchGyroD, rollGyroD; |
| extern volatile int16_t gyro_PID[2]; |
| extern volatile int16_t gyro_ATT[2]; |
| extern volatile int16_t gyroD[2]; |
| |
| extern volatile uint16_t ADCycleCount; |
| extern volatile int16_t UBat; |
| extern volatile int16_t yawGyro; |
| 159,25 → 180,14 |
|---|
| /* |
| * This is not really for external use - but the ENC-03 gyro modules needs it. |
| */ |
| extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum; |
| extern volatile int16_t rawGyroSum[2], rawYawGyroSum; |
| |
| /* |
| * The acceleration values that this module outputs |
| * The acceleration values that this module outputs. They are zero based. |
| */ |
| extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc; |
| extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc; |
| extern volatile int16_t acc[2], ZAcc; |
| extern volatile int16_t filteredAcc[2]; |
| |
| // Only for debugging! Not to be exported! Remove when finished. |
| // 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; |
| // extern volatile int16_t StartAirPressure; |
| // extern uint8_t PressureSensorOffset; |
| // extern int8_t ExpandBaro; |
| |
| /* |
| * Flag: Interrupt handler has done all A/D conversion and processing. |
| */ |
| 186,8 → 196,8 |
|---|
| // 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 pitchGyroNoisePeak, rollGyroNoisePeak; |
| extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak; |
| extern volatile uint16_t gyroNoisePeak[2]; |
| extern volatile uint16_t accNoisePeak[2]; |
| |
| // void SearchAirPressureOffset(void); |
| |