/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
80,4 → 80,5 |
staticParams.driftCompDivider = 1; |
staticParams.driftCompLimit = 200; |
staticParams.zerothOrderCorrection = 25; |
staticParams.imuReversedFlags = IMU_REVERSE_ACC_XY; |
} |
/branches/dongfang_FC_rewrite/analog.c |
---|
173,6 → 173,11 |
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
int32_t filteredAirPressure; |
#define MAX_AIRPRESSURE_WINDOW_LENGTH 64 |
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH]; |
int32_t windowedAirPressure; |
uint8_t windowPtr; |
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
int32_t airPressureSum; |
264,6 → 269,12 |
//Trigger Source to Free Running Mode |
ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0)); |
for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) { |
airPressureWindow[i] = 0; |
} |
windowedAirPressure = 0; |
startAnalogConversionCycle(); |
// restore global interrupt flags |
296,7 → 307,9 |
* Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type. |
*/ |
uint16_t getSimplePressure(int advalue) { |
return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10; |
return result; |
} |
void startAnalogConversionCycle(void) { |
434,8 → 447,6 |
if (pressureAutorangingWait) { |
//A range switch was done recently. Wait for steadying. |
pressureAutorangingWait--; |
debugOut.analog[27] = (uint16_t) OCR0A; |
debugOut.analog[31] = simpleAirPressure; |
} else { |
rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
if (rawAirPressure < MIN_RAWPRESSURE) { |
467,8 → 478,6 |
// Even if the sample is off-range, use it. |
simpleAirPressure = getSimplePressure(rawAirPressure); |
debugOut.analog[27] = (uint16_t) OCR0A; |
debugOut.analog[31] = simpleAirPressure; |
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
// Danger: pressure near lower end of range. If the measurement saturates, the |
486,7 → 495,7 |
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
} else { |
// normal case. |
// If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
// If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample. |
// The 2 cases above (end of range) are ignored for this. |
debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING - 1) |
502,6 → 511,13 |
+ airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
pressureMeasurementCount = airPressureSum = 0; |
} |
//int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH]; |
//int32_t windowedAirPressure = 0; |
//uint8_t windowPtr; |
windowedAirPressure += simpleAirPressure; |
windowedAirPressure -= airPressureWindow[windowPtr]; |
airPressureWindow[windowPtr] = simpleAirPressure; |
windowPtr = (windowPtr+1) % MAX_AIRPRESSURE_WINDOW_LENGTH; |
} |
} |
/branches/dongfang_FC_rewrite/attitude.c |
---|
396,7 → 396,6 |
temp = filteredAcc[2] >> 3; |
accVector += temp * temp; |
debugOut.analog[18] = accVector; |
debugOut.analog[19] = dynamicParams.maxAccVector; |
} |
/************************************************************************ |
/branches/dongfang_FC_rewrite/configuration.c |
---|
214,6 → 214,7 |
void setOtherDefaults(void) { |
// Height Control |
staticParams.airpressureAccZCorrection = 128+56; |
staticParams.heightP = 10; |
staticParams.heightD = 30; |
staticParams.heightSetting = 251; |
311,7 → 312,6 |
mixerMatrix.motor[i][MIX_ROLL] = 0; |
mixerMatrix.motor[i][MIX_YAW] = 0; |
} |
/* |
// default = Quadro |
mixerMatrix.motor[0][MIX_PITCH] = +64; |
mixerMatrix.motor[0][MIX_YAW] = +64; |
322,8 → 322,8 |
mixerMatrix.motor[3][MIX_ROLL] = +64; |
mixerMatrix.motor[3][MIX_YAW] = -64; |
memcpy(mixerMatrix.name, "Quadro\0", 7); |
*/ |
/* |
// default = X |
mixerMatrix.motor[0][MIX_PITCH] = +45; |
mixerMatrix.motor[0][MIX_ROLL] = +45; |
340,6 → 340,7 |
mixerMatrix.motor[3][MIX_PITCH] = -45; |
mixerMatrix.motor[3][MIX_ROLL] = +45; |
mixerMatrix.motor[3][MIX_YAW] = -64; |
*/ |
memcpy(mixerMatrix.name, "X\0", 7); |
} |
/branches/dongfang_FC_rewrite/configuration.h |
---|
121,7 → 121,8 |
uint8_t bitConfig; // see upper defines for bitcoding |
// Height Control |
uint8_t airpressureFilter; |
uint8_t airpressureFilter; |
uint8_t airpressureAccZCorrection; |
uint8_t heightP; |
uint8_t heightD; |
uint8_t heightSetting; |
/branches/dongfang_FC_rewrite/flight.c |
---|
55,10 → 55,6 |
#include "flight.h" |
#include "output.h" |
// Only for debug. Remove. |
//#include "analog.h" |
//#include "rc.h" |
// Necessary for external control and motor test |
#include "uart0.h" |
#include "twimaster.h" |
182,7 → 178,7 |
/************************************************************************/ |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
beepRCAlarm(); |
if (controlMixer_didReceiveSignal) beepRCAlarm(); |
if (emergencyFlightTime) { |
// continue emergency flight |
emergencyFlightTime--; |
220,9 → 216,7 |
*/ |
if (isFlying < 256) { |
IPart[PITCH] = IPart[ROLL] = 0; |
// TODO: Don't stomp on other modules' variables!!! |
// controlYaw = 0; |
PDPartYaw = 0; // instead. |
PDPartYaw = 0; |
if (isFlying == 250) { |
// HC_setGround(); |
updateCompassCourse = 1; |
285,13 → 279,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] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
/* |
* Now blend in the D-part - proportional to the Differential of the integral = the rate. |
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
* where pfactor is in [0..1]. |
*/ |
PDPart[axis] += (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)); |
PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
} |
334,7 → 323,6 |
debugOut.digital[0] |= DEBUG_CLIP; |
yawTerm = throttleTerm / 2; |
} |
//CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
} else { |
if (yawTerm < -MIN_YAWGAS / 2) { |
debugOut.digital[0] |= DEBUG_CLIP; |
343,10 → 331,8 |
debugOut.digital[0] |= DEBUG_CLIP; |
yawTerm = MIN_YAWGAS / 2; |
} |
//CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
// FIXME: Throttle may exceed maxThrottle (there is no check no more). |
tmp_int = staticParams.maxThrottle * CONTROL_SCALING; |
if (yawTerm < -(tmp_int - throttleTerm)) { |
yawTerm = -(tmp_int - throttleTerm); |
392,7 → 378,6 |
} else if (term[axis] > tmp_int) { |
debugOut.digital[1] |= DEBUG_CLIP; |
} |
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/branches/dongfang_FC_rewrite/heightControl.c |
---|
65,7 → 65,7 |
} |
} else { |
// Switch is not activated; take the "max-height" as the target height. |
targetHeight = (uint16_t) dynamicParams.heightSetting * 100; |
targetHeight = (uint16_t) dynamicParams.heightSetting * 5L - 500L; // should be: 100 (or make a param out of it) |
} |
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
77,7 → 77,7 |
} else { |
rampedTargetHeight = targetHeight; |
} |
} else if (rampedTargetHeight != targetHeight) { |
} else { |
// descending |
if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) { |
rampedTargetHeight -= staticParams.heightSlewRate; |
99,7 → 99,7 |
uint16_t HC_getThrottle(uint16_t throttle) { |
int32_t height = getHeight(); |
int32_t heightError = rampedTargetHeight - height; |
static int32_t lastHeight; |
int16_t dHeight = height - lastHeight; |
111,10 → 111,10 |
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
// iHeight += heightError; |
if (dHeight > 0) { |
if (heightError > 0) { |
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
} else if (dHeight < 0) { |
} else if (heightError < 0) { |
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
} |
126,15 → 126,18 |
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
*/ |
int16_t dThrottle = ((heightError * staticParams.heightP) << 10) |
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD; |
int16_t dThrottle = ((heightError * staticParams.heightP) >> 9) |
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */-((dHeight * staticParams.heightD) >> 7); |
// the "minGas" is now a limit for how much up / down the throttle can be varied |
if (dThrottle > staticParams.heightControlMaxThrottleChange) |
dThrottle = staticParams.heightControlMaxThrottleChange; |
else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
dThrottle = -staticParams.heightControlMaxThrottleChange; |
debugOut.analog[19] = rampedTargetHeight; |
debugOut.analog[21] = dThrottle; |
debugOut.analog[26] = height; |
if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
|| (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) { |
/branches/dongfang_FC_rewrite/invenSense.c |
---|
39,5 → 39,5 |
staticParams.driftCompDivider = 2; |
staticParams.driftCompLimit = 5; |
staticParams.zerothOrderCorrection = 1; |
staticParams.imuReversedFlags = 8; |
staticParams.imuReversedFlags = IMU_REVERSE_ACC_Z; |
} |
/branches/dongfang_FC_rewrite/makefile |
---|
23,18 → 23,18 |
#RC = DSL |
#RC = SPECTRUM |
GYRO=ENC-03_FC1.3 |
GYRO_HW_NAME=ENC |
GYRO_HW_FACTOR=1.304f |
#GYRO=ENC-03_FC1.3 |
#GYRO_HW_NAME=ENC |
#GYRO_HW_FACTOR=1.304f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
GYRO=ADXRS610_FC2.0 |
GYRO_HW_NAME=ADXR |
GYRO_HW_FACTOR=1.2288f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
#GYRO=ADXRS610_FC2.0 |
#GYRO_HW_NAME=ADXR |
#GYRO_HW_FACTOR=1.2288f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
#GYRO=invenSense |
#GYRO_HW_NAME=Isense |
#GYRO_HW_FACTOR=0.6827f |
/branches/dongfang_FC_rewrite/rc.c |
---|
61,8 → 61,8 |
// The channel array is now 0-based. |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile int16_t PPM_diff[MAX_CHANNELS]; |
volatile uint8_t NewPpmData = 1; |
volatile uint8_t RCQuality = 0; |
volatile uint8_t newPPMData = 1; |
volatile uint8_t RCQuality; |
int16_t RC_PRTY[4]; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t commandTimer = 0; |
160,7 → 160,7 |
// if a sync gap happens and there where at least 4 channels decoded before |
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
if (index >= 3) { |
NewPpmData = 0; // Null means NewData for the first 4 channels |
newPPMData = 0; // Null means NewData for the first 4 channels |
} |
// synchronize channel index |
index = 0; |
243,7 → 243,7 |
int16_t tmp1, tmp2; |
if (RCQuality) { |
RCQuality--; |
if (NewPpmData-- == 0) { |
if (newPPMData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
+ RCDiff(CH_PITCH) * staticParams.stickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
274,8 → 274,6 |
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
= RC_PRTY[CONTROL_YAW] = 0; |
} |
debugOut.analog[21] = RCQuality; |
} |
/* |
/branches/dongfang_FC_rewrite/timer0.c |
---|
170,8 → 170,7 |
beepModulation = BEEP_MODULATION_NONE; |
} |
// Beeper remains silent till the first time a control signal was received. |
if (beeper_On & controlMixer_didReceiveSignal) { |
if (beeper_On) { |
// set speaker port to high. |
if (boardRelease == 10) |
PORTD |= (1 << PORTD2); // Speaker at PD2 |
/branches/dongfang_FC_rewrite/uart0.c |
---|
140,14 → 140,14 |
"gyroP ", |
"ControlAct/10 ", |
"Acc. Vector ", |
"Max. Acc. Vector", |
"targetheight ", |
"var0 ", //20 |
"rc signal ", |
"dHeightThrottle ", |
"M1 ", |
"M2 ", |
"M3 ", |
"M4 ", //25 |
"ControlYaw ", |
"Height ", |
"Airpress. Range ", |
"DriftCompPitch ", |
"DriftCompRoll ", |