/branches/dongfang_FC_rewrite/analog.c |
---|
124,6 → 124,8 |
*/ |
volatile int16_t UBat = 100; |
volatile int16_t filteredAirPressure; |
/* |
* Control and status. |
*/ |
137,14 → 139,14 |
volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak; |
// ADC channels |
#define AD_GYRO_YAW 0 |
#define AD_GYRO_ROLL 1 |
#define AD_GYRO_PITCH 2 |
#define AD_AIRPRESSURE 3 |
#define AD_UBAT 4 |
#define AD_ACC_Z 5 |
#define AD_ACC_ROLL 6 |
#define AD_ACC_PITCH 7 |
#define AD_GYRO_YAW 0 |
#define AD_GYRO_ROLL 1 |
#define AD_GYRO_PITCH 2 |
#define AD_AIRPRESSURE 3 |
#define AD_UBAT 4 |
#define AD_ACC_Z 5 |
#define AD_ACC_ROLL 6 |
#define AD_ACC_PITCH 7 |
/* |
* Table of AD converter inputs for each state. |
163,14 → 165,14 |
AD_GYRO_ROLL, |
AD_GYRO_YAW, |
AD_ACC_PITCH, |
AD_ACC_ROLL, |
AD_ACC_PITCH, |
// AD_AIRPRESSURE, |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_ACC_Z, // at 7, measure Z acc. |
AD_ACC_Z, // at 7, finish Z acc. |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW, // at 10, finish yaw gyro |
177,11 → 179,11 |
AD_ACC_PITCH, // at 11, finish pitch axis acc. |
AD_ACC_ROLL, // at 12, finish roll axis acc. |
AD_AIRPRESSURE, // at 13, finish air pressure. |
AD_GYRO_PITCH, // at 13, finish pitch gyro |
AD_GYRO_ROLL, // at 14, finish roll gyro |
AD_UBAT // at 15, measure battery. |
AD_GYRO_PITCH, // at 14, finish pitch gyro |
AD_GYRO_ROLL, // at 15, finish roll gyro |
AD_UBAT // at 16, measure battery. |
}; |
// Feature removed. Could be reintroduced later - but should work for all gyro types then. |
226,6 → 228,19 |
} |
} |
#define ADCENTER (1023/2) |
#define HALFRANGE 400 |
uint8_t stepsize = 53; |
uint16_t getAbsPressure(int advalue) { |
return (uint16_t)OCR0A * (uint16_t)stepsize + advalue; |
} |
uint16_t filterAirPressure(uint16_t rawpressure) { |
return rawpressure; |
} |
/*****************************************************/ |
/* Interrupt Service Routine for ADC */ |
/*****************************************************/ |
238,7 → 253,8 |
static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0}; |
uint8_t i; |
int16_t step = OCR0A; |
// for various filters... |
static int16_t pitchGyroFilter, rollGyroFilter, tempOffsetGyro; |
287,7 → 303,25 |
measureNoise(rollAxisAcc, &rollAccNoisePeak, 1); |
break; |
case 13: // pitch gyro |
case 13: // air pressure |
if (sensorInputs[AD_AIRPRESSURE] < ADCENTER-HALFRANGE) { |
// value is too low, so decrease voltage on the op amp minus input, making the value higher. |
step -= ((HALFRANGE-sensorInputs[AD_AIRPRESSURE]) / stepsize + 1); |
if (step<0) step = 0; |
OCR0A = step; |
// wait = ... (calculate something here .. calculate at what time the R/C filter is to within one sample off) |
} else if (sensorInputs[AD_AIRPRESSURE] > ADCENTER+HALFRANGE) { |
// value is too high, so increase voltage on the op amp minus input, making the value lower. |
step += ((sensorInputs[AD_AIRPRESSURE] - HALFRANGE)/stepsize + 1); |
if (step>254) step = 254; |
OCR0A = step; |
// wait = ... (calculate something here .. calculate at what time the R/C filter is to within one sample off) |
} else { |
filteredAirPressure = filterAirPressure(getAbsPressure(sensorInputs[AD_AIRPRESSURE])); |
} |
break; |
case 14: // pitch gyro |
rawPitchGyroSum = sensorInputs[AD_GYRO_PITCH]; |
// Filter already before offsetting. The offsetting resolution improvement obtained by divding by |
// GYROS_FIRSTORDERFILTER _after_ offsetting is too small to be worth pursuing. |
307,7 → 341,7 |
measureNoise(hiResPitchGyro, &pitchGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING); |
break; |
case 14: // Roll gyro. Works the same as pitch. |
case 15: // Roll gyro. Works the same as pitch. |
rawRollGyroSum = sensorInputs[AD_GYRO_ROLL]; |
rollGyroFilter = (rollGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawRollGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER; |
#ifdef GYRO_REVERSE_ROLL |
321,7 → 355,7 |
measureNoise(hiResRollGyro, &rollGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING); |
break; |
case 15: |
case 16: |
// battery |
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
analogDataReady = 1; // mark |
/branches/dongfang_FC_rewrite/analog.h |
---|
2,8 → 2,8 |
#define _ANALOG_H |
#include <inttypes.h> |
#include "invenSense.h" |
// #include "ENC-03_FC1.3.h" |
//#include "invenSense.h" |
#include "ENC-03_FC1.3.h" |
/* |
/branches/dongfang_FC_rewrite/attitude.c |
---|
288,11 → 288,11 |
} |
DebugOut.Analog[3] = pitchRate; |
DebugOut.Analog[3 + 3] = ACPitchRate; |
// DebugOut.Analog[3 + 3] = ACPitchRate; |
DebugOut.Analog[4] = rollRate; |
DebugOut.Analog[4 + 3] = ACRollRate; |
// DebugOut.Analog[4 + 3] = ACRollRate; |
DebugOut.Analog[5] = yawRate; |
DebugOut.Analog[5 + 3] = ACYawRate; |
// DebugOut.Analog[5 + 3] = ACYawRate; |
/* |
DebugOut.Analog[9] = int_cos(pitchAngle); |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
148,8 → 148,12 |
controlPitch = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
controlRoll = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
DebugOut.Analog[14] = controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]; |
DebugOut.Analog[15] = controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]; |
controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
DebugOut.Analog[6] = controlPitch; |
DebugOut.Analog[7] = controlRoll; |
DebugOut.Analog[8] = controlYaw; |
if (RC_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
/branches/dongfang_FC_rewrite/flight.c |
---|
558,8 → 558,8 |
DebugOut.Analog[9] = setPointYaw; |
DebugOut.Analog[10] = yawIFactor; |
DebugOut.Analog[11] = gyroIFactor; |
DebugOut.Analog[12] = RC_getVariable(0); |
DebugOut.Analog[13] = dynamicParams.UserParams[0]; |
// DebugOut.Analog[12] = RC_getVariable(0); |
// DebugOut.Analog[13] = dynamicParams.UserParams[0]; |
DebugOut.Analog[14] = RC_getVariable(4); |
DebugOut.Analog[15] = dynamicParams.UserParams[4]; |
/* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */ |
/branches/dongfang_FC_rewrite/main.c |
---|
129,12 → 129,12 |
// Parameter Set handling |
ParamSet_Init(); |
// wait for a short time (otherwise the RC channel check won't work below) |
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
// while(!CheckDelay(timer)); |
// Instead, while away the time by flashing the 2 outputs: |
// First J16, then J17. Makes it easier to see which is which. |
timer = SetDelay(200); |
J16_ON; |
GRN_OFF; |
153,6 → 153,7 |
J17_OFF; |
twi_diagnostics(); |
printf("\n\r==================================="); |
/* |
195,7 → 196,7 |
// Init flight parameters |
flight_setNeutral(); |
RED_OFF; |
// RED_OFF; |
beep(2000); |
260,8 → 261,7 |
RED_OFF; |
} |
// allow Serial Data Transmit if motors must not updated or motors are not running |
// Why only when that??? |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
USART0_TransmitTxData(); |
} |
/branches/dongfang_FC_rewrite/makefile |
---|
22,9 → 22,9 |
RC = DSL |
#RC = SPECTRUM |
#GYRO=ENC-03_FC1.3 |
GYRO=ENC-03_FC1.3 |
#GYRO=ADXRS610_FC2.0 |
GYRO=invenSense |
#GYRO=invenSense |
#------------------------------------------------------------------- |
# get SVN revision |
/branches/dongfang_FC_rewrite/rc.c |
---|
56,6 → 56,10 |
#include "controlMixer.h" |
#include "configuration.h" |
// for DebugOut only. |
#include "uart0.h" |
// The channel array is 1-based. The 0th entry is not used. |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile int16_t PPM_diff[MAX_CHANNELS]; |
127,7 → 131,7 |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________| |
___| |_| |_| |_| |_.............| |________________| |
<-----><-------><------><--------> <------> <--- |
t0 t1 t2 t4 tn t0 |
212,8 → 216,12 |
if(RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
DebugOut.Analog[12] = stickOffsetPitch; |
DebugOut.Analog[13] = stickOffsetRoll; |
RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[6] + 120; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
276,8 → 284,8 |
// In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
stickOffsetPitch = stickOffsetRoll = 0; |
} else { |
stickOffsetPitch += RCChannel(CH_PITCH); |
stickOffsetRoll += RCChannel(CH_ROLL); |
stickOffsetPitch = RCChannel(CH_PITCH); |
stickOffsetRoll = RCChannel(CH_ROLL); |
} |
} |
/branches/dongfang_FC_rewrite/timer0.c |
---|
139,8 → 139,7 |
if(!cnt--) { // every 10th run (9.765kHz/10 = 976Hz) |
cnt = 9; |
cnt_1ms++; |
cnt_1ms %= 2; |
cnt_1ms^=1; |
if(!cnt_1ms) runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
CountMilliseconds++; // increment millisecond counter |
} |
/branches/dongfang_FC_rewrite/uart0.c |
---|
128,14 → 128,14 |
"GyroPitch ", |
"GyroRoll ", |
"GyroYaw ", //5 |
"ACGyroPitch ", |
"ACGyroRoll ", |
"ACGyroYaw ", |
"ControlPitch ", |
"ControlRoll ", |
"ControlYaw ", |
"SetPointYaw ", |
"YawRateIFactor ", //10 |
"Gyro I Factor ", |
"R/C Var 0 ", |
"User Param 0 ", |
"StickOffsetPitch", |
"StickOffsetRoll ", |
"R/C Var 4 ", |
"User Param 4 ", //15 |
"RCQuality ", |