Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1954 → Rev 1955

/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
56,8 → 56,7
}
}
 
analog_start();
 
startAnalogConversionCycle();
delay_ms_Mess(i < 10 ? 10 : 2);
}
delay_ms_Mess(70);
/branches/dongfang_FC_rewrite/analog.c
59,13 → 59,13
// for Delay functions
#include "timer0.h"
 
// For DebugOut
// For debugOut
#include "uart0.h"
 
// For reading and writing acc. meter offsets.
#include "eeprom.h"
 
// For DebugOut.Digital
// For debugOut.digital
#include "output.h"
 
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
283,7 → 283,6
for (uint8_t axis=0; axis<2; axis++) {
tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis];
// DebugOut.Analog[6 + 3 * axis ] = tempGyro;
/*
* Process the gyro data for the PID controller.
*/
292,14 → 291,14
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {
if (tempGyro < SENSOR_MIN_PITCHROLL) {
DebugOut.Digital[0] |= DEBUG_SENSORLIMIT;
debugOut.digital[0] |= DEBUG_SENSORLIMIT;
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
} else if (tempGyro > SENSOR_MAX_PITCHROLL) {
DebugOut.Digital[0] |= DEBUG_SENSORLIMIT;
debugOut.digital[0] |= DEBUG_SENSORLIMIT;
tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE
+ SENSOR_MAX_PITCHROLL;
} else {
DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT;
debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
}
}
344,6 → 343,10
yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
else
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
 
debugOut.analog[3] = gyro_ATT[PITCH];
debugOut.analog[4] = gyro_ATT[ROLL];
debugOut.analog[5] = yawGyro;
}
 
void analog_updateAccelerometers(void) {
374,6 → 377,9
stronglyFilteredAcc[Z] =
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
*/
 
debugOut.analog[6] = acc[PITCH];
debugOut.analog[7] = acc[ROLL];
}
 
void analog_updateAirPressure(void) {
384,8 → 390,8
if (pressureAutorangingWait) {
//A range switch was done recently. Wait for steadying.
pressureAutorangingWait--;
DebugOut.Analog[27] = (uint16_t) OCR0A;
DebugOut.Analog[31] = simpleAirPressure;
debugOut.analog[27] = (uint16_t) OCR0A;
debugOut.analog[31] = simpleAirPressure;
} else {
rawAirPressure = sensorInputs[AD_AIRPRESSURE];
if (rawAirPressure < MIN_RAWPRESSURE) {
417,13 → 423,13
// Even if the sample is off-range, use it.
simpleAirPressure = getSimplePressure(rawAirPressure);
DebugOut.Analog[27] = (uint16_t) OCR0A;
DebugOut.Analog[31] = simpleAirPressure;
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
// copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
DebugOut.Digital[1] |= DEBUG_SENSORLIMIT;
debugOut.digital[1] |= DEBUG_SENSORLIMIT;
airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
+ (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
430,7 → 436,7
} else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
// Danger: pressure near upper end of range. If the measurement saturates, the
// copter may descend uncontrolledly... Simulate a drastic increase in pressure.
DebugOut.Digital[1] |= DEBUG_SENSORLIMIT;
debugOut.digital[1] |= DEBUG_SENSORLIMIT;
airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
+ (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
438,7 → 444,7
// normal case.
// 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;
debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1)
airPressureSum += simpleAirPressure / 2;
else
459,7 → 465,7
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
// This is divided by 3 --> 10.34 counts per volt.
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
DebugOut.Analog[11] = UBat;
debugOut.analog[11] = UBat;
}
 
void analog_update(void) {
/branches/dongfang_FC_rewrite/analog.h
249,7 → 249,7
/*
* Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
*/
void analog_update(void) {
void analog_update(void);
 
/*
* "Warm" calibration: Zero-offset gyros.
/branches/dongfang_FC_rewrite/attitude.c
203,6 → 203,8
void getAnalogData(void) {
uint8_t axis;
 
analog_update();
 
for (axis = PITCH; axis <= ROLL; axis++) {
rate_PID[axis] = gyro_PID[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];
rate_ATT[axis] = gyro_ATT[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];
216,7 → 218,7
// We are done reading variables from the analog module.
// Interrupt-driven sensor reading may restart.
analogDataReady = 0;
startAnalogConversionCycle(0);
startAnalogConversionCycle();
}
 
/*
295,8 → 297,8
// Well actually the Z axis acc. check is not so silly.
uint8_t axis;
int32_t temp;
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
 
if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
<= dynamicParams.UserParams[7]) {
310,8 → 312,6
debugFullWeight = 0;
}
 
/*
 
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
permilleAcc /= 2;
debugFullWeight = 0;
319,10 → 319,10
 
if (controlActivity > 10000) { // reduce effect during stick commands
permilleAcc /= 4;
DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
debugOut.digital[0] |= DEBUG_ACC0THORDER;
if (controlActivity > 20000) { // reduce effect during stick commands
permilleAcc /= 4;
DebugOut.Digital[1] |= DEBUG_ACC0THORDER;
debugOut.digital[1] |= DEBUG_ACC0THORDER;
}
}
 
331,7 → 331,7
*/
for (axis = PITCH; axis <= ROLL; axis++) {
accDerived = getAngleEstimateFromAcc(axis);
DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
 
// 1000 * the correction amount that will be added to the gyro angle in next line.
temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
340,11 → 340,9
correctionSum[axis] += angle[axis] - temp;
}
} else {
DebugOut.Analog[9] = 0;
DebugOut.Analog[10] = 0;
debugOut.analog[9] = 0;
debugOut.analog[10] = 0;
 
DebugOut.Analog[16] = 0;
DebugOut.Analog[17] = 0;
// experiment: Kill drift compensation updates when not flying smooth.
correctionSum[PITCH] = correctionSum[ROLL] = 0;
}
381,8 → 379,8
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
// DebugOut.Analog[11 + axis] = correctionSum[axis];
DebugOut.Analog[16 + axis] = correctionSum[axis];
DebugOut.Analog[28 + axis] = driftComp[axis];
// DebugOut.Analog[16 + axis] = correctionSum[axis];
debugOut.analog[28 + axis] = driftComp[axis];
 
correctionSum[axis] = 0;
}
396,10 → 394,6
getAnalogData();
integrate();
 
DebugOut.Analog[3] = rate_PID[PITCH];
DebugOut.Analog[4] = rate_PID[ROLL];
DebugOut.Analog[5] = yawRate;
 
#ifdef ATTITUDE_USE_ACC_SENSORS
correctIntegralsByAcc0thOrder();
driftCorrection();
/branches/dongfang_FC_rewrite/attitudeControl.c
35,6 → 35,6
- throttle;
}
deltaThrottle = ((int32_t)y * effect) >> 6;
DebugOut.Analog[8] = deltaThrottle;
debugOut.analog[8] = deltaThrottle;
return throttle + deltaThrottle;
}
/branches/dongfang_FC_rewrite/configuration.c
73,7 → 73,7
SET_POT_MM(dynamicParams.HeightP,staticParams.HeightP,0,100);
SET_POT(dynamicParams.Height_ACC_Effect,staticParams.Height_ACC_Effect);
SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect);
SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,10,255);
SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,0,255);
SET_POT(dynamicParams.GyroI,staticParams.GyroI);
SET_POT(dynamicParams.GyroD,staticParams.GyroD);
SET_POT(dynamicParams.IFactor,staticParams.IFactor);
/branches/dongfang_FC_rewrite/controlMixer.c
182,7 → 182,7
updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]);
dampenControlActivity();
 
DebugOut.Analog[14] = controlActivity/10;
//debugOut.analog[14] = controlActivity/10;
 
tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]);
controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle);
/branches/dongfang_FC_rewrite/flight.c
134,9 → 134,12
}
 
void setNormalFlightParameters(void) {
setFlightParameters(dynamicParams.IFactor, dynamicParams.GyroP,
setFlightParameters(dynamicParams.IFactor,
dynamicParams.GyroP,
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
dynamicParams.GyroP, dynamicParams.UserParams[6]);
dynamicParams.GyroP,
dynamicParams.UserParams[6]
);
}
 
void setStableFlightParameters(void) {
355,22 → 358,22
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING;
// Limit yawTerm
DebugOut.Digital[0] &= ~DEBUG_CLIP;
debugOut.digital[0] &= ~DEBUG_CLIP;
if (throttleTerm > MIN_YAWGAS) {
if (yawTerm < -throttleTerm / 2) {
DebugOut.Digital[0] |= DEBUG_CLIP;
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = -throttleTerm / 2;
} else if (yawTerm > throttleTerm / 2) {
DebugOut.Digital[0] |= DEBUG_CLIP;
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;
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = -MIN_YAWGAS / 2;
} else if (yawTerm > MIN_YAWGAS / 2) {
DebugOut.Digital[0] |= DEBUG_CLIP;
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = MIN_YAWGAS / 2;
}
//CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
380,14 → 383,14
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING;
if (yawTerm < -(tmp_int - throttleTerm)) {
yawTerm = -(tmp_int - throttleTerm);
DebugOut.Digital[0] |= DEBUG_CLIP;
debugOut.digital[0] |= DEBUG_CLIP;
} else if (yawTerm > (tmp_int - throttleTerm)) {
yawTerm = (tmp_int - throttleTerm);
DebugOut.Digital[0] |= DEBUG_CLIP;
debugOut.digital[0] |= DEBUG_CLIP;
}
 
// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
DebugOut.Digital[1] &= ~DEBUG_CLIP;
debugOut.digital[1] &= ~DEBUG_CLIP;
for (axis = PITCH; axis <= ROLL; axis++) {
/*
* Compose pitch and roll terms. This is finally where the sticks come into play.
418,9 → 421,9
* TODO: Why a growing function of yaw?
*/
if (term[axis] < -tmp_int) {
DebugOut.Digital[1] |= DEBUG_CLIP;
debugOut.digital[1] |= DEBUG_CLIP;
} else if (term[axis] > tmp_int) {
DebugOut.Digital[1] |= DEBUG_CLIP;
debugOut.digital[1] |= DEBUG_CLIP;
}
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
}
430,10 → 433,10
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
DebugOut.Analog[12] = term[PITCH];
DebugOut.Analog[13] = term[ROLL];
//DebugOut.Analog[14] = yawTerm;
DebugOut.Analog[15] = throttleTerm;
debugOut.analog[12] = term[PITCH];
debugOut.analog[13] = term[ROLL];
debugOut.analog[14] = yawTerm;
debugOut.analog[15] = throttleTerm;
 
for (i = 0; i < MAX_MOTORS; i++) {
int32_t tmp;
458,9 → 461,9
CHECK_MIN_MAX(tmp, 1, 255);
throttle = tmp;
 
if (i < 4) DebugOut.Analog[22 + i] = throttle;
if (i < 4) debugOut.analog[22 + i] = throttle;
 
if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) {
if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) {
motor[i].SetPoint = throttle;
} else if (motorTestActive) {
motor[i].SetPoint = motorTest[i];
476,21 → 479,12
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
debugOut.analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
 
DebugOut.Analog[6] = 64 >> 4;
DebugOut.Analog[7] = -64 >> 4;
 
/*
DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);
DebugOut.Analog[24] = controlYaw;
DebugOut.Analog[25] = yawAngleDiff / 100L;
DebugOut.Analog[26] = accNoisePeak[PITCH];
DebugOut.Analog[27] = accNoisePeak[ROLL];
DebugOut.Analog[30] = gyroNoisePeak[PITCH];
DebugOut.Analog[31] = gyroNoisePeak[ROLL];
*/
debugOut.analog[16] = gyroPFactor;
debugOut.analog[17] = gyroIFactor;
debugOut.analog[18] = dynamicParams.GyroD;
}
}
/branches/dongfang_FC_rewrite/heightControl.c
99,7 → 99,7
}
 
// height, in meters (so the division factor is: 100)
DebugOut.Analog[30] = height / 10;
debugOut.analog[30] = height / 10;
}
 
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
124,11 → 124,11
// iHeight += heightError;
 
if (dHeight > 0) {
DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF;
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF;
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
} else if (dHeight < 0) {
DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF;
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF;
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
}
 
/*
167,13 → 167,13
if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
&& stronglyFilteredHeightDiff > -3) {
hoverThrottle = stronglyFilteredThrottle;
DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE;
debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
// DebugOut.Analog[18] = hoverThrottle;
} else
DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE;
debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
 
DebugOut.Analog[20] = dThrottle;
DebugOut.Analog[21] = hoverThrottle;
debugOut.analog[20] = dThrottle;
debugOut.analog[21] = hoverThrottle;
 
return throttle;
}
/branches/dongfang_FC_rewrite/main.c
219,9 → 219,9
if (runFlightControl) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
if (!analogDataReady) {
DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER;
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
} else {
DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
 
J4HIGH;
flight_control();
/branches/dongfang_FC_rewrite/makefile
28,18 → 28,18
#GYRO_PITCHROLL_CORRECTION=1.0f
#GYRO_YAW_CORRECTION=1.0f
 
#GYRO=ADXRS610_FC2.0
#GYRO_HW_NAME=ADXR
#GYRO_HW_FACTOR=1.2288f
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
#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
REV=0001
263,12 → 263,13
#AVRDUDE_PROGRAMMER = dt006
#AVRDUDE_PROGRAMMER = stk200
#AVRDUDE_PROGRAMMER = ponyser
AVRDUDE_PROGRAMMER = avrispv2
AVRDUDE_PROGRAMMER = usbtiny
#falls Ponyser ausgewaehlt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden
 
#AVRDUDE_PORT = com1 # programmer connected to serial device
#AVRDUDE_PORT = lpt1 # programmer connected to parallel port
AVRDUDE_PORT = usb # programmer connected to USB
AVRDUDE_PORT = usb
# programmer connected to USB
 
#AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex
AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS)
275,7 → 276,8
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
 
#avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
AVRDUDE_FLAGS = -F -p $(MCU) -P usb -B 1 -c $(AVRDUDE_PROGRAMMER)
#AVRDUDE_FLAGS = -F -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
 
# Uncomment the following if you want avrdude's erase cycle counter.
# Note that this counter needs to be initialized first using -Yn,
/branches/dongfang_FC_rewrite/output.c
118,8 → 118,8
OUTPUT_SET(0, 0);
OUTPUT_SET(1, 1);
} else {
OUTPUT_SET(0, DebugOut.Digital[0] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(1, DebugOut.Digital[1] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(0, debugOut.digital[0] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(1, debugOut.digital[1] & DIGITAL_DEBUG_MASK);
}
}
 
/branches/dongfang_FC_rewrite/timer0.c
148,9 → 148,9
cnt_1ms ^= 1;
if (!cnt_1ms) {
if (runFlightControl == 1)
DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER;
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
else
DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER;
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
}
millisecondsCount++; // increment millisecond counter
217,7 → 217,7
while (!checkDelay(t_stop)) {
if (analogDataReady) {
analogDataReady = 0;
analog_start();
startAnalogConversionCycle();
}
}
}
/branches/dongfang_FC_rewrite/uart0.c
107,7 → 107,7
int16_t Heading;
}__attribute__((packed)) Heading_t;
 
DebugOut_t DebugOut;
DebugOut_t debugOut;
Data3D_t Data3D;
UART_VersionInfo_t UART_VersionInfo;
 
129,8 → 129,8
"GyroPitch(PID) ",
"GyroRoll(PID) ",
"GyroYaw ", //5
"GYRO_RATE_FACTOR",
"GYRO_RATE_FACTOR",
"AccPitch (raw) ",
"AccRoll (raw) ",
"AttitudeControl ",
"AccPitch (angle)",
"AccRoll (angle) ", //10
137,11 → 137,11
"UBat ",
"Pitch Term ",
"Roll Term ",
"controlActivity ",
"Yaw Term ",
"ca debug ", //15
"0th O Corr pitch",
"0th O Corr roll ",
"unused ",
"gyroP ",
"gyroI ",
"gyroD ",
"unused ",
"dHeightThrottle ", //20
"hoverThrottle ",
707,7 → 707,7
 
if (((DebugData_Interval && checkDelay(DebugData_Timer)) || request_DebugData)
&& txd_complete) {
SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut));
SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut));
DebugData_Timer = setDelay(DebugData_Interval);
request_DebugData = FALSE;
}
/branches/dongfang_FC_rewrite/uart0.h
22,11 → 22,11
extern uint8_t motorTest[16];
 
typedef struct {
uint8_t Digital[2];
uint16_t Analog[32]; // Debugvalues
uint8_t digital[2];
uint16_t analog[32]; // Debugvalues
}__attribute__((packed)) DebugOut_t;
 
extern DebugOut_t DebugOut;
extern DebugOut_t debugOut;
 
typedef struct {
int16_t AngleNick; // in 0.1 deg