Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1820 → Rev 1821

/branches/dongfang_FC_rewrite/analog.h
24,66 → 24,66
// #define ACC_FILTER 4
 
/*
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.
Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
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.
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:
 
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:
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:
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:
gyro = I * ADCValue.
gyro = I * ADCValue.
 
where I is 8 for FC1.0 and 16 for all others.
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]
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]
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!)
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: gyro = 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,
108,38 → 108,38
#define ACC_SUMMATION_FACTOR_Z 1
 
/*
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.
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
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.
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:
 
For one degree of rotation: t*v = 1:
gyroIntegral =
N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
 
gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
where N is the number of summations; N = t/2ms.
 
This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
For one degree of rotation: t*v = 1:
 
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
*/
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 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.
239,7 → 239,6
*/
extern volatile uint8_t analogDataReady;
 
 
void analog_init(void);
 
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit