/branches/dongfang_FC_rewrite/flight2.c |
---|
File deleted |
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c |
---|
1,6 → 1,8 |
#include "ADXRS610_FC2.0.h" |
#include "configuration.h" |
const uint8_t GYROS_REVERSE[2] = {0,0}; |
void gyro_calibrate(void) {} |
void gyro_setDefaults(void) { |
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.h |
---|
7,7 → 7,6 |
*/ |
#define GYRO_HW_NAME "ADXR" |
#define GYRO_HW_FACTOR 1.2288f |
/* |
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
5,6 → 5,7 |
#include "configuration.h" |
#include "timer0.h" |
const uint8_t GYROS_REVERSE[2] = {0,0}; |
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510 |
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515 |
25,12 → 26,12 |
numberOfAxesInRange = 0; |
if(rawPitchGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_PITCH]--; |
else if(rawPitchGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_PITCH]++; |
if(rawGyroSum[PITCH] < PITCHROLL_MINLIMIT) DACValues[PITCH]--; |
else if(rawGyroSum[PITCH] > PITCHROLL_MAXLIMIT) DACValues[PITCH]++; |
else numberOfAxesInRange++; |
if(rawRollGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_ROLL]--; |
else if(rawRollGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_ROLL]++; |
if(rawGyroSum[ROLL] < PITCHROLL_MINLIMIT) DACValues[ROLL]--; |
else if(rawGyroSum[ROLL] > PITCHROLL_MAXLIMIT) DACValues[ROLL]++; |
else numberOfAxesInRange++; |
if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--; |
37,15 → 38,15 |
else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ; |
else numberOfAxesInRange++; |
if(DACValues[DAC_PITCH] < 10) { |
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 10; |
} else if(DACValues[DAC_PITCH] > 245) { |
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 245; |
if(DACValues[PITCH] < 10) { |
/* GyroDefectNick = 1; */ DACValues[PITCH] = 10; |
} else if(DACValues[PITCH] > 245) { |
/* GyroDefectNick = 1; */ DACValues[PITCH] = 245; |
} |
if(DACValues[DAC_ROLL] < 10) { |
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 10; |
/* GyroDefectRoll = 1; */ DACValues[ROLL] = 10; |
} else if(DACValues[DAC_ROLL] > 245) { |
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 245; |
/* GyroDefectRoll = 1; */ DACValues[ROLL] = 245; |
} |
if(DACValues[DAC_YAW] < 10) { |
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 10; |
/branches/dongfang_FC_rewrite/H_and_I_AxisCoupling.txt |
---|
68,9 → 68,9 |
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / CONTROL_SCALING); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / CONTROL_SCALING); |
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / CONTROL_SCALING) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / CONTROL_SCALING)); |
messwertNick er nu P-part.... |
/branches/dongfang_FC_rewrite/analog.c |
---|
65,41 → 65,34 |
#include "eeprom.h" |
/* |
* Arrays could have been used for the 2 * 3 axes, but despite some repetition, |
* the code is easier to read without. |
* |
* For each A/D conversion cycle, each channel (eg. the yaw gyro, or the Z axis |
* accelerometer) is sampled a number of times (see array channelsForStates), and |
* the results for each channel are summed. Here are those for the gyros and the |
* acc. meters. They are not zero-offset. |
* For each A/D conversion cycle, each analog channel is sampled a number of times |
* (see array channelsForStates), and the results for each channel are summed. |
* Here are those for the gyros and the acc. meters. They are not zero-offset. |
* They are exported in the analog.h file - but please do not use them! The only |
* reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
* the offsets with the DAC. |
*/ |
volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum; |
volatile int16_t pitchAxisAcc = 0, rollAxisAcc = 0, ZAxisAcc = 0; |
volatile int16_t filteredPitchAxisAcc = 0, filteredRollAxisAcc = 0; |
volatile int16_t rawGyroSum[2], rawYawGyroSum; |
volatile int16_t acc[2] = {0,0}, ZAcc = 0; |
volatile int16_t filteredAcc[2] = {0,0}; |
// that float one - "Top" - is missing. |
/* |
* These 4 exported variables are zero-offset. The "filtered" ones are |
* (if configured to with the GYROS_SECONDORDERFILTER define) low pass |
* filtered versions of the other 2. |
* They are derived from the "raw" values above, by zero-offsetting. |
* These 4 exported variables are zero-offset. The "PID" ones are used |
* in the attitude control as rotation rates. The "ATT" ones are for |
* integration to angles. |
*/ |
volatile int16_t hiResPitchGyro = 0, hiResRollGyro = 0; |
volatile int16_t filteredHiResPitchGyro = 0, filteredHiResRollGyro = 0; |
volatile int16_t pitchGyroD = 0, rollGyroD = 0; |
volatile int16_t gyro_PID[2]; |
volatile int16_t gyro_ATT[2]; |
volatile int16_t gyroD[2]; |
volatile int16_t yawGyro = 0; |
/* |
* Offset values. These are the raw gyro and acc. meter sums when the copter is |
* standing still. They are used for adjusting the gyro and acc. meter values |
* to be zero when the copter stands still. |
* to be centered on zero. |
*/ |
volatile int16_t pitchOffset, rollOffset, yawOffset; |
volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset; |
volatile int16_t gyroOffset[2], yawGyroOffset; |
volatile int16_t accOffset[2], ZAccOffset; |
/* |
* This allows some experimentation with the gyro filters. |
110,12 → 103,14 |
volatile uint8_t GYROS_DFILTER; |
volatile uint8_t ACC_FILTER; |
// Air pressure (no support right now). |
// volatile int32_t AirPressure = 32000; |
// volatile uint8_t average_pressure = 0; |
// volatile int16_t StartAirPressure; |
// volatile uint16_t ReadingAirPressure = 1023; |
// volatile int16_t HeightD = 0; |
/* |
* Air pressure measurement. |
*/ |
#define MIN_RAWPRESSURE 200 |
#define MAX_RAWPRESSURE (1023-MIN_RAWPRESSURE) |
volatile uint8_t rangewidth = 53; |
volatile uint16_t rawAirPressure; |
volatile uint16_t filteredAirPressure; |
/* |
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
124,8 → 119,6 |
*/ |
volatile int16_t UBat = 100; |
volatile int16_t filteredAirPressure; |
/* |
* Control and status. |
*/ |
135,8 → 128,8 |
/* |
* Experiment: Measuring vibration-induced sensor noise. |
*/ |
volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak; |
volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak; |
volatile uint16_t gyroNoisePeak[2]; |
volatile uint16_t accNoisePeak[2]; |
// ADC channels |
#define AD_GYRO_YAW 0 |
228,13 → 221,8 |
} |
} |
#define ADCENTER (1023/2) |
#define HALFRANGE 400 |
uint8_t stepsize = 53; |
uint16_t getAbsPressure(int advalue) { |
return (uint16_t)OCR0A * (uint16_t)stepsize + advalue; |
return (uint16_t)OCR0A * (uint16_t)rangewidth + advalue; |
} |
uint16_t filterAirPressure(uint16_t rawpressure) { |
241,22 → 229,21 |
return rawpressure; |
} |
/*****************************************************/ |
/* Interrupt Service Routine for ADC */ |
/*****************************************************/ |
// Runs at 312.5 kHz or 3.2 µs |
// When all states are processed the interrupt is disabled |
// and the update of further AD conversions is stopped. |
/***************************************************** |
* Interrupt Service Routine for ADC |
* Runs at 312.5 kHz or 3.2 µs. When all states are |
* processed the interrupt is disabled and further |
* AD conversions are stopped. |
*****************************************************/ |
ISR(ADC_vect) { |
static uint8_t ad_channel = AD_GYRO_PITCH, state = 0; |
static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0}; |
static uint8_t pressure_wait = 10; |
uint8_t i, axis; |
int16_t range; |
uint8_t i; |
int16_t step = OCR0A; |
// for various filters... |
static int16_t pitchGyroFilter, rollGyroFilter, tempOffsetGyro; |
int16_t tempOffsetGyro, tempGyro; |
sensorInputs[ad_channel] += ADC; |
267,9 → 254,9 |
switch(state++) { |
case 7: // Z acc |
#ifdef ACC_REVERSE_ZAXIS |
ZAxisAcc = -ZAxisAccOffset - sensorInputs[AD_ACC_Z]; |
ZAcc = -ZAccOffset - sensorInputs[AD_ACC_Z]; |
#else |
ZAxisAcc = sensorInputs[AD_ACC_Z] - ZAxisAccOffset; |
ZAcc = sensorInputs[AD_ACC_Z] - ZAccOffset; |
#endif |
break; |
276,83 → 263,115 |
case 10: // yaw gyro |
rawYawGyroSum = sensorInputs[AD_GYRO_YAW]; |
#ifdef GYRO_REVERSE_YAW |
yawGyro = rawYawGyroSum - yawOffset; |
yawGyro = rawYawGyroSum - yawGyroOffset; |
#else |
yawGyro = yawOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3). |
yawGyro = yawGyroOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3). |
#endif |
break; |
case 11: // pitch axis acc. |
#ifdef ACC_REVERSE_PITCHAXIS |
pitchAxisAcc = -pitchAxisAccOffset - sensorInputs[AD_ACC_PITCH]; |
acc[PITCH] = -accOffset[PITCH] - sensorInputs[AD_ACC_PITCH]; |
#else |
pitchAxisAcc = sensorInputs[AD_ACC_PITCH] - pitchAxisAccOffset; |
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH]; |
#endif |
filteredPitchAxisAcc = (filteredPitchAxisAcc * (ACC_FILTER-1) + pitchAxisAcc) / ACC_FILTER; |
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER; |
measureNoise(pitchAxisAcc, &pitchAccNoisePeak, 1); |
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1); |
break; |
case 12: // roll axis acc. |
#ifdef ACC_REVERSE_ROLLAXIS |
rollAxisAcc = sensorInputs[AD_ACC_ROLL] - rollAxisAccOffset; |
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL]; |
#else |
rollAxisAcc = -rollAxisAccOffset - sensorInputs[AD_ACC_ROLL]; |
acc[ROLL] = -accOffset[ROLL] - sensorInputs[AD_ACC_ROLL]; |
#endif |
filteredRollAxisAcc = (filteredRollAxisAcc * (ACC_FILTER-1) + rollAxisAcc) / ACC_FILTER; |
measureNoise(rollAxisAcc, &rollAccNoisePeak, 1); |
filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER; |
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1); |
break; |
case 13: // air pressure |
if (sensorInputs[AD_AIRPRESSURE] < ADCENTER-HALFRANGE) { |
if (pressure_wait) { |
// A range switch was done recently. Wait for steadying. |
pressure_wait--; |
break; |
} |
range = OCR0A; |
rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
if (rawAirPressure < MIN_RAWPRESSURE) { |
// 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) { |
range -= (MAX_RAWPRESSURE - rawAirPressure) / rangewidth - 1; |
if (range < 0) range = 0; |
pressure_wait = (OCR0A - range) * 4; |
OCR0A = range; |
} else if (rawAirPressure > MAX_RAWPRESSURE) { |
// 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) |
range += (rawAirPressure - MIN_RAWPRESSURE) / rangewidth - 1; |
if (range > 254) range = 254; |
pressure_wait = (range - OCR0A) * 4; |
OCR0A = range; |
} else { |
filteredAirPressure = filterAirPressure(getAbsPressure(sensorInputs[AD_AIRPRESSURE])); |
filteredAirPressure = filterAirPressure(getAbsPressure(rawAirPressure)); |
} |
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. |
pitchGyroFilter = (pitchGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawPitchGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER; |
// Offset to 0. |
#ifdef GYROS_REVERSE_PITCH |
tempOffsetGyro = pitchOffset - pitchGyroFilter; |
#else |
tempOffsetGyro = pitchGyroFilter - pitchOffset; |
#endif |
// Calculate the delta from last shot and filter it. |
pitchGyroD = (pitchGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResPitchGyro)) / GYROS_DFILTER; |
// How we can overwrite the last value. This value is used for the D part of the PID controller. |
hiResPitchGyro = tempOffsetGyro; |
// Filter a little more. This value is used in integration to angles. |
filteredHiResPitchGyro = (filteredHiResPitchGyro * (GYROS_SECONDORDERFILTER-1) + hiResPitchGyro) / GYROS_SECONDORDERFILTER; |
measureNoise(hiResPitchGyro, &pitchGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING); |
DebugOut.Analog[12] = range; |
DebugOut.Analog[13] = rawAirPressure; |
DebugOut.Analog[14] = filteredAirPressure; |
break; |
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 |
tempOffsetGyro = rollOffset - rollGyroFilter; |
#else |
tempOffsetGyro = rollGyroFilter - rollOffset; |
#endif |
rollGyroD = (rollGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResRollGyro)) / GYROS_DFILTER; |
hiResRollGyro = tempOffsetGyro; |
filteredHiResRollGyro = (filteredHiResRollGyro * (GYROS_SECONDORDERFILTER-1) + hiResRollGyro) / GYROS_SECONDORDERFILTER; |
measureNoise(hiResRollGyro, &rollGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING); |
case 14: |
case 15: // pitch or roll gyro. |
axis = state - 15; |
tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis]; |
// DebugOut.Analog[6 + 3 * axis ] = tempGyro; |
/* |
* Process the gyro data for the PID controller. |
*/ |
// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
// gyro with a wider range, and helps counter saturation at full control. |
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
if (tempGyro < SENSOR_MIN_PITCHROLL) { |
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
} |
else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL; |
} |
} |
// 2) Apply sign and offset, scale before filtering. |
if (GYROS_REVERSE[axis]) { |
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
} else { |
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
} |
// 3) Scale and filter. |
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PIDFILTER-1) + tempOffsetGyro) / GYROS_PIDFILTER; |
// 4) Measure noise. |
measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
// 5) Differential measurement. |
gyroD[axis] = (gyroD[axis] * (GYROS_DFILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_DFILTER; |
// 6) Done. |
gyro_PID[axis] = tempOffsetGyro; |
/* |
* Now process the data for attitude angles. |
*/ |
tempGyro = rawGyroSum[axis]; |
// 1) Apply sign and offset, scale before filtering. |
if (GYROS_REVERSE[axis]) { |
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
} else { |
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
} |
// 2) Filter. |
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_INTEGRALFILTER-1) + tempOffsetGyro) / GYROS_INTEGRALFILTER; |
break; |
case 16: |
390,7 → 409,7 |
GYROS_DFILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1; |
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1; |
pitchOffset = rollOffset = yawOffset = 0; |
gyroOffset[PITCH] = gyroOffset[ROLL] = yawGyroOffset = 0; |
gyro_calibrate(); |
397,28 → 416,25 |
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
for(i=0; i < GYRO_OFFSET_CYCLES; i++) { |
Delay_ms_Mess(10); |
_pitchOffset += rawPitchGyroSum * GYRO_FACTOR_PITCHROLL; |
_rollOffset += rawRollGyroSum * GYRO_FACTOR_PITCHROLL; |
_pitchOffset += rawGyroSum[PITCH]; |
_rollOffset += rawGyroSum[ROLL]; |
_yawOffset += rawYawGyroSum; |
} |
pitchOffset = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
rollOffset = (_rollOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
yawOffset = (_yawOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
gyroOffset[PITCH] = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
gyroOffset[ROLL] = (_rollOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
yawGyroOffset = (_yawOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
filteredHiResPitchGyro = filteredHiResRollGyro = 0; |
gyro_PID[PITCH] = gyro_PID[ROLL] = 0; |
gyro_ATT[PITCH] = gyro_ATT[ROLL] = 0; |
pitchAxisAccOffset = (int16_t)GetParamWord(PID_ACC_NICK); |
rollAxisAccOffset = (int16_t)GetParamWord(PID_ACC_ROLL); |
ZAxisAccOffset = (int16_t)GetParamWord(PID_ACC_TOP); |
// Noise is relative to offset. So, reset noise measurements when |
// changing offsets. |
pitchGyroNoisePeak = rollGyroNoisePeak = 0; |
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
// Setting offset values has an influence in the analog.c ISR |
// Therefore run measurement for 100ms to achive stable readings |
Delay_ms_Mess(100); |
accOffset[PITCH] = (int16_t)GetParamWord(PID_ACC_PITCH); |
accOffset[ROLL] = (int16_t)GetParamWord(PID_ACC_ROLL); |
ZAccOffset = (int16_t)GetParamWord(PID_ACC_TOP); |
} |
/* |
432,22 → 448,54 |
#define ACC_OFFSET_CYCLES 10 |
uint8_t i; |
int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0; |
// int16_t pressureDiff, savedRawAirPressure; |
pitchAxisAccOffset = rollAxisAccOffset = ZAxisAccOffset = 0; |
accOffset[PITCH] = accOffset[ROLL] = ZAccOffset = 0; |
for(i=0; i < ACC_OFFSET_CYCLES; i++) { |
Delay_ms_Mess(10); |
_pitchAxisOffset += pitchAxisAcc; |
_rollAxisOffset += rollAxisAcc; |
_ZAxisOffset += ZAxisAcc; |
_pitchAxisOffset += acc[PITCH]; |
_rollAxisOffset += acc[ROLL]; |
_ZAxisOffset += ZAcc; |
} |
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_NICK, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES)); |
SetParamWord(PID_ACC_PITCH, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES)); |
SetParamWord(PID_ACC_ROLL, (uint16_t)((_rollAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES)); |
SetParamWord(PID_ACC_TOP, (uint16_t)((_ZAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES)); |
// Noise is relative to offset. So, reset noise measurements when |
// changing offsets. |
pitchAccNoisePeak = rollAccNoisePeak = 0; |
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
// Setting offset values has an influence in the analog.c ISR |
// Therefore run measurement for 100ms to achive stable readings |
// Delay_ms_Mess(100); |
// Set the feedback so that air pressure ends up in the middle of the range. |
// (raw pressure high --> OCR0A also high...) |
// OCR0A += (rawAirPressure - 512) / rangewidth; |
// Delay_ms_Mess(500); |
/* |
pressureDiff = 0; |
DebugOut.Analog[16] = rawAirPressure; |
#define PRESSURE_CAL_CYCLE_COUNT 2 |
for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
savedRawAirPressure = rawAirPressure; |
OCR0A++; |
Delay_ms_Mess(200); |
// raw pressure will decrease. |
pressureDiff += (savedRawAirPressure - rawAirPressure); |
savedRawAirPressure = rawAirPressure; |
OCR0A--; |
Delay_ms_Mess(200); |
// raw pressure will increase. |
pressureDiff += (rawAirPressure - savedRawAirPressure); |
} |
DebugOut.Analog[15] = rangewidth = |
(pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2); |
*/ |
} |
/branches/dongfang_FC_rewrite/analog.h |
---|
5,31 → 5,30 |
//#include "invenSense.h" |
#include "ENC-03_FC1.3.h" |
/* |
* How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro. |
* How much low pass filtering to apply for gyro_PID. |
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
* Temporarily replaced by userparam-configurable variable. |
*/ |
//#define GYROS_FIRSTORDERFILTER 2 |
#define GYROS_PIDFILTER 1 |
/* |
* How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro. |
* How much low pass filtering to apply for gyro_ATT. |
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
* Temporarily replaced by userparam-configurable variable. |
*/ |
//#define GYROS_SECONDORDERFILTER 2 |
#define GYROS_INTEGRALFILTER 1 |
// Temporarily replaced by userparam-configurable variable. |
//#define ACC_FILTER 4 |
/* |
About setting constants right for different gyros: |
About setting constants for different gyros: |
Main parameters are positive directions and voltage/angular speed gain. |
The "Positive direction" is the rotation direction around an axis where |
the corresponding gyro gives a voltage > the no-rotation voltage. |
the corresponding gyro outputs a voltage > the no-rotation voltage. |
A gyro is considered, in this code, to be "forward" if its positive |
direction is the same as in FC1.0/1.1/1.2/1.3, and reverse otherwise. |
direction is the same as in FC1.0-1.3, and reverse otherwise. |
Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
44,12 → 43,12 |
roll. |
So: |
HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
where V is 2 for FC1.0 and 4 for all others. |
Assuming constant ADCValue, in the H&I code: |
HiResXXXX = I * ADCValue. |
gyro = I * ADCValue. |
where I is 8 for FC1.0 and 16 for all others. |
79,16 → 78,17 |
(only about half as sensitive as V1.3. But it will take about twice the |
rotation rate!) |
All together: HiResXXXX = I * H * rotation rate [units / (deg/s)]. |
All together: gyro = I * H * rotation rate [units / (deg/s)]. |
*/ |
/* |
* A factor that the raw gyro values are multiplied by, |
* before being zero-offset, filtered and passed to the attitude module. |
* before being filtered and passed to the attitude module. |
* A value of 1 would cause a little bit of loss of precision in the |
* filtering (on the other hand the values are so noisy in flight that |
* it will not really matter - but when testing on the desk it might be |
* noticeable). 4 is fine for the default filtering. |
* Experiment: Set it to 1. |
*/ |
#define GYRO_FACTOR_PITCHROLL 4 |
111,12 → 111,12 |
constant is there. |
The control loop executes every 2ms, and for each iteration |
HiResXXXX is added to gyroIntegralXXXX. |
Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this |
explanation), we get: |
gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
Assuming a constant rotation rate v and a zero initial gyroIntegral |
(for this explanation), we get: |
gyroIntegralXXXX = |
N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR = |
gyroIntegral = |
N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
where N is the number of summations; N = t/2ms. |
134,7 → 134,7 |
*/ |
/* |
* The value of hiResXXXX for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
* 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) |
141,17 → 141,38 |
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
/* |
* Gyro saturation prevention. |
*/ |
// 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) |
// Max. boost to add "virtually" to gyro signal at total saturation. |
#define EXTRAPOLATION_LIMIT 2500 |
// Slope of the boost (calculated) |
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL) |
/* |
* This value is subtracted from the gyro noise measurement in each iteration, |
* making it return towards zero. |
*/ |
#define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
#define PITCH 0 |
#define ROLL 1 |
/* |
* The values that this module outputs |
* These first 2 exported arrays are zero-offset. The "PID" ones are used |
* in the attitude control as rotation rates. The "ATT" ones are for |
* integration to angles. For the same axis, the PID and ATT variables |
* generally have about the same values. There are just some differences |
* in filtering, and when a gyro becomes near saturated. |
* Maybe this distinction is not really necessary. |
*/ |
extern volatile int16_t hiResPitchGyro, hiResRollGyro; |
extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro; |
extern volatile int16_t pitchGyroD, rollGyroD; |
extern volatile int16_t gyro_PID[2]; |
extern volatile int16_t gyro_ATT[2]; |
extern volatile int16_t gyroD[2]; |
extern volatile uint16_t ADCycleCount; |
extern volatile int16_t UBat; |
extern volatile int16_t yawGyro; |
159,25 → 180,14 |
/* |
* This is not really for external use - but the ENC-03 gyro modules needs it. |
*/ |
extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum; |
extern volatile int16_t rawGyroSum[2], rawYawGyroSum; |
/* |
* The acceleration values that this module outputs |
* The acceleration values that this module outputs. They are zero based. |
*/ |
extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc; |
extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc; |
extern volatile int16_t acc[2], ZAcc; |
extern volatile int16_t filteredAcc[2]; |
// Only for debugging! Not to be exported! Remove when finished. |
// extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset; |
// Air pressure measurement not supported right now. |
// extern volatile int32_t AirPressure; |
// extern volatile int16_t HeightD; |
// extern volatile uint16_t ReadingAirPressure; |
// extern volatile int16_t StartAirPressure; |
// extern uint8_t PressureSensorOffset; |
// extern int8_t ExpandBaro; |
/* |
* Flag: Interrupt handler has done all A/D conversion and processing. |
*/ |
186,8 → 196,8 |
// Diagnostics: Gyro noise level because of motor vibrations. The variables |
// only really reflect the noise level when the copter stands still but with |
// its motors running. |
extern volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak; |
extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak; |
extern volatile uint16_t gyroNoisePeak[2]; |
extern volatile uint16_t accNoisePeak[2]; |
// void SearchAirPressureOffset(void); |
/branches/dongfang_FC_rewrite/attitude.c |
---|
84,11 → 84,11 |
* The variables are overwritten at each attitude calculation invocation - the values |
* are not preserved or reused. |
*/ |
int16_t pitchRate, rollRate, yawRate; |
int16_t rate[2], yawRate; |
// With different (less) filtering |
int16_t pitchRate_PID, rollRate_PID; |
int16_t pitchDifferential, rollDifferential; |
int16_t rate_PID[2]; |
int16_t differential[2]; |
/* |
* Gyro readings, after performing "axis coupling" - that is, the transfomation |
99,13 → 99,13 |
* The variables are overwritten at each attitude calculation invocation - the values |
* are not preserved or reused. |
*/ |
int16_t ACPitchRate, ACRollRate, ACYawRate; |
int16_t ACRate[2], ACYawRate; |
/* |
* Gyro integrals. These are the rotation angles of the airframe compared to the |
* horizontal plane, yaw relative to yaw at start. |
*/ |
int32_t pitchAngle, rollAngle, yawAngle; |
int32_t angle[2], yawAngle; |
int readingHeight = 0; |
124,12 → 124,12 |
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
int32_t pitchCorrectionSum = 0, rollCorrectionSum = 0; |
int32_t correctionSum[2] = {0,0}; |
/* |
* Experiment: Compensating for dynamic-induced gyro biasing. |
*/ |
int16_t dynamicOffsetPitch = 0, dynamicOffsetRoll = 0, dynamicOffsetYaw = 0; |
int16_t dynamicOffset[2] = {0,0}, dynamicOffsetYaw = 0; |
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
// int16_t dynamicCalCount; |
144,21 → 144,16 |
* it is hardly worth the trouble. |
************************************************************************/ |
int32_t getPitchAngleEstimateFromAcc(void) { |
return GYRO_ACC_FACTOR * (int32_t)filteredPitchAxisAcc; |
int32_t getAngleEstimateFromAcc(uint8_t axis) { |
return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis]; |
} |
int32_t getRollAngleEstimateFromAcc(void) { |
return GYRO_ACC_FACTOR * (int32_t)filteredRollAxisAcc; |
} |
void setStaticAttitudeAngles(void) { |
#ifdef ATTITUDE_USE_ACC_SENSORS |
pitchAngle = getPitchAngleEstimateFromAcc(); |
rollAngle = getRollAngleEstimateFromAcc(); |
angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
#else |
pitchAngle = 0; |
rollAngle = 0; |
angle[PITCH] = angle[ROLL] = 0; |
#endif |
} |
169,13 → 164,13 |
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
dynamicOffsetPitch = dynamicOffsetRoll = 0; |
dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0; |
// Calibrate hardware. |
analog_calibrate(); |
// reset gyro readings |
pitchRate = rollRate = yawRate = 0; |
rate[PITCH] = rate[ROLL] = yawRate = 0; |
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
193,70 → 188,26 |
* Get sensor data from the analog module, and release the ADC |
* TODO: Ultimately, the analog module could do this (instead of dumping |
* the values into variables). |
* The rate variable end up in a range of about [-1024, 1023]. |
* When scaled down by CONTROL_SCALING, the interval is about [-256, 256]. |
*************************************************************************/ |
void getAnalogData(void) { |
// For the differential calculation. Diff. is not supported right now. |
// int16_t d2Pitch, d2Roll; |
pitchRate_PID = (hiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR; |
pitchRate = (filteredHiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR; |
pitchDifferential = pitchGyroD; |
uint8_t axis; |
rollRate_PID = (hiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR; |
rollRate = (filteredHiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR; |
rollDifferential = rollGyroD; |
for (axis=PITCH; axis <=ROLL; axis++) { |
rate_PID[axis] = (gyro_PID[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
rate[axis] = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
differential[axis] = gyroD[axis]; |
} |
yawRate = yawGyro + dynamicOffsetYaw; |
// We are done reading variables from the analog module. Interrupt-driven sensor reading may restart. |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
analogDataReady = 0; |
analog_start(); |
} |
/************************************************************************ |
* Axis coupling, H&I Style |
* Currently not working (and there is a bug in it, |
* which causes unstable flight in heading-hold mode). |
************************************************************************/ |
void H_and_I_axisCoupling(void) { |
int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0; |
int16_t CouplingNickRoll = 0, CouplingRollNick = 0; |
tmp13 = (rollRate * pitchAngle) / 2048L; |
tmp13 *= dynamicParams.AxisCoupling2; // 65 |
tmp13 /= 4096L; |
CouplingNickRoll = tmp13; |
tmp14 = (pitchRate * rollAngle) / 2048L; |
tmp14 *= dynamicParams.AxisCoupling2; // 65 |
tmp14 /= 4096L; |
CouplingRollNick = tmp14; |
tmp14 -= tmp13; |
ACYawRate = yawRate + tmp14; |
/* |
if(!dynamicParams.AxisCouplingYawCorrection) ACYawRate = yawRate - tmp14 / 2; // force yaw |
else ACYawRate |
*/ |
tmpl = ((yawRate + tmp14) * pitchAngle) / 2048L; |
tmpl *= dynamicParams.AxisCoupling1; |
tmpl /= 4096L; |
tmpl2 = ((yawRate + tmp14) * rollAngle) / 2048L; |
tmpl2 *= dynamicParams.AxisCoupling1; |
tmpl2 /= 4096L; |
// if(abs(yawRate > 64)) { |
// if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
// } |
ACPitchRate = pitchRate - tmpl2 + tmpl / 100L; |
ACRollRate = rollRate + tmpl - tmpl2 / 100L; |
} |
/* |
* This is the standard flight-style coordinate system transformation |
* (from airframe-local axes to a ground-based system). For some reason |
* the MK uses a left-hand coordinate system. The tranformation has been |
263,49 → 214,38 |
* changed accordingly. |
*/ |
void trigAxisCoupling(void) { |
int16_t cospitch = int_cos(pitchAngle); |
int16_t cosroll = int_cos(rollAngle); |
int16_t sinroll = int_sin(rollAngle); |
int16_t tanpitch = int_tan(pitchAngle); |
int16_t cospitch = int_cos(angle[PITCH]); |
int16_t cosroll = int_cos(angle[ROLL]); |
int16_t sinroll = int_sin(angle[ROLL]); |
int16_t tanpitch = int_tan(angle[PITCH]); |
#define ANTIOVF 1024 |
ACPitchRate = ((int32_t)pitchRate * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
ACRollRate = rollRate + (((int32_t)pitchRate * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(rollAngle) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
ACYawRate = ((int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
ACRate[PITCH] = ((int32_t) rate[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
ACRate[ROLL] = rate[ROLL] + (((int32_t)rate[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
ACYawRate = ((int32_t) rate[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
} |
void integrate(void) { |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
// The rotary rate limiter bit is abused for selecting axis coupling algorithm instead. |
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) |
trigAxisCoupling(); |
else |
H_and_I_axisCoupling(); |
} else { |
ACPitchRate = pitchRate; |
ACRollRate = rollRate; |
ACRate[PITCH] = rate[PITCH]; |
ACRate[ROLL] = rate[ROLL]; |
ACYawRate = yawRate; |
} |
DebugOut.Analog[3] = pitchRate; |
// DebugOut.Analog[3 + 3] = ACPitchRate; |
DebugOut.Analog[4] = rollRate; |
// DebugOut.Analog[4 + 3] = ACRollRate; |
DebugOut.Analog[5] = yawRate; |
// DebugOut.Analog[5 + 3] = ACYawRate; |
DebugOut.Analog[3] = ACRate[PITCH]; |
DebugOut.Analog[4] = ACRate[ROLL]; |
DebugOut.Analog[5] = ACYawRate; |
/* |
DebugOut.Analog[9] = int_cos(pitchAngle); |
DebugOut.Analog[10] = int_sin(pitchAngle); |
DebugOut.Analog[11] = int_tan(pitchAngle); |
*/ |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
yawGyroHeading += ACYawRate; |
// Why is yawAngle not wrapped 'round? |
320,21 → 260,13 |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
pitchAngle += ACPitchRate; |
if(pitchAngle > PITCHROLLOVER180) { |
pitchAngle -= PITCHROLLOVER360; |
} else if (pitchAngle <= -PITCHROLLOVER180) { |
pitchAngle += PITCHROLLOVER360; |
for (axis=PITCH; axis<=ROLL; axis++) { |
angle[axis] += ACRate[axis]; |
if(angle[axis] > PITCHROLLOVER180) { |
angle[axis] -= PITCHROLLOVER360; |
} else if (angle[axis] <= -PITCHROLLOVER180) { |
angle[axis] += PITCHROLLOVER360; |
} |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
rollAngle += ACRollRate; |
if(rollAngle > PITCHROLLOVER180) { |
rollAngle -= PITCHROLLOVER360; |
} else if (rollAngle <= -PITCHROLLOVER180) { |
rollAngle += PITCHROLLOVER360; |
} |
} |
349,17 → 281,16 |
// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
// are less than ....., or reintroduce Kalman. |
// Well actually the Z axis acc. check is not so silly. |
if(!looping && //((ZAxisAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction |
ZAxisAcc >= -dynamicParams.UserParams[7] && ZAxisAcc <= dynamicParams.UserParams[7]) { |
uint8_t axis; |
if(!looping && //((ZAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction |
ZAcc >= -dynamicParams.UserParams[7] && ZAcc <= dynamicParams.UserParams[7]) { |
DebugOut.Digital[0] = 1; |
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
uint8_t debugFullWeight = 1; |
int32_t accDerived[2]; |
int32_t accDerivedPitch = getPitchAngleEstimateFromAcc(); |
int32_t accDerivedRoll = getRollAngleEstimateFromAcc(); |
if((maxControlPitch > 64) || (maxControlRoll > 64)) { // reduce effect during stick commands |
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
372,12 → 303,13 |
/* |
* Add to each sum: The amount by which the angle is changed just below. |
*/ |
pitchCorrectionSum += permilleAcc * (accDerivedPitch - pitchAngle); |
rollCorrectionSum += permilleAcc * (accDerivedRoll - rollAngle); |
for (axis=PITCH; axis<=ROLL; axis++) { |
accDerived[axis] = getAngleEstimateFromAcc(axis); |
correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]); |
// There should not be a risk of overflow here, since the integrals do not exceed a few 100000. |
pitchAngle = ((int32_t)(1000 - permilleAcc) * pitchAngle + (int32_t)permilleAcc * accDerivedPitch) / 1000L; |
rollAngle = ((int32_t)(1000 - permilleAcc) * rollAngle + (int32_t)permilleAcc * accDerivedRoll) / 1000L; |
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L; |
} |
DebugOut.Digital[1] = debugFullWeight; |
} else { |
400,22 → 332,18 |
void driftCompensation(void) { |
static int16_t timer = DRIFTCORRECTION_TIME; |
int16_t deltaCompensation; |
uint8_t axis; |
if (! --timer) { |
timer = DRIFTCORRECTION_TIME; |
deltaCompensation = ((pitchCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME); |
for (axis=PITCH; axis<=ROLL; axis++) { |
deltaCompensation = ((correctionSum[axis] + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME); |
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp); |
dynamicOffsetPitch += deltaCompensation / staticParams.GyroAccTrim; |
deltaCompensation = ((rollCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME); |
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp); |
dynamicOffsetRoll += deltaCompensation / staticParams.GyroAccTrim; |
pitchCorrectionSum = rollCorrectionSum = 0; |
DebugOut.Analog[28] = dynamicOffsetPitch; |
DebugOut.Analog[29] = dynamicOffsetRoll; |
dynamicOffset[axis] += deltaCompensation / staticParams.GyroAccTrim; |
correctionSum[axis] = 0; |
DebugOut.Analog[28 + axis] = dynamicOffset; |
} |
} |
} |
/************************************************************************ |
* Main procedure. |
/branches/dongfang_FC_rewrite/attitude.h |
---|
1,6 → 1,6 |
/*####################################################################################### |
Attitude sense system (processing of gyro, accelerometer and altimeter data) |
#######################################################################################*/ |
/*********************************************************************************/ |
/* Attitude sense system (processing of gyro, accelerometer and altimeter data) */ |
/*********************************************************************************/ |
#ifndef _ATTITUDE_H |
#define _ATTITUDE_H |
33,7 → 33,7 |
* Gyro readings are divided by this before being used in attitude control. This will scale them |
* to match the scale of the stick control etc. variables. This is just a rough non-precision |
* scaling - the below definitions make precise conversion factors. |
* Will be about 4 for InvenSense, 8 for FC1.3 and 13 for ADXRS610 (hmmm - originally is was only 8???) |
* Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610. |
* The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
*/ |
#define HIRES_GYRO_INTEGRATION_FACTOR (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
81,13 → 81,13 |
/* |
* Rotation rates |
*/ |
extern int16_t pitchRate_PID, rollRate_PID, yawRate; |
extern int16_t pitchDifferential, rollDifferential; |
extern int16_t rate_PID[2], yawRate; |
extern int16_t differential[2]; |
/* |
* Attitudes calcualted by numerical integration of gyro rates |
* Attitudes calculated by numerical integration of gyro rates |
*/ |
extern int32_t pitchAngle, rollAngle, yawAngle; |
extern int32_t angle[2], yawAngle; |
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
104,12 → 104,6 |
extern uint16_t badCompassHeading; |
/* |
* Height control |
*/ |
extern int readingHeight; |
extern int setPointHeight; |
/* |
* Interval wrap-over values for attitude integrals |
*/ |
extern long turnOver180Pitch, turnOver180Roll; |
127,8 → 121,7 |
* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
* - Invent your own... |
*/ |
extern int16_t dynamicOffsetPitch, dynamicOffsetRoll, dynamicOffsetYaw; |
// extern int16_t savedDynamicOffsetPitch, savedDynamicOffsetRoll; |
extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
/* |
* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
/branches/dongfang_FC_rewrite/control.c |
---|
259,14 → 259,14 |
if(stickThrottle < 0) stickThrottle = 0; |
if(abs(stickPitch / STICK_GAIN) > maxStickPitch) { |
maxStickPitch = abs(stickPitch) / STICK_GAIN; |
if(abs(stickPitch / CONTROL_SCALING) > maxStickPitch) { |
maxStickPitch = abs(stickPitch) / CONTROL_SCALING; |
if(maxStickPitch > 100) maxStickPitch = 100; |
} |
else if (maxStickPitch) maxStickPitch--; |
if(abs(stickRoll / STICK_GAIN) > maxStickRoll) { |
maxStickRoll = abs(stickRoll) / STICK_GAIN; |
if(abs(stickRoll / CONTROL_SCALING) > maxStickRoll) { |
maxStickRoll = abs(stickRoll) / CONTROL_SCALING; |
if(maxStickRoll > 100) maxStickRoll = 100; |
} |
else if (maxStickRoll) maxStickRoll--; |
/branches/dongfang_FC_rewrite/control.h |
---|
1,6 → 1,6 |
/*####################################################################################### |
Stick control interface |
#######################################################################################*/ |
/*********************************************************************************/ |
/* Stick control interface */ |
/*********************************************************************************/ |
/* |
OBSOLETED BY controlMixer.h. But this is how it looked - maybe somebody will find it simpler? |
8,7 → 8,7 |
#define _CONTROL_H |
#include <inttypes.h> |
#define STICK_GAIN 4 |
#define CONTROL_SCALING 4 |
// defines for lookup staticParams.ChannelAssignment |
#define CH_PITCH 0 |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
63,8 → 63,8 |
*/ |
#define COMMAND_TIMER 200 |
uint16_t maxControlPitch = 0, maxControlRoll = 0; |
int16_t controlPitch = 0, controlRoll = 0, controlYaw = 0, controlThrottle = 0; |
uint16_t maxControl[2] = {0,0}; |
int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0; |
uint8_t looping = 0; |
// Internal variables for reading commands made with an R/C stick. |
128,8 → 128,6 |
uint8_t controlMixer_getSignalQuality(void) { |
uint8_t rcQ = RC_getSignalQuality(); |
uint8_t ecQ = EC_getSignalQuality(); |
DebugOut.Analog[16] = rcQ; |
DebugOut.Analog[17] = ecQ; |
// This needs not be the only correct solution... |
return rcQ > ecQ ? rcQ : ecQ; |
} |
140,6 → 138,7 |
void controlMixer_update(void) { |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
// TODO: If no signal --> zero. |
uint8_t axis; |
RC_update(); |
EC_update(); |
146,15 → 145,11 |
int16_t* RC_PRTY = RC_getPRTY(); |
int16_t* EC_PRTY = EC_getPRTY(); |
controlPitch = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
controlRoll = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
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(); |
configuration_applyVariablesToParams(); |
178,15 → 173,12 |
/* |
* Record maxima |
*/ |
if(abs(controlPitch / STICK_GAIN) > maxControlPitch) { |
maxControlPitch = abs(controlPitch) / STICK_GAIN; |
if(maxControlPitch > 100) maxControlPitch = 100; |
} else if (maxControlPitch) maxControlPitch--; |
if(abs(controlRoll / STICK_GAIN) > maxControlRoll) { |
maxControlRoll = abs(controlRoll) / STICK_GAIN; |
if(maxControlRoll > 100) maxControlRoll = 100; |
for (axis=PITCH; axis<=ROLL; axis++) { |
if(abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
if(maxControl[axis] > 100) maxControl[axis] = 100; |
} else if (maxControl[axis]) maxControl[axis]--; |
} |
else if (maxControlRoll) maxControlRoll--; |
// Here we could blend in other sources. |
uint8_t commandNow = RC_getCommand(); |
/branches/dongfang_FC_rewrite/controlMixer.h |
---|
93,9 → 93,9 |
/* |
* Our output. |
*/ |
extern int16_t controlPitch, controlRoll, controlYaw, controlThrottle; |
extern int16_t control[2], controlYaw, controlThrottle; |
// extern int16_t stickOffsetNick, stickOffsetRoll; |
extern uint16_t maxControlPitch, maxControlRoll; |
extern uint16_t maxControl[2]; |
extern uint8_t looping; |
void controlMixer_initVariables(void); |
117,7 → 117,12 |
uint8_t controlMixer_getSignalQuality(void); |
#define STICK_GAIN 4 |
/* |
* The motor throttles can be set in an [0..256[ interval. |
* The calculation of motor throttle values from sensor and control information (basically |
* flight.c) take place in an [0..256*CONTROL_SCALING[ interval for better precision. |
*/ |
#define CONTROL_SCALING 4 |
/* |
* Gets the argument for the current command (a number). |
/branches/dongfang_FC_rewrite/eeprom.h |
---|
8,7 → 8,7 |
#define PID_PARAM_REVISION 1 // byte |
#define PID_ACTIVE_SET 2 // byte |
#define PID_PRESSURE_OFFSET 3 // byte |
#define PID_ACC_NICK 4 // word |
#define PID_ACC_PITCH 4 // word |
#define PID_ACC_ROLL 6 // word |
#define PID_ACC_TOP 8 // word |
/branches/dongfang_FC_rewrite/flight.c |
---|
56,8 → 56,8 |
#include "flight.h" |
// Only for debug. Remove. |
#include "analog.h" |
#include "rc.h" |
//#include "analog.h" |
//#include "rc.h" |
// Necessary for external control and motor test |
#include "uart0.h" |
74,7 → 74,7 |
* These are no longer maintained, just left at 0. The original implementation just summed the acc. |
* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
*/ |
int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
// MK flags |
uint16_t isFlying = 0; |
128,9 → 128,6 |
dynamicParams.KalmanMaxFusion = 32; |
controlMixer_initVariables(); |
// TODO: Move off. |
// RC_Quality = 100; |
} |
/************************************************************************/ |
150,10 → 147,10 |
} |
/* |
DebugOut.Analog[12] = Motor[0].SetPoint; // Front |
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear |
DebugOut.Analog[14] = Motor[3].SetPoint; // Left |
DebugOut.Analog[15] = Motor[2].SetPoint; // Right |
DebugOut.Analog[] = Motor[0].SetPoint; // Front |
DebugOut.Analog[] = Motor[1].SetPoint; // Rear |
DebugOut.Analog[] = Motor[3].SetPoint; // Left |
DebugOut.Analog[] = Motor[2].SetPoint; // Right |
*/ |
// Start I2C Interrupt Mode |
I2C_Start(TWI_STATE_MOTOR_TX); |
248,12 → 245,11 |
void flight_control(void) { |
int16_t tmp_int; |
// Mixer Fractions that are combined for Motor Control |
int16_t yawTerm, throttleTerm, pitchTerm, rollTerm; |
int16_t yawTerm, throttleTerm, term[2]; |
// PID controller variables |
int16_t PDPartPitch, PDPartRoll, PDPartYaw, PPartPitch, PPartRoll; |
static int32_t IPartPitch = 0, IPartRoll = 0; |
int16_t PDPart[2], PDPartYaw, PPart[2]; |
static int32_t IPart[2] = {0,0}; |
static int32_t setPointYaw = 0; |
// Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
261,17 → 257,12 |
// static int32_t CorrectionPitch, CorrectionRoll; |
static uint16_t emergencyFlightTime; |
// No support for altitude control right now. |
// static uint8_t HeightControlActive = 0; |
// static int16_t HeightControlGas = 0; |
static int8_t debugDataTimer = 1; |
// High resolution motor values for smoothing of PID motor outputs |
static int16_t motorFilters[MAX_MOTORS]; |
uint8_t i; |
uint8_t i, axis; |
// Fire the main flight attitude calculation, including integration of angles. |
calculateFlightAttitude(); |
328,11 → 319,9 |
* When standing on the ground, do not apply I controls and zero the yaw stick. |
* Probably to avoid integration effects that will cause the copter to spin |
* or flip when taking off. |
* TODO: What was the value of IPartPitch? At 1st run of this, it's 0 already. |
*/ |
if(isFlying < 256) { |
IPartPitch = 0; |
IPartRoll = 0; |
IPart[PITCH] = IPart[ROLL] = 0; |
// TODO: Don't stomp on other modules' variables!!! |
controlYaw = 0; |
if(isFlying == 250) { |
399,9 → 388,6 |
*/ |
#if defined (USE_MK3MAG) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/************************************************************************/ |
/* GPS is currently not supported. */ |
/************************************************************************/ |
417,39 → 403,35 |
*/ |
#endif |
#define SENSOR_LIMIT (4096 * 4) |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
if(looping & LOOPING_PITCH_AXIS) { |
PPartPitch = 0; |
for (axis=PITCH; axis<=ROLL; axis++) { |
if(looping & (1<<(4+axis))) { |
PPart[axis] = 0; |
} else { // TODO: Where do the 44000 come from??? |
PPartPitch = pitchAngle * gyroIFactor / (44000 / STICK_GAIN); // P-Part - Proportional to Integral |
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
} |
// Now blend in the D-part - proportional to the Differential of the integral = the rate. |
PDPartPitch = PPartPitch + (int32_t)((int32_t)pitchRate_PID * gyroPFactor / (256L / STICK_GAIN)) |
+ (pitchDifferential * (int16_t)dynamicParams.GyroD) / 16; |
/* |
* Now blend in the D-part - proportional to the Differential of the integral = the rate. |
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
* where pfactor is in [0..1]. |
*/ |
PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
+ (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
// The P-part is actually the I-part... |
if(looping & LOOPING_ROLL_AXIS) { |
PPartRoll = 0; |
} else { // TODO: Where do the 44000 come from??? |
PPartRoll = (rollAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral |
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
} |
// Now blend in the P-part - proportional to the Differential of the integral = the rate |
PDPartRoll = PPartRoll + (int32_t)((int32_t)rollRate_PID * gyroPFactor / (256L / STICK_GAIN)) |
+ (rollDifferential * (int16_t)dynamicParams.GyroD) / 16; |
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
+ (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / STICK_GAIN) |
+ (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / STICK_GAIN)); |
// limit control feedback |
#define SENSOR_LIMIT (4096 * 4) |
CHECK_MIN_MAX(PDPartPitch, -SENSOR_LIMIT, SENSOR_LIMIT); |
CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT); |
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
/* |
467,33 → 449,30 |
* Height control was here. |
*/ |
if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
throttleTerm *= STICK_GAIN; |
throttleTerm *= CONTROL_SCALING; |
/* |
* Compose yaw term. |
* The yaw term is limited: Absolute value is max. = the throttle term / 2. |
* However, at low throttle the yaw term is limited to a fixed value, |
* and at high throttle it is limited by the throttle reserve (the difference |
* between current throttle and maximum throttle). |
*/ |
#define MIN_YAWGAS (40 * STICK_GAIN) // yaw also below this gas value |
yawTerm = PDPartYaw - setPointYaw * STICK_GAIN; |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
yawTerm = PDPartYaw - setPointYaw * CONTROL_SCALING; |
// limit yawTerm |
if(throttleTerm > MIN_YAWGAS) { |
/* |
* -throttle/2 < -20 <= yawTerm <= 20 < throttle/2 |
*/ |
CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
} else { |
/* |
* -20 <= yawTerm <= 20 |
*/ |
CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
tmp_int = staticParams.MaxThrottle * STICK_GAIN; |
/* |
* throttle-MaxThrottle <= yawTerm <= MaxThrottle-throttle |
*/ |
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
for (axis=PITCH; axis<=ROLL; axis++) { |
/* |
* Compose pitch and roll terms. This is finally where the sticks come into play. |
*/ |
501,23 → 480,18 |
// Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
// That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
// TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
IPartPitch += PPartPitch - controlPitch; // Integrate difference between P part (the angle) and the stick pos. |
IPartRoll += PPartRoll - controlRoll; // I-part for attitude control OK |
IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
} else { |
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
IPartPitch += PDPartPitch - controlPitch; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
IPartRoll += PDPartRoll - controlRoll; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
// To keep up with a full stick PDPart should be about 156... |
IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
} |
// TODO: From which planet comes the 16000? |
CHECK_MIN_MAX(IPartPitch, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
pitchTerm = PDPartPitch - controlPitch + IPartPitch / Ki; // PID-controller for pitch |
term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
rollTerm = PDPartRoll - controlRoll + IPartRoll / Ki; // PID-controller for roll |
/* |
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
524,22 → 498,22 |
* (max. pitch or roll term is the throttle value). |
* TODO: Why a growing function of yaw? |
*/ |
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
CHECK_MIN_MAX(pitchTerm, -tmp_int, tmp_int); |
CHECK_MIN_MAX(rollTerm, -tmp_int, tmp_int); |
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i = 0; i < MAX_MOTORS; i++) { |
int16_t tmp; |
if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered. |
tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
tmp += ((int32_t)pitchTerm * Mixer.Motor[i][MIX_PITCH]) / 64L; |
tmp += ((int32_t)rollTerm * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
tmp += ((int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
tmp = motorFilters[i] / STICK_GAIN; |
tmp = motorFilters[i] / CONTROL_SCALING; |
CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
Motor[i].SetPoint = tmp; |
} |
547,21 → 521,21 |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// Debugging |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!(--debugDataTimer)) { |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
DebugOut.Analog[0] = (10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
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[9] = setPointYaw; |
DebugOut.Analog[10] = yawIFactor; |
DebugOut.Analog[11] = gyroIFactor; |
// 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[14] = RC_getVariable(4); |
DebugOut.Analog[15] = dynamicParams.UserParams[4]; |
// DebugOut.Analog[14] = RC_getVariable(4); |
// DebugOut.Analog[15] = dynamicParams.UserParams[4]; |
/* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */ |
// 12..15 are the controls. |
570,17 → 544,18 |
// DebugOut.Analog[18] = ZAxisAcc; |
DebugOut.Analog[19] = throttleTerm; |
DebugOut.Analog[20] = pitchTerm; |
DebugOut.Analog[21] = rollTerm; |
DebugOut.Analog[20] = term[PITCH]; |
DebugOut.Analog[21] = term[ROLL]; |
DebugOut.Analog[22] = yawTerm; |
DebugOut.Analog[23] = PPartPitch; // |
DebugOut.Analog[24] = IPartPitch /Ki; // meget meget lille. |
DebugOut.Analog[25] = PDPartPitch; // omtrent lig ppart. |
DebugOut.Analog[26] = pitchAccNoisePeak; |
DebugOut.Analog[27] = rollAccNoisePeak; |
DebugOut.Analog[23] = PPart[PITCH]; // |
DebugOut.Analog[24] = IPart[PITCH] /Ki; // meget meget lille. |
DebugOut.Analog[25] = PDPart[PITCH]; // omtrent lig ppart. |
DebugOut.Analog[30] = pitchGyroNoisePeak; |
DebugOut.Analog[31] = rollGyroNoisePeak; |
DebugOut.Analog[26] = accNoisePeak[PITCH]; |
DebugOut.Analog[27] = accNoisePeak[ROLL]; |
DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
} |
} |
/branches/dongfang_FC_rewrite/flight.h |
---|
1,6 → 1,6 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
/*********************************************************************************/ |
/* Flight Control */ |
/*********************************************************************************/ |
#ifndef _FLIGHT_H |
#define _FLIGHT_H |
9,6 → 9,9 |
#include "analog.h" |
#include "configuration.h" |
#define PITCH 0 |
#define ROLL 1 |
extern uint8_t RequiredMotors; |
// looping params |
/branches/dongfang_FC_rewrite/invenSense.c |
---|
8,6 → 8,8 |
#define AUTOZERO_DDR DDRD |
#define AUTOZERO_BIT 5 |
const uint8_t GYROS_REVERSE[2] = {0,0}; |
void gyro_calibrate() { |
// If port not already set to output and high, do it. |
if (!(AUTOZERO_DDR & (1<<AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1<<AUTOZERO_BIT))) { |
/branches/dongfang_FC_rewrite/main.c |
---|
102,8 → 102,8 |
output_init(); |
timer0_init(); |
timer2_init(); |
USART0_Init(); |
if(CPUType == ATMEGA644P) USART1_Init(); |
usart0_Init(); |
if(CPUType == ATMEGA644P) usart1_Init(); |
RC_Init(); |
analog_init(); |
I2C_init(); |
263,10 → 263,10 |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
USART0_TransmitTxData(); |
usart0_TransmitTxData(); |
} |
USART0_ProcessRxData(); |
usart0_ProcessRxData(); |
if(CheckDelay(timer)) { |
if(UBat < staticParams.LowVoltageWarning) { |
/branches/dongfang_FC_rewrite/menu.c |
---|
150,8 → 150,8 |
*/ |
case 2:// Attitude Menu Item |
LCD_printfxy(0,0,"Attitude"); |
LCD_printfxy(0,1,"Nick: %5i", pitchAngle / GYRO_DEG_FACTOR_PITCHROLL); |
LCD_printfxy(0,2,"Roll: %5i", pitchAngle / GYRO_DEG_FACTOR_PITCHROLL); |
LCD_printfxy(0,1,"Nick: %5i", angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL); |
LCD_printfxy(0,2,"Roll: %5i", angle[ROLL ] / GYRO_DEG_FACTOR_PITCHROLL); |
LCD_printfxy(0,3,"Heading: %5i", compassHeading); |
break; |
case 3:// Remote Control Channel Menu Item |
/branches/dongfang_FC_rewrite/rc.c |
---|
56,10 → 56,6 |
#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]; |
122,7 → 118,6 |
SREG = sreg; |
} |
/********************************************************************/ |
/* Every time a positive edge is detected at PD6 */ |
/********************************************************************/ |
216,8 → 211,8 |
if(RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
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_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - stickOffsetPitch + RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - stickOffsetRoll + RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
280,8 → 275,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) * staticParams.StickP; |
stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
} |
} |
/branches/dongfang_FC_rewrite/sensors.h |
---|
1,14 → 1,7 |
extern const uint8_t GYROS_REVERSE[2]; |
/* |
* Common procedures for all gyro types. |
*/ |
/* |
* For InvenSense, set a port bit for the AUTO_ZERO line. |
* Moved to gyro_calibrate. |
*/ |
// void gyro_init(void); |
/* |
* FC 1.3 hardware: Searching the DAC values that return neutral readings. |
* FC 2.0 hardware: Nothing to do. |
* InvenSense hardware: Output a pulse on the AUTO_ZERO line. |
15,4 → 8,7 |
*/ |
void gyro_calibrate(void); |
/* |
* Set some default FC parameters, depending on gyro type: Drift correction etc. |
*/ |
void gyro_setDefaults(void); |
/branches/dongfang_FC_rewrite/spi.c |
---|
59,7 → 59,7 |
#include "timer0.h" |
#include "analog.h" |
#include "attitude.h" |
#include "GPScontrol.h" |
#include "GPSControl.h" |
#include "flight.h" |
//----------------------------------------- |
200,15 → 200,15 |
cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
// update content of packet to NaviCtrl |
ToNaviCtrl.IntegralNick = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.IntegralRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.IntegralNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
ToNaviCtrl.GyroNick = pitchRate_PID; // TODO: Which one should it be?? |
ToNaviCtrl.GyroRoll = rollRate_PID; |
ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be?? |
ToNaviCtrl.GyroRoll = rate_PID[ROLL]; |
ToNaviCtrl.GyroYaw = yawRate; |
ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
// naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
switch(ToNaviCtrl.Command) { |
case SPI_CMD_USER: |
260,7 → 260,7 |
compassCalState = 0; |
} |
ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
ToNaviCtrl.Param.Int[1] = readingHeight; // at address of Byte 2 and 3 |
ToNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
/branches/dongfang_FC_rewrite/spi.h |
---|
4,11 → 4,8 |
//#include <util/delay.h> |
#include <inttypes.h> |
#define SPI_PROTOCOL_COMP 1 |
#define SPI_CMD_USER 10 |
#define SPI_CMD_STICK 11 |
#define SPI_CMD_MISC 12 |
15,8 → 12,7 |
#define SPI_CMD_PARAMETER1 13 |
#define SPI_CMD_VERSION 14 |
typedef struct |
{ |
typedef struct { |
uint8_t Sync1; |
uint8_t Sync2; |
uint8_t Command; |
28,8 → 24,7 |
int16_t GyroNick; |
int16_t GyroRoll; |
int16_t GyroYaw; |
union |
{ |
union { |
int8_t sByte[12]; |
uint8_t Byte[12]; |
int16_t Int[6]; |
39,15 → 34,12 |
uint8_t Chksum; |
} __attribute__((packed)) ToNaviCtrl_t; |
#define SPI_CMD_OSD_DATA 100 |
#define SPI_CMD_GPS_POS 101 |
#define SPI_CMD_GPS_TARGET 102 |
#define SPI_KALMAN 103 |
typedef struct |
{ |
typedef struct { |
uint8_t Command; |
int16_t GPSStickNick; |
int16_t GPSStickRoll; |
55,8 → 47,7 |
int16_t CompassHeading; |
int16_t Status; |
uint16_t BeepTime; |
union |
{ |
union { |
int8_t Byte[12]; |
int16_t Int[6]; |
int32_t Long[3]; |
66,8 → 57,7 |
} __attribute__((packed)) FromNaviCtrl_t; |
typedef struct |
{ |
typedef struct { |
uint8_t Major; |
uint8_t Minor; |
uint8_t Patch; |
74,13 → 64,10 |
uint8_t Compatible; |
} __attribute__((packed)) SPI_VersionInfo_t; |
extern ToNaviCtrl_t ToNaviCtrl; |
extern FromNaviCtrl_t FromNaviCtrl; |
typedef struct |
{ |
typedef struct { |
int8_t KalmanK; |
int8_t KalmanMaxDrift; |
int8_t KalmanMaxFusion; |
87,7 → 74,6 |
uint8_t SerialDataOkay; |
} __attribute__((packed)) NCData_t; |
extern uint8_t NCDataOkay; |
extern uint8_t NCSerialDataOkay; |
95,6 → 81,4 |
void SPI_StartTransmitPacket(void); |
void SPI_TransmitByte(void); |
#endif //_SPI_H |
/branches/dongfang_FC_rewrite/uart0.c |
---|
125,20 → 125,20 |
"AnglePitch ", //0 |
"AngleRoll ", |
"AngleYaw ", |
"GyroPitch ", |
"GyroRoll ", |
"GyroYaw ", //5 |
"ControlPitch ", |
"ControlRoll ", |
"ControlYaw ", |
"SetPointYaw ", |
"YawRateIFactor ", //10 |
"Gyro I Factor ", |
" ", |
" ", |
"R/C Var 4 ", |
"User Param 4 ", //15 |
"RCQuality ", |
"GyroPitch(AC) ", |
"GyroRoll(AC) ", |
"GyroYaw(AC) ", //5 |
"Pitch_Raw ", |
"HiResPitch_PID ", |
"PitchGyroOffset ", |
"Roll_Raw ", |
"HiResRoll_PID ", //10 |
"rollGyroOffset ", |
"Pressure range ", |
"Pressure A/D ", |
"Pressure Value ", |
"Press. rangewidz", //15 |
"Press. init A/D ", |
"Ext. Quality ", |
"Looping ", |
"throttleTerm ", |
159,7 → 159,7 |
/****************************************************************/ |
/* Initialization of the USART0 */ |
/****************************************************************/ |
void USART0_Init (void) { |
void usart0_Init (void) { |
uint8_t sreg = SREG; |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1); |
425,7 → 425,7 |
} |
// -------------------------------------------------------------------------- |
void USART0_ProcessRxData(void) { |
void usart0_ProcessRxData(void) { |
// if data in the rxd buffer are not locked immediately return |
if(!rxd_buffer_locked) return; |
585,9 → 585,9 |
rxd_buffer_locked = FALSE; |
} |
//############################################################################ |
//Routine für die Serielle Ausgabe |
//############################################################################ |
/************************************************************************/ |
/* Routine für die Serielle Ausgabe */ |
/************************************************************************/ |
int16_t uart_putchar (int8_t c) { |
if (c == '\n') |
uart_putchar('\r'); |
599,7 → 599,7 |
} |
//--------------------------------------------------------------------------------------------- |
void USART0_TransmitTxData(void) { |
void usart0_TransmitTxData(void) { |
if(!txd_complete) return; |
if(Request_VerInfo && txd_complete) { |
641,8 → 641,8 |
if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete) { |
SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D)); |
Data3D.AngleNick = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.AngleRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
Data3D_Timer = SetDelay(Data3D_Interval); |
Request_Data3D = FALSE; |
655,8 → 655,8 |
#ifdef USE_MK3MAG |
if((CheckDelay(Compass_Timer)) && txd_complete) { |
ToMk3Mag.Attitude[0] = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
ToMk3Mag.Attitude[1] = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
ToMk3Mag.CalState = CompassCalState; |
/branches/dongfang_FC_rewrite/uart0.h |
---|
11,9 → 11,9 |
//Baud rate of the USART |
#define USART0_BAUD 57600 |
extern void USART0_Init (void); |
extern void USART0_TransmitTxData(void); |
extern void USART0_ProcessRxData(void); |
extern void usart0_Init (void); |
extern void usart0_TransmitTxData(void); |
extern void usart0_ProcessRxData(void); |
extern int16_t uart_putchar(int8_t c); |
extern uint8_t RemotePollDisplayLine; |
/branches/dongfang_FC_rewrite/uart1.c |
---|
70,7 → 70,7 |
/****************************************************************/ |
/* Initialization of the USART1 */ |
/****************************************************************/ |
void USART1_Init (void) { |
void usart1_Init (void) { |
// USART1 Control and Status Register A, B, C and baud rate register |
uint8_t sreg = SREG; |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1); |
134,9 → 134,7 |
/****************************************************************/ |
/* USART1 data register empty ISR */ |
/****************************************************************/ |
/*ISR(USART1_UDRE_vect) |
{ |
/*ISR(USART1_UDRE_vect) { |
} |
*/ |
143,16 → 141,13 |
/****************************************************************/ |
/* USART1 transmitter ISR */ |
/****************************************************************/ |
/*ISR(USART1_TX_vect) |
{ |
/*ISR(USART1_TX_vect) { |
} |
*/ |
/****************************************************************/ |
/* USART1 receiver ISR */ |
/****************************************************************/ |
ISR(USART1_RX_vect) |
{ |
ISR(USART1_RX_vect) { |
uint8_t c; |
c = UDR1; // get data byte |
#if defined (USE_MK3MAG) |
/branches/dongfang_FC_rewrite/uart1.h |
---|
1,8 → 1,6 |
#ifndef _UART1_H |
#define _UART1_H |
extern void usart1_Init (void); |
extern void USART1_Init (void); |
#endif //_UART1_H |
/branches/dongfang_FC_rewrite/userparams.txt |
---|
1,10 → 1,10 |
User param 4: Throttle stick D value. |
Userparam 4: Throttle stick D value. Recommended: 12-15. Good fun to fly with. |
Userparam 5: Filter control. |
bits 0-1: Gyros 1st order |
bits 2-3: Gyros 2nd order |
bits 4-5: GyroD (in series with 1st order) |
bits 6-7: Acc. |
bits 0-1: Gyro signal for the flight control PID (filter constants 1-4) |
bits 2-3: Gyro signal for the attitude angle integration (ATT) (filter constants 1-4) |
bits 4-5: GyroD (filter constants 1-4) |
bits 6-7: Acc. (filter constants 1-4) |
Userparam6: Motor smoothing. |
0: No filter |
12,11 → 12,10 |
2: As H&I |
3: Reverse H&I |
Userparam7: Yaw I factor. Default 0 (may be nasty to fly!) |
Userparam7: Yaw I factor. Default 0 (may be nasty to fly!). Recommended: ca. 100. |
Userparam8: Acc integral correction Z axis limit. Default 0 (!) |
Other params: |
GyroAccFactor = 0'th order integral drift correction - acceleration sensor part as 1/1000s. |
Default 5 (ENC-03) |
25,7 → 24,11 |
DriftComp = Max offset correction per iteration (=per 1/2 second). |
This limits the value _before_ division by GyroAccTrim. Default 32 (ENC-03) |
Rotary rate limiter flag ON = dongfang axis coupling used. OFF = H&I axis coupling |
(with the modification that it does not affect the rate values). |
If axis coupling is off: No effect. |
The rotary rate limiter was removed. |
Rotary rate limiter flag ON = The gyro signals for flight control are extrapolated with |
a sharp rise in "virtual" rotation rates when the gyros are near saturation. This |
prevents (via the flight control) the copter from turning fast enough that the gyros |
saturate. Saturation will have the effect that stick controls overwhelm the (now clipped) |
gyro feedback in the flight control, and the copter flips VERY fast. Saturation is |
normally undesirable but it provides for a very entertaining stunt! |
The rotary rate limiter (beginner feature to make the copter _very_ irresponsive to |
stick controls) was removed. |