/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]; |
369,6 → 376,10 |
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. |
return dHeight; |
} |
debugOut.analog[30] = result; |
// dHeight = -dPressure, so here it is the old pressure minus the current, not opposite. |
return result; |
} |
/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; |