Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2085 → Rev 2086

/branches/dongfang_FC_rewrite/analog.c
42,10 → 42,13
int16_t gyro_PID[2];
int16_t gyro_ATT[2];
int16_t gyroD[2];
int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];
uint8_t gyroDWindowIdx = 0;
int16_t yawGyro;
int16_t magneticHeading;
 
int32_t groundPressure;
int16_t dHeight;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
360,7 → 363,11
measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
 
// 5) Differential measurement.
gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
// gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
int16_t diff = tempOffsetGyro[axis] - gyro_PID[axis];
gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];
gyroD[axis] += diff;
gyroDWindow[axis][gyroDWindowIdx] = diff;
 
// 6) Done.
gyro_PID[axis] = tempOffsetGyro[axis];
368,7 → 375,11
// Prepare tempOffsetGyro for next calculation below...
tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
}
 
if (gyroDWindowIdx >= GYRO_D_WINDOW_LENGTH) {
gyroDWindowIdx = 0;
}
 
/*
* Now process the data for attitude angles.
*/
492,9 → 503,14
filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
}
 
dAirPressureWindow[dWindowPtr++] = (int16_t)(filteredAirPressure - lastFilteredAirPressure);
// positive diff of pressure
int16_t diff = filteredAirPressure - lastFilteredAirPressure;
// is a negative diff of height.
dHeight -= diff;
// remove old sample (fifo) from window.
dHeight += dAirPressureWindow[dWindowPtr];
dAirPressureWindow[dWindowPtr++] = diff;
if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0;
 
pressureMeasurementCount = airPressureSum = 0;
}
}
532,9 → 548,14
}
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
for (uint8_t i=PITCH; i<=ROLL; i++) {
gyroNoisePeak[i] = 0;
accNoisePeak[i] = 0;
gyroD[i] = 0;
for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) {
gyroDWindow[i][j] = 0;
}
}
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
delay_ms_with_adc_measurement(100, 0);
623,12 → 644,5
}
 
int16_t analog_getDHeight(void) {
int16_t result = 0;
for (int i=0; i<staticParams.airpressureDWindowLength; i++) {
result -= dAirPressureWindow[i]; // minus pressure is plus height.
}
 
debugOut.analog[30] = result;
// dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
return result;
return dHeight;
}
/branches/dongfang_FC_rewrite/analog.h
109,6 → 109,7
*/
extern int16_t gyro_PID[2];
extern int16_t gyro_ATT[2];
#define GYRO_D_WINDOW_LENGTH 3
extern int16_t gyroD[2];
extern int16_t yawGyro;
extern volatile uint16_t ADCycleCount;
/branches/dongfang_FC_rewrite/attitude.c
401,8 → 401,6
heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
//targetHeading = heading;
headingError = 0;
 
debugOut.digital[0] ^= DEBUG_COMPASS;
}
 
void correctHeadingToMagnetic(void) {
452,6 → 450,15
int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
//debugOut.analog[30] = correction;
 
debugOut.digital[0] = &= ~DEBUG_COMPASS;
debugOut.digital[1] = &= ~DEBUG_COMPASS;
 
if (correction > 0) {
debugOut.digital[0] ^= DEBUG_COMPASS;
} else if (correction < 0) {
debugOut.digital[1] ^= DEBUG_COMPASS;
}
 
// The correction is added both to current heading (the direction in which the copter thinks it is pointing)
// and to the heading error (the angle of yaw that the copter is off the set heading).
heading += correction;
/branches/dongfang_FC_rewrite/configuration.h
213,6 → 213,9
uint8_t naviI;
uint8_t naviD;
 
uint8_t naviTestOscPeriod;
uint8_t naviTestOscAmplitude;
 
// User params
uint8_t userParams[8];
 
/branches/dongfang_FC_rewrite/directGPSNaviControl.c
164,8 → 164,7
if (pTargetPos != NULL) { // if there is a target position
if (pTargetPos->status != INVALID) { // and the position data are valid
// if the target data are updated or the target pointer has changed
if ((pTargetPos->status != PROCESSED)
|| (pTargetPos != pLastTargetPos)) {
if ((pTargetPos->status != PROCESSED) || (pTargetPos != pLastTargetPos)) {
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
268,6 → 267,27
static uint8_t GPS_P_Delay = 0;
static uint16_t beep_rythm = 0;
 
static uint16_t navi_testOscPrescaler = 0;
static uint8_t navi_testOscTimer = 0;
 
navi_testOscPrescaler++;
if (navi_testOscPrescaler == 488) {
navi_testOscPrescaler = 0;
navi_testOscTimer++;
if (navi_testOscTimer == staticParams.naviTestOscPeriod) {
navi_testOscTimer = 0;
if (staticParams.naviTestOscAmplitude) {
holdPosition->status = NEWDATA;
holdPosition->latitude += staticParams.naviTestOscAmplitude * 90L;
}
} else if (navitestOscTimer == staticParams.naviTestOscPeriod/2) {
if (staticParams.naviTestOscAmplitude) {
holdPosition->status = NEWDATA;
holdPosition->latitude -= staticParams.naviTestOscAmplitude * 90L;
}
}
}
 
navi_updateFlightMode();
 
// store home position if start of flight flag is set
/branches/dongfang_FC_rewrite/flight.c
204,6 → 204,12
IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
}
 
// So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
// attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
// We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
// That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
// So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
 
// With normal Ki, limit I parts to +/- 205 (of about 1024)
if (IPart[axis] < -64000) {
IPart[axis] = -64000;
/branches/dongfang_FC_rewrite/heightControl.c
17,8 → 17,8
int32_t setHeight;
int32_t targetHeight;
 
uint16_t testOscPrescaler = 0;
uint8_t testOscTimer = 0;
uint16_t hc_testOscPrescaler = 0;
uint8_t hc_testOscTimer = 0;
 
int32_t maxHeightThisFlight;
int32_t iHeight;
27,7 → 27,7
analog_setGround();
// This should also happen when height control is enabled in-flight.
setHeight = targetHeight = analog_getHeight();
testOscTimer = 0;
hc_testOscTimer = 0;
maxHeightThisFlight = 0;
iHeight = 0;
}
52,7 → 52,7
if (setHeightLatch == LATCH_TIME) {
// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
setHeight = height;
testOscTimer = 0;
hc_testOscTimer = 0;
iHeight = 0;
}
// Time not yet reached.
90,21 → 90,21
// uint8_t heightControlTestOscPeriod;
// uint8_t heightControlTestOscAmplitude;
 
testOscPrescaler++;
if (testOscPrescaler == 488) {
testOscPrescaler = 0;
testOscTimer++;
if (testOscTimer == staticParams.heightControlTestOscPeriod) {
testOscTimer = 0;
hc_testOscPrescaler++;
if (hc_testOscPrescaler == 488) {
hc_testOscPrescaler = 0;
hc_testOscTimer++;
if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {
hc_testOscTimer = 0;
if (staticParams.heightControlTestOscAmplitude)
iHeight = 0;
} else if (testOscTimer == staticParams.heightControlTestOscPeriod/2) {
} else if (hc_testOscTimer == staticParams.heightControlTestOscPeriod/2) {
if (staticParams.heightControlTestOscAmplitude)
iHeight = 0;
}
}
 
if (testOscTimer < staticParams.heightControlTestOscPeriod/2)
if (hc_testOscTimer < staticParams.heightControlTestOscPeriod/2)
targetHeight = setHeight;
else
targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * 25L;