Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2159 → Rev 2160

/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
12,7 → 12,7
IMUConfig.accQuadrant = 4;
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_XY;
staticParams.gyroD = 5;
IMUConfig.driftCompDivider = 1;
IMUConfig.driftCompDivider = 2;
IMUConfig.driftCompLimit = 3;
IMUConfig.zerothOrderCorrection = 1;
IMUConfig.zerothOrderCorrection = 3;
}
/branches/dongfang_FC_rewrite/analog.c
306,17 → 306,24
}
 
void measureGyroActivity(int16_t newValue) {
gyroActivity += (uint32_t)((int32_t)newValue * newValue);
gyroActivity += newValue * newValue;
// abs(newValue); // (uint32_t)((int32_t)newValue * newValue);
}
 
#define GADAMPING 6
void dampenGyroActivity(void) {
static uint8_t cnt = 0;
 
if (++cnt >= IMUConfig.gyroActivityDamping) {
cnt = 0;
gyroActivity *= (uint32_t)((1L<<GADAMPING)-1);
gyroActivity >>= GADAMPING;
}
 
/*
if (gyroActivity >= 10) gyroActivity -= 10;
else if (gyroActivity <=- 10) gyroActivity += 10;
*/
}
 
void analog_updateGyros(void) {
469,8 → 476,8
// Even if the sample is off-range, use it.
simpleAirPressure = getSimplePressure(rawAirPressure);
debugOut.analog[6] = rawAirPressure;
debugOut.analog[7] = simpleAirPressure;
// debugOut.analog[6] = rawAirPressure;
// debugOut.analog[7] = simpleAirPressure;
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
// Danger: pressure near lower end of range. If the measurement saturates, the
/branches/dongfang_FC_rewrite/attitude.c
228,19 → 228,17
uint8_t axis;
int32_t temp;
 
debugOut.analog[13] = gyroActivity / 65536L;
debugOut.analog[12] = IMUConfig.zerothOrderCorrection;
 
uint16_t ca;
ca = gyroActivity >> 12;
debugOut.analog[14] = ca;
uint16_t ca = gyroActivity >> 14;
uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance;
debugOut.analog[15] = gyroActivityWeighted;
 
uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance;
if (!gyroActivityWeighted) gyroActivityWeighted = 1;
 
uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted;
 
debugOut.analog[28] = IMUConfig.rateTolerance;
debugOut.analog[15] = gyroActivityWeighted;
debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
 
295,8 → 293,8
driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider;
CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit);
// DebugOut.Analog[11 + axis] = correctionSum[axis];
// DebugOut.Analog[16 + axis] = correctionSum[axis];
// debugOut.analog[28 + axis] = driftComp[axis];
debugOut.analog[6 + axis] = correctionSum[axis];
debugOut.analog[13 + axis] = driftComp[axis];
correctionSum[axis] = 0;
}
}
/branches/dongfang_FC_rewrite/attitude.h
73,7 → 73,7
* sin v = acc / sqrt(acc^2 + acc_z^2)
* Using that v is a small angle, and the near side is about equal to the the hypothenusis:
* sin v ~= acc / acc_z
* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,
* Assuming that the multicopter is hovering at small pitch and roll angles, acc_z is about 410,
* and sin v ~= v (small angles, in radians):
* sin v ~= acc / 410
* v / 57.3 ~= acc / 410
/branches/dongfang_FC_rewrite/configuration.c
71,8 → 71,12
}
 
void setCPUType(void) { // works only after reset or power on when the registers have default values
#if (MCU_TYPE==atmega644)
CPUType=ATMEGA644;
#else
if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P; // initial Values for 644P after reset
else CPUType = ATMEGA644;
#endif
}
 
/*
245,7 → 249,8
motorMixer.matrix[i][MIX_YAW] = 0;
motorMixer.matrix[i][MIX_OPPOSITE_MOTOR] = (uint8_t)-1;
}
// default = Quadro
/*
// default = Quadro+
motorMixer.matrix[0][MIX_PITCH] = +64;
motorMixer.matrix[0][MIX_YAW] = +64;
motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1;
263,27 → 268,30
motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2;
 
memcpy(motorMixer.name, "Quadro +\0", 9);
*/
 
/*
// default = X
mixerMatrix.motor[0][MIX_PITCH] = +45;
mixerMatrix.motor[0][MIX_ROLL] = +45;
mixerMatrix.motor[0][MIX_YAW] = +64;
// default = Quadro
motorMixer.matrix[0][MIX_PITCH] = +64;
motorMixer.matrix[0][MIX_ROLL] = +64;
motorMixer.matrix[0][MIX_YAW] = +64;
motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1;
 
mixerMatrix.motor[1][MIX_PITCH] = -45;
mixerMatrix.motor[1][MIX_ROLL] = -45;
mixerMatrix.motor[1][MIX_YAW] = +64;
motorMixer.matrix[1][MIX_PITCH] = -64;
motorMixer.matrix[1][MIX_ROLL] = -64;
motorMixer.matrix[1][MIX_YAW] = +64;
motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0;
 
mixerMatrix.motor[2][MIX_PITCH] = +45;
mixerMatrix.motor[2][MIX_ROLL] = -45;
mixerMatrix.motor[2][MIX_YAW] = -64;
motorMixer.matrix[2][MIX_PITCH] = +64;
motorMixer.matrix[2][MIX_ROLL] = -64;
motorMixer.matrix[2][MIX_YAW] = -64;
motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3;
 
mixerMatrix.motor[3][MIX_PITCH] = -45;
mixerMatrix.motor[3][MIX_ROLL] = +45;
mixerMatrix.motor[3][MIX_YAW] = -64;
motorMixer.matrix[3][MIX_PITCH] = -64;
motorMixer.matrix[3][MIX_ROLL] = +64;
motorMixer.matrix[3][MIX_YAW] = -64;
motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2;
 
memcpy(motorMixer.name, "Quadro X\0", 9);
*/
}
 
/***************************************************/
290,6 → 298,7
/* Default Values for R/C Channels */
/***************************************************/
void channelMap_default(void) {
channelMap.RCPolarity = 1;
channelMap.channels[CH_PITCH] = 1;
channelMap.channels[CH_ROLL] = 0;
channelMap.channels[CH_THROTTLE] = 2;
/branches/dongfang_FC_rewrite/configuration.h
84,10 → 84,12
*/
 
typedef struct {
uint8_t trim;
uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky.
uint8_t HWTrim;
uint8_t variableOffset;
uint8_t channels[MAX_CHANNELS];
uint8_t channels[MAX_CHANNELS];
} ChannelMap_t;
 
extern ChannelMap_t channelMap;
 
typedef struct {
/branches/dongfang_FC_rewrite/directGPSNaviControl.c
431,9 → 431,11
PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
 
debugOut.analog[16] = flightMode;
debugOut.analog[17] = naviStatus;
//debugOut.analog[16] = flightMode;
//debugOut.analog[17] = naviStatus;
 
/*
debugOut.analog[18] = naviSticks[CONTROL_PITCH];
debugOut.analog[19] = naviSticks[CONTROL_ROLL];
*/
}
/branches/dongfang_FC_rewrite/flight.c
250,16 → 250,21
debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
 
debugOut.analog[16] = acc[PITCH];
debugOut.analog[17] = acc[ROLL];
 
debugOut.analog[3] = rate_ATT[PITCH];
debugOut.analog[4] = rate_ATT[ROLL];
debugOut.analog[5] = yawRate;
}
 
/*
debugOut.analog[6] = term[PITCH];
debugOut.analog[7] = term[ROLL];
debugOut.analog[8] = yawTerm;
debugOut.analog[9] = throttleTerm;
*/
 
//debugOut.analog[16] = gyroActivity;
 
for (i = 0; i < MAX_MOTORS; i++) {
int32_t tmp;
uint8_t throttle;
/branches/dongfang_FC_rewrite/heightControl.c
176,9 → 176,9
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE;
 
debugOut.analog[10] = dThrottleP;
debugOut.analog[11] = dThrottleI;
debugOut.analog[12] = dThrottleD;
//debugOut.analog[10] = dThrottleP;
//debugOut.analog[11] = dThrottleI;
//debugOut.analog[12] = dThrottleD;
//debugOut.analog[13] = heightError/10;
 
int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD;
/branches/dongfang_FC_rewrite/makefile
1,6 → 1,6
#--------------------------------------------------------------------
# MCU name
MCU = atmega644p
MCU = atmega644
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
16,22 → 16,22
 
# Use one of the extensions for a gps solution
#EXT = NAVICTRL
EXT = DIRECT_GPS
#EXT = DIRECT_GPS
#EXT = MK3MAG
#EXT =
EXT =
 
#GYRO=ENC-03_FC1.3
#GYRO_HW_NAME=ENC
#GYRO_HW_FACTOR=1.304f
#GYRO_PITCHROLL_CORRECTION=0.7f
#GYRO_YAW_CORRECTION=0.9f
 
GYRO=ADXRS610_FC2.0
GYRO_HW_NAME=ADXR
GYRO_HW_FACTOR=1.2288f
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=invenSense
#GYRO_HW_NAME=Isense
#GYRO_HW_FACTOR=0.6827f
255,8 → 255,8
#AVRDUDE_PROGRAMMER = dt006
#AVRDUDE_PROGRAMMER = stk200
#AVRDUDE_PROGRAMMER = ponyser
#AVRDUDE_PROGRAMMER = avrispv2
AVRDUDE_PROGRAMMER = usbtiny
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
/branches/dongfang_FC_rewrite/rc.c
11,10 → 11,15
// The channel array is 0-based!
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
volatile uint16_t RC_buffer[MAX_CHANNELS];
volatile uint8_t inBfrPnt = 0;
 
volatile uint8_t RCQuality;
uint8_t lastRCCommand = COMMAND_NONE;
uint8_t commandTimer = 0;
 
#define TIME(s) ((int16_t)(((long)F_CPU/(long)64000)*(float)s + 0.5f))
 
/***************************************************************
* 16bit timer 1 is used to decode the PPM-Signal
***************************************************************/
41,18 → 46,23
PORTD &= ~(1<<PORTD3);
}
 
// Timer/Counter1 Control Register A, B, C
 
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
// Enable input capture noise cancler (bit: ICNC1=1)
// Trigger on positive edge of the input capture pin (bit: ICES1=1),
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10));
TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
TCCR1B &= ~((1<<WGM13) | (1<<WGM12) | (1<<CS12));
TCCR1B |= (1<<CS11) | (1<<CS10) | (1<<ICNC1);
TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
 
if (channelMap.RCPolarity) {
TCCR1B |= (1<<ICES1);
} else {
TCCR1B &= ~(1<<ICES1);
}
 
TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
 
// Timer/Counter1 Interrupt Mask Register
67,6 → 77,22
SREG = sreg;
}
 
/*
* This new and much faster interrupt handler should reduce servo jolts.
*/
ISR(TIMER1_CAPT_vect) {
static uint16_t oldICR1 = 0;
uint16_t signal = (uint16_t)ICR1 - oldICR1;
oldICR1 = ICR1;
//sync gap? (3.5 ms < signal < 25.6 ms)
if (signal > TIME(3.5)) {
inBfrPnt = 0;
} else if (inBfrPnt<MAX_CHANNELS) {
RC_buffer[inBfrPnt++] = signal;
if (RCQuality <= 200-4) RCQuality+=4; else RCQuality = 200;
}
}
 
/********************************************************************/
/* Every time a positive edge is detected at PD6 */
/********************************************************************/
88,64 → 114,18
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
the syncronization gap.
*/
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
int16_t signal = 0, tmp;
static int16_t index;
static uint16_t oldICR1 = 0;
 
// 16bit Input Capture Register ICR1 contains the timer value TCNT1
// at the time the edge was detected
 
// calculate the time delay to the previous event time which is stored in oldICR1
// calculatiing the difference of the two uint16_t and converting the result to an int16_t
// implicit handles a timer overflow 65535 -> 0 the right way.
signal = (uint16_t) ICR1 - oldICR1;
oldICR1 = ICR1;
 
//sync gap? (3.52 ms < signal < 25.6 ms)
if ((signal > 1100) && (signal < 8000)) {
index = 0;
} else { // within the PPM frame
if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
// check for valid signal length (0.8 ms < signal < 2.1984 ms)
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
if ((signal > 250) && (signal < 687)) {
// shift signal to zero symmetric range -154 to 159
signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
// check for stable signal
if (abs(signal - PPM_in[index]) < 6) {
if (RCQuality < 200)
RCQuality += 10;
else
RCQuality = 200;
}
// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.
// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
// In addition, if the signal is very close to 0, just set it to 0.
if (signal >= -1 && signal <= 1) {
tmp = 0;
//} else {
// tmp = PPM_in[index];
// }
} else
tmp = signal;
// calculate signal difference on good signal level
if (RCQuality >= 195)
PPM_diff[index] = signal - PPM_in[index]; //((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
else
PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
void RC_process(void) {
if (RCQuality) RCQuality--;
for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
uint16_t signal = RC_buffer[channel];
if (signal != 0) {
RC_buffer[channel] = 0; // reset to flag value already used.
if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
signal -= TIME(1.5);
PPM_diff[channel] = signal - PPM_in[channel];
PPM_in[channel] = signal;
}
index++; // next channel
// demux sum signal for channels 5 to 7 to J3, J4, J5
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
// channels are usually available at the receiver anyway.
// if(index == 5) J3HIGH; else J3LOW;
// if(index == 6) J4HIGH; else J4LOW;
// if(CPUType != ATMEGA644P) // not used as TXD1
// {
// if(index == 7) J5HIGH; else J5LOW;
// }
}
}
}
182,6 → 162,7
*/
void RC_periodicTaskAndPRTY(int16_t* PRTY) {
int16_t tmp1, tmp2;
RC_process();
if (RCQuality) {
RCQuality--;
PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
/branches/dongfang_FC_rewrite/uart0.c
94,20 → 94,20
"GyroPitch ",
"GyroRoll ",
"GyroYaw ", //5
"PitchTerm ",
"RollTerm ",
"correctionSum pi",
"correctionSum ro",
"ThrottleTerm ",
"YawTerm ",
"heightP ", //10
"heightI ",
"heightD ",
"gyroActivity ",
"ca ",
"gyroDPitch ", //10
"gyroDRoll ",
"zerothOrderCorr ",
"DriftCompPitch ",
"DriftCompRoll ",
"GActivityDivider", //15
"NaviMode ",
"NaviStatus ",
"NaviStickP ",
"NaviStickR ",
"AccPitch ",
"AccRoll ",
" ",
" ",
"control act wghd", //20
"acc vector wghd ",
"Height[dm] ",
116,7 → 116,7
"EFT ", //25
"naviPitch ",
"naviRoll ",
"tolerance ",
"Rate Tolerance ",
"Gyro Act Cont. ",
"GPS altitude ", //30
"GPS vert accura "
735,7 → 735,8
}
 
if (request_PPMChannels && txd_complete) {
sendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in));
uint8_t length = MAX_CHANNELS;
sendOutData('P', FC_ADDRESS, 2, &length, 1, (uint8_t*)&PPM_in, sizeof(PPM_in));
request_PPMChannels = FALSE;
}
 
744,6 → 745,7
request_variables = FALSE;
}
 
#ifdef USE_DIRECT_GPS
if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) {
int32_t height = analog_getHeight();
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
753,4 → 755,5
OSD_timer = setDelay(OSD_interval);
request_OSD = FALSE;
}
#endif
}