/branches/dongfang_FC_rewrite/analog.h |
---|
4,21 → 4,6 |
#include "configuration.h" |
/* |
* 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_PID_FILTER 1 |
/* |
* 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_ATT_FILTER 1 |
// #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 |
31,25 → 16,16 |
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. |
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. |
So: |
roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here. |
*/ |
#define GYRO_FACTOR_PITCHROLL 1 |
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. |
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] * |
57,9 → 33,7 |
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_HW_FACTOR is: |
gyro sensitivity [V / deg/s] * |
amplifier gain [units] * |
1024 [units] / |
67,30 → 41,21 |
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). |
GYRO_HW_FACTOR = 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). |
GYRO_HW_FACTOR = 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)]. |
*/ |
GYRO_HW_FACTOR is given in the makefile. |
*/ |
/* |
* 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. |
*/ |
#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. |
*/ |
101,44 → 66,15 |
#define ACC_OVERSAMPLING_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. |
The control loop executes at 488Hz, 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*488. |
For one degree of rotation: t*v = 1: |
gyroIntegralXXXX = t * 488 * 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=4, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=1 --> integralDegreeFactor = 2545 |
FC2.0: I=4, H=1.2288, HIRES_GYRO_INTEGRATION_FACTOR=1 --> integralDegreeFactor = 2399 |
My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1333 |
* 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) |
/* |
* 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_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW) |
/* |
* Gyro saturation prevention. |
/branches/dongfang_FC_rewrite/attitude.h |
---|
33,13 → 33,36 |
* Gyro readings are divided by this before being used in attitude control. This will scale them |
* to match the scale of the stick control etc. variables. This is just a rough non-precision |
* scaling - the below definitions make precise conversion factors. |
* Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610. |
* The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
*/ |
#define HIRES_GYRO_INTEGRATION_FACTOR 1 |
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
/* |
Gyro integration: |
The control loop executes at INTEGRATION_FREQUENCY Hz, 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 = |
t * INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR |
For one degree of rotation: t*v = 1: |
gyroIntegral = INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR |
This number (INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR) is the integral-to-degree factor. |
Examples: |
FC1.3: GYRO_DEG_FACTOR_PITCHROLL = 2545 |
FC2.0: GYRO_DEG_FACTOR_PITCHROLL = 2399 |
My InvenSense copter: GYRO_DEG_FACTOR_PITCHROLL = 1333 |
*/ |
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
/* |
* Constant for deriving an attitude angle from acceleration measurement. |
* |
* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
62,14 → 85,6 |
*/ |
#define DEG_ACC_FACTOR 7 |
/* |
* Growth of the integral per degree: |
* The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
* HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
* The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
*/ |
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
/* |
* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
121,9 → 121,35 |
int16_t tempPRTY[4]; |
void controlMixer_periodicTask(void) { |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
// TODO: If no signal --> zero. |
// HC_periodicTask(); |
// Decode commands. |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
: COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
: COMMAND_NONE; |
// Update variables ("potis"). |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
controlMixer_didReceiveSignal = 1; |
} else { // Signal is not OK |
// Could handle switch to emergency flight here. |
// throttle is handled elsewhere. |
} |
if (rcCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == rcCommand); |
lastCommand = rcCommand; |
lastArgument = RC_getArgument(); |
} else if (ecCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == ecCommand); |
lastCommand = ecCommand; |
lastArgument = EC_getArgument(); |
} else { |
// Both sources have no command, or one or both are out. |
// Just set to false. There is no reason to check if the none-command was repeated anyway. |
isCommandRepeated = 0; |
lastCommand = COMMAND_NONE; |
} |
// This will init the values (not just add to them). |
RC_periodicTaskAndPRTY(tempPRTY); |
151,15 → 177,6 |
debugOut.analog[17] = controlActivity/10; |
// Update variables ("potis"). |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
controlMixer_didReceiveSignal = 1; |
} else { // Signal is not OK |
// Could handle switch to emergency flight here. |
// throttle is handled elsewhere. |
} |
// We can safely do this even with a bad signal - the variables will not have been updated then. |
configuration_applyVariablesToParams(); |
184,25 → 201,4 |
maxControl[axis]--; |
} |
*/ |
// Decode commands. |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
: COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
: COMMAND_NONE; |
if (rcCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == rcCommand); |
lastCommand = rcCommand; |
lastArgument = RC_getArgument(); |
} else if (ecCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == ecCommand); |
lastCommand = ecCommand; |
lastArgument = EC_getArgument(); |
} else { |
// Both sources have no command, or one or both are out. |
// Just set to false. There is no reason to check if the none-command was repeated anyway. |
isCommandRepeated = 0; |
lastCommand = COMMAND_NONE; |
} |
} |
/branches/dongfang_FC_rewrite/controlMixer.h |
---|
76,7 → 76,7 |
//extern uint16_t maxControl[2]; |
void controlMixer_initVariables(void); |
void controlMixer_updateVariables(void); |
//void controlMixer_updateVariables(void); |
void controlMixer_setNeutral(void); |
/branches/dongfang_FC_rewrite/flight.c |
---|
263,8 → 263,8 |
/************************************************************************/ |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
for (axis = PITCH; axis <= ROLL; axis++) { |
PDPart[axis] = attitude[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)); |
PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
} |
277,8 → 277,8 |
CHECK_MIN_MAX(headingDiff, -50000, 50000); |
debugOut.analog[29] = headingDiff; |
PDPartYaw = (int32_t) (headingDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
PDPartYaw = (int32_t)(headingDiff * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
PDPartYaw += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_PITCHROLL >> 6); |
// limit control feedback |
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
/branches/dongfang_FC_rewrite/main.c |
---|
200,6 → 200,10 |
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
J4HIGH; |
// This is probably the correct order: |
// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
// Flight control uses results from both. |
calculateFlightAttitude(); |
controlMixer_periodicTask(); |
flight_control(); |