/branches/dongfang_FC_rewrite/analog.c |
---|
97,6 → 97,8 |
int16_t gyroD[2]; |
int16_t yawGyro; |
int32_t groundPressure; |
/* |
* 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 |
172,6 → 174,7 |
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
int32_t filteredAirPressure; |
int32_t lastFilteredAirPressure; |
#define MAX_AIRPRESSURE_WINDOW_LENGTH 64 |
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH]; |
506,6 → 509,7 |
// 2 samples were added. |
pressureMeasurementCount += 2; |
if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) { |
lastFilteredAirPressure = filteredAirPressure; |
filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
+ airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
pressureMeasurementCount = airPressureSum = 0; |
524,7 → 528,6 |
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
// This is divided by 3 --> 10.34 counts per volt. |
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
debugOut.analog[11] = UBat; |
} |
void analog_update(void) { |
633,3 → 636,16 |
accOffset_writeToEEProm(); |
startAnalogConversionCycle(); |
} |
void analog_setGround() { |
groundPressure = filteredAirPressure; |
} |
int32_t analog_getHeight(void) { |
return groundPressure - filteredAirPressure; |
} |
int16_t analog_getDHeight(void) { |
// dHeight = -dPressure, so here it is the old pressure minus the current, not opposite. |
return lastFilteredAirPressure - filteredAirPressure; |
} |
/branches/dongfang_FC_rewrite/analog.h |
---|
293,4 → 293,11 |
* Zero-offset accelerometers and write the calibration data to EEPROM. |
*/ |
void analog_calibrateAcc(void); |
void analog_setGround(); |
int32_t analog_getHeight(void); |
int16_t analog_getDHeight(void); |
#endif //_ANALOG_H |
/branches/dongfang_FC_rewrite/attitude.c |
---|
156,7 → 156,7 |
int32_t getAngleEstimateFromAcc(uint8_t axis) { |
//int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
return GYRO_ACC_FACTOR * filteredAcc[axis];// + correctionTerm; |
return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
// return 342L * filteredAcc[axis]; |
} |
332,7 → 332,7 |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
accDerived = getAngleEstimateFromAcc(axis); |
//debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
// 1000 * the correction amount that will be added to the gyro angle in next line. |
temp = angle[axis]; |
angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
340,18 → 340,12 |
correctionSum[axis] += angle[axis] - temp; |
} |
} else { |
//debugOut.analog[9] = 0; |
//debugOut.analog[10] = 0; |
debugOut.analog[9] = 0; |
debugOut.analog[10] = 0; |
// experiment: Kill drift compensation updates when not flying smooth. |
// correctionSum[PITCH] = correctionSum[ROLL] = 0; |
debugOut.digital[0] |= DEBUG_ACC0THORDER; |
} |
int32_t accDerived = getAngleEstimateFromAcc(0); |
debugOut.analog[9 + 0] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
accDerived = getAngleEstimateFromAcc(1); |
debugOut.analog[9 + 1] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
} |
/************************************************************************ |
/branches/dongfang_FC_rewrite/configuration.h |
---|
164,7 → 164,7 |
uint8_t heightI; |
uint8_t heightD; |
uint8_t heightSetting; |
uint8_t heightControlMaxIntegralThrottleChange; |
uint8_t heightControlMaxIntegral; |
uint8_t heightControlMaxThrottleChange; |
uint8_t heightSlewRate; |
215,7 → 215,7 |
#define ATMEGA644P 1 |
#define SYSCLK F_CPU |
// Not really a part of configuration, but LEDs and HW version test are the same. |
// Not really a part of configuration, but LEDs and HW s test are the same. |
#define RED_OFF {if((boardRelease == 10) || (boardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);} |
#define RED_ON {if((boardRelease == 10) || (boardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);} |
#define RED_FLASH PORTB ^= (1<<PORTB0) |
/branches/dongfang_FC_rewrite/eeprom.h |
---|
24,7 → 24,7 |
#define EEPROM_ADR_PARAMSET_BEGIN 256 |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 1 |
#define EEPARAM_REVISION 2 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
/branches/dongfang_FC_rewrite/heightControl.c |
---|
1,4 → 1,5 |
#include <inttypes.h> |
#include "analog.h" |
#include "attitude.h" |
#include "uart0.h" |
#include "configuration.h" |
13,7 → 14,6 |
#define LATCH_TIME 40 |
int32_t groundPressure; |
int32_t targetHeight; |
int32_t rampedTargetHeight; |
21,20 → 21,16 |
int32_t maxHeightThisFlight; |
int32_t iHeight; |
int32_t getHeight(void) { |
return groundPressure - filteredAirPressure; |
} |
void HC_setGround(void) { |
groundPressure = filteredAirPressure; |
analog_setGround(); |
// This should also happen when height control is enabled in-flight. |
rampedTargetHeight = getHeight(); |
rampedTargetHeight = targetHeight = analog_getHeight(); |
maxHeightThisFlight = 0; |
iHeight = 0; |
} |
void HC_update(void) { |
int32_t height = getHeight(); |
int32_t height = analog_getHeight(); |
static uint8_t setHeightLatch = 0; |
if (height > maxHeightThisFlight) |
47,7 → 43,7 |
if (setHeightLatch <= LATCH_TIME) { |
if (setHeightLatch == LATCH_TIME) { |
// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
targetHeight = height; |
rampedTargetHeight = targetHeight = height; |
} |
// Time not yet reached. |
setHeightLatch++; |
58,7 → 54,7 |
} |
} else { |
// Switch is not activated; take the "max-height" as the target height. |
targetHeight = (uint16_t) dynamicParams.heightSetting * 50L - 3000L; // should be: 100 (or make a param out of it) |
targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
} |
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
90,17 → 86,13 |
// takes 180-200 usec (with integral term). That is too heavy!!! |
// takes 100 usec without integral term. |
uint16_t HC_getThrottle(uint16_t throttle) { |
int32_t height = getHeight(); |
int32_t height = analog_getHeight(); |
int32_t heightError = rampedTargetHeight - height; |
static int32_t lastHeight; |
//static int32_t lastHeight; |
int16_t dHeight = analog_getDHeight(); |
//lastHeight = height; |
int16_t dHeight = height - lastHeight; |
lastHeight = height; |
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
iHeight += heightError; |
if (heightError > 0) { |
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
108,14 → 100,28 |
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
} |
int16_t dThrottle = (iHeight * staticParams.heightI) / 10000L; |
if (dThrottle > staticParams.heightControlMaxIntegralThrottleChange) |
dThrottle = staticParams.heightControlMaxIntegralThrottleChange; |
else if (dThrottle < -staticParams.heightControlMaxIntegralThrottleChange) |
dThrottle = -staticParams.heightControlMaxIntegralThrottleChange; |
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
iHeight += heightError; |
#define IHEIGHT_SCALE 24 |
// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
int16_t dThrottle = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE); |
// dt = (iHeight * staticParams.heightI) >> (IHEIGHT_SCALE) = staticParams.heightControlMaxIntegral |
// (iHeight * staticParams.heightI) = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) |
// iHeight = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) /staticParams.heightI |
if (dThrottle > staticParams.heightControlMaxIntegral) { |
dThrottle = staticParams.heightControlMaxIntegral; |
iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
} else if (dThrottle < -staticParams.heightControlMaxIntegral) { |
dThrottle = -staticParams.heightControlMaxIntegral; |
iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
} |
debugOut.analog[27] = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE); |
dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7); |
if (dThrottle > staticParams.heightControlMaxThrottleChange) |
126,7 → 132,6 |
debugOut.analog[19] = rampedTargetHeight; |
debugOut.analog[21] = dThrottle; |
debugOut.analog[26] = height; |
debugOut.analog[27] = (iHeight * staticParams.heightI) / 10000L; |
if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
/branches/dongfang_FC_rewrite/heightControl.h |
---|
1,3 → 1,2 |
void HC_setGround(void); |
void HC_update(void); |
uint16_t HC_getThrottle(uint16_t throttle); |
/branches/dongfang_FC_rewrite/makefile |
---|
8,8 → 8,8 |
VERSION_PATCH = 0 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
VERSION_SERIAL_MINOR = 2 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
#------------------------------------------------------------------- |
#OPTIONS |
/branches/dongfang_FC_rewrite/uart0.c |
---|
132,7 → 132,7 |
"AccZ ", |
"AccPitch (angle)", |
"AccRoll (angle) ", //10 |
"UBat ", |
"- ", |
"Pitch Term ", |
"Roll Term ", |
"Yaw Term ", |