Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2018 → Rev 2019

/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 ",