0,0 → 1,211 |
#ifndef _ANALOG_H |
#define _ANALOG_H |
#include <inttypes.h> |
|
//#include "invenSense.h" |
#include "ENC-03_FC1.3.h" |
|
|
/* |
* How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro. |
* 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 |
|
/* |
* How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro. |
* 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 |
|
// Temporarily replaced by userparam-configurable variable. |
//#define ACC_FILTER 4 |
|
/* |
About setting constants right 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. |
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. |
Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
|
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 |
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: |
|
HiResXXXX = 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. |
|
where I is 8 for FC1.0 and 16 for all others. |
|
The relation between rotation rate and ADCValue: |
ADCValue [units] = |
rotational speed [deg/s] * |
gyro sensitivity [V / deg/s] * |
amplifier gain [units] * |
1024 [units] / |
3V 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 sensitivity [V / deg/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: |
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: |
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) |
(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)]. |
*/ |
|
/* |
* A factor that the raw gyro values are multiplied by, |
* before being zero-offset, 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. |
*/ |
#define GYRO_FACTOR_PITCHROLL 4 |
|
/* |
* How many samples are summed 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 |
|
/* |
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 |
180 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. |
Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this |
explanation), we get: |
|
gyroIntegralXXXX = |
N * HiResXXXX / 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. |
|
This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
|
Examples: |
FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304 |
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. |
* 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) |
|
/* |
* This value is subtracted from the gyro noise measurement in each iteration, |
* making it return towards zero. |
*/ |
#define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
|
/* |
* The values that this module outputs |
*/ |
extern volatile int16_t hiResPitchGyro, hiResRollGyro; |
extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro; |
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 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. |
*/ |
extern volatile uint8_t analogDataReady; |
|
// 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; |
|
// void SearchAirPressureOffset(void); |
|
void analog_init(void); |
|
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
|
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
|
/* |
* "Warm" calibration: Zero-offset gyros. |
*/ |
void analog_calibrate(void); |
|
/* |
* "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM. |
*/ |
void analog_calibrateAcc(void); |
#endif //_ANALOG_H |