Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2048 → Rev 2049

/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();