/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
37,9 → 37,9 |
for (axis = PITCH; axis <= YAW; axis++) { |
if (axis == YAW) |
factor = GYRO_SUMMATION_FACTOR_YAW; |
factor = GYRO_OVERSAMPLING_YAW; |
else |
factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
factor = GYRO_OVERSAMPLING_PITCHROLL; |
if (rawGyroValue(axis) < 510 * factor) |
gyroAmplifierOffset.offsets[axis]--; |
66,7 → 66,7 |
void gyro_init() { |
if (gyroAmplifierOffset_readFromEEProm()) { |
printf("gyro amp invalid%s", recal); |
printf("gyro amp invalid, recalibrate."); |
gyroAmplifierOffset.offsets[PITCH] = |
gyroAmplifierOffset.offsets[ROLL] = |
gyroAmplifierOffset.offsets[YAW] = (uint8_t)(255 * 1.2089 / 3.0); |
/branches/dongfang_FC_rewrite/analog.c |
---|
134,9 → 134,8 |
* 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
*/ |
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
//void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
/* |
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
// Pitch to Pitch part |
149,7 → 148,7 |
int8_t yy = rotationTab[quadrant]; |
int16_t xIn = result[0]; |
result[0] = xx*result[0] + xy*result[1]; |
result[0] = xx*xIn + xy*result[1]; |
result[1] = yx*xIn + yy*result[1]; |
if (quadrant & 1) { |
161,7 → 160,7 |
result[1] = (result[1]*11) >> 4; |
} |
} |
*/ |
/* |
* Air pressure |
*/ |
173,7 → 172,7 |
// Value of 2 samples, with range. |
uint16_t simpleAirPressure; |
// Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
int32_t filteredAirPressure; |
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
492,7 → 491,7 |
// If AIRPRESSURE_SUMMATION_FACTOR 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_SUMMATION_FACTOR - 1) |
if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING - 1) |
airPressureSum += simpleAirPressure / 2; |
else |
airPressureSum += simpleAirPressure; |
500,7 → 499,7 |
// 2 samples were added. |
pressureMeasurementCount += 2; |
if (pressureMeasurementCount >= AIRPRESSURE_SUMMATION_FACTOR) { |
if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) { |
filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
+ airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
pressureMeasurementCount = airPressureSum = 0; |
513,7 → 512,6 |
// This is divided by 3 --> 10.34 counts per volt. |
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
debugOut.analog[11] = UBat; |
debugOut.analog[21] = sensorInputs[AD_UBAT]; |
} |
void analog_update(void) { |
528,14 → 526,14 |
if (gyroOffset_readFromEEProm()) { |
printf("gyro offsets invalid%s",recal); |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL; |
gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW; |
} |
if (accOffset_readFromEEProm()) { |
printf("acc. meter offsets invalid%s",recal); |
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL; |
accOffset.offsets[Z] = 717 * ACC_SUMMATION_FACTOR_Z; |
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
} |
// Noise is relative to offset. So, reset noise measurements when changing offsets. |
547,7 → 545,7 |
delay_ms_with_adc_measurement(100, 0); |
// Rough estimate. Hmm no nothing happens at calibration anyway. |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2); |
// pressureMeasurementCount = 0; |
} |
568,8 → 566,8 |
for (axis = PITCH; axis <= YAW; axis++) { |
gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
int16_t min = (512-200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
int16_t max = (512+200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
} |
603,16 → 601,16 |
if (axis==Z) { |
if (staticParams.imuReversedFlags & 8) { |
// TODO: This assumes a sensitivity of +/- 2g. |
min = (256-200) * ACC_SUMMATION_FACTOR_Z; |
max = (256+200) * ACC_SUMMATION_FACTOR_Z; |
min = (256-200) * ACC_OVERSAMPLING_Z; |
max = (256+200) * ACC_OVERSAMPLING_Z; |
} else { |
// TODO: This assumes a sensitivity of +/- 2g. |
min = (768-200) * ACC_SUMMATION_FACTOR_Z; |
max = (768+200) * ACC_SUMMATION_FACTOR_Z; |
min = (768-200) * ACC_OVERSAMPLING_Z; |
max = (768+200) * ACC_OVERSAMPLING_Z; |
} |
} else { |
min = (512-200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
max = (512+200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
min = (512-200) * ACC_OVERSAMPLING_XY; |
max = (512+200) * ACC_OVERSAMPLING_XY; |
} |
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) { |
versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis; |
/branches/dongfang_FC_rewrite/analog.h |
---|
94,11 → 94,11 |
* respectively. This is = the number of occurences of each channel in the |
* channelsForStates array in analog.c. |
*/ |
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4 |
#define GYRO_SUMMATION_FACTOR_YAW 2 |
#define GYRO_OVERSAMPLING_PITCHROLL 4 |
#define GYRO_OVERSAMPLING_YAW 2 |
#define ACC_SUMMATION_FACTOR_PITCHROLL 2 |
#define ACC_SUMMATION_FACTOR_Z 1 |
#define ACC_OVERSAMPLING_XY 2 |
#define ACC_OVERSAMPLING_Z 1 |
/* |
Integration: |
137,8 → 137,8 |
* 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_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
#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. |
146,7 → 146,7 |
// How far from the end of its range a gyro is considered near-saturated. |
#define SENSOR_MIN_PITCHROLL 32 |
// Other end of the range (calculated) |
#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
// Max. boost to add "virtually" to gyro signal at total saturation. |
#define EXTRAPOLATION_LIMIT 2500 |
// Slope of the boost (calculated) |
235,7 → 235,7 |
This is OCR2 = 143.15 at 1.5V in --> simple pressure = |
*/ |
#define AIRPRESSURE_SUMMATION_FACTOR 14 |
#define AIRPRESSURE_OVERSAMPLING 14 |
#define AIRPRESSURE_FILTER 8 |
// Minimum A/D value before a range change is performed. |
#define MIN_RAWPRESSURE (200 * 2) |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
194,8 → 194,6 |
tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
// controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
controlMixer_didReceiveSignal = 1; |
/branches/dongfang_FC_rewrite/flight.c |
---|
183,7 → 183,6 |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
beepRCAlarm(); |
if (emergencyFlightTime) { |
// continue emergency flight |
emergencyFlightTime--; |
/branches/dongfang_FC_rewrite/makefile |
---|
23,11 → 23,11 |
#RC = DSL |
#RC = SPECTRUM |
#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=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 |
35,11 → 35,11 |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
GYRO=invenSense |
GYRO_HW_NAME=Isense |
GYRO_HW_FACTOR=0.6827f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
#GYRO=invenSense |
#GYRO_HW_NAME=Isense |
#GYRO_HW_FACTOR=0.6827f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
#------------------------------------------------------------------- |
# get SVN revision |
/branches/dongfang_FC_rewrite/menu.c |
---|
203,7 → 203,7 |
*/ |
case 7:// Battery Voltage / Remote Control Level |
LCD_printfxy(0,1,"Voltage: %3i.%1iV",UBat/10, UBat%10); |
LCD_printfxy(0,2,"RC-Level: %5i",RC_Quality); |
LCD_printfxy(0,2,"RC-Level: %5i",RCQuality); |
break; |
case 8:// Compass Menu Item |
LCD_printfxy(0,0,"Compass "); |
/branches/dongfang_FC_rewrite/output.c |
---|
100,40 → 100,35 |
} |
} |
void flashingLights(void) { |
void output_update(void) { |
static int8_t delay = 0; |
if (!delay--) { // 10 ms intervals |
delay = 4; |
} |
if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) { |
outputSet(0, 1); |
outputSet(1, 1); |
} else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) { |
outputSet(0, 0); |
outputSet(1, 0); |
} else { |
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_0_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
// alarm |
flashingLight(0, 25, 0x55, 25); |
} else { |
} else if (staticParams.outputDebugMask) { |
outputSet(0, debugOut.digital[0] & staticParams.outputDebugMask); |
} else if (!delay) { |
flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing); |
} |
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_1_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
// alarm |
flashingLight(1, 25, 0x55, 25); |
} else { |
} else if (staticParams.outputDebugMask) { |
outputSet(1, debugOut.digital[1] & staticParams.outputDebugMask); |
} else if (!delay) { |
flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing); |
} |
} |
} |
void output_update(void) { |
if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) { |
outputSet(0, 1); |
outputSet(1, 1); |
} else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) { |
outputSet(0, 0); |
outputSet(1, 0); |
} else if (!staticParams.outputDebugMask) { |
flashingLights(); |
} else { |
outputSet(0, debugOut.digital[0] & staticParams.outputDebugMask); |
outputSet(1, debugOut.digital[1] & staticParams.outputDebugMask); |
} |
} |
void beep(uint16_t millis) { |
beepTime = millis; |
} |
/branches/dongfang_FC_rewrite/rc.c |
---|
62,7 → 62,7 |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile int16_t PPM_diff[MAX_CHANNELS]; |
volatile uint8_t NewPpmData = 1; |
volatile uint8_t RC_Quality = 0; |
volatile uint8_t RCQuality = 0; |
int16_t RC_PRTY[4]; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t commandTimer = 0; |
114,7 → 114,7 |
TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
TIMSK1 |= (1<<ICIE1); |
RC_Quality = 0; |
RCQuality = 0; |
SREG = sreg; |
} |
173,10 → 173,10 |
signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
// check for stable signal |
if (abs(signal - PPM_in[index]) < 6) { |
if (RC_Quality < 200) |
RC_Quality += 10; |
if (RCQuality < 200) |
RCQuality += 10; |
else |
RC_Quality = 200; |
RCQuality = 200; |
} |
// If signal is the same as before +/- 1, just keep it there. |
if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
189,7 → 189,7 |
} else |
tmp = signal; |
// calculate signal difference on good signal level |
if (RC_Quality >= 195) |
if (RCQuality >= 195) |
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
else |
PPM_diff[index] = 0; |
241,8 → 241,8 |
*/ |
void RC_update() { |
int16_t tmp1, tmp2; |
if (RC_Quality) { |
RC_Quality--; |
if (RCQuality) { |
RCQuality--; |
if (NewPpmData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
+ RCDiff(CH_PITCH) * staticParams.stickD; |
274,6 → 274,8 |
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
= RC_PRTY[CONTROL_YAW] = 0; |
} |
debugOut.analog[21] = RCQuality; |
} |
/* |
298,11 → 300,11 |
} |
uint8_t RC_getSignalQuality(void) { |
if (RC_Quality >= 160) |
if (RCQuality >= 160) |
return SIGNAL_GOOD; |
if (RC_Quality >= 140) |
if (RCQuality >= 140) |
return SIGNAL_OK; |
if (RC_Quality >= 120) |
if (RCQuality >= 120) |
return SIGNAL_BAD; |
return SIGNAL_LOST; |
} |
/branches/dongfang_FC_rewrite/rc.h |
---|
12,7 → 12,7 |
extern volatile int16_t PPM_in[MAX_CHANNELS]; |
// extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported?? |
extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame |
extern volatile uint8_t RC_Quality; // rc signal quality indicator (0 to 200) |
extern volatile uint8_t RCQuality; // rc signal quality indicator (0 to 200) |
// defines for lookup staticParams.ChannelAssignment |
#define CH_PITCH 0 |
/branches/dongfang_FC_rewrite/uart0.c |
---|
142,7 → 142,7 |
"Acc. Vector ", |
"Max. Acc. Vector", |
"var0 ", //20 |
"raw ubat ", |
"rc signal ", |
"M1 ", |
"M2 ", |
"M3 ", |