Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1960 → Rev 1961

/branches/dongfang_FC_rewrite/analog.c
100,14 → 100,7
* standing still. They are used for adjusting the gyro and acc. meter values
* to be centered on zero.
*/
/*
volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512
* GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW };
 
volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512
* ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z };
*/
 
sensorOffset_t gyroOffset;
sensorOffset_t accOffset;
 
474,7 → 467,31
analog_updateBatteryVoltage();
}
 
void analog_calibrate(void) {
void analog_setNeutral() {
if (gyroOffset_readFromEEProm()) {
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL;
gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW;
}
 
if (accOffset_readFromEEProm()) {
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL;
accOffset.offsets[Z] = 512 * ACC_SUMMATION_FACTOR_Z;
}
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 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);
 
// Rough estimate. Hmm no nothing happens at calibration anyway.
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2);
// pressureMeasurementCount = 0;
}
 
void analog_calibrateGyros(void) {
#define GYRO_OFFSET_CYCLES 32
uint8_t i, axis;
int32_t deltaOffsets[3] = { 0, 0, 0 };
492,20 → 509,8
gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
debugOut.analog[6+axis] = gyroOffset.offsets[axis];
}
// Noise is relativ to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
accOffset_readFromEEProm();
// accOffset[PITCH] = getParamWord(PID_ACC_PITCH);
// accOffset[ROLL] = getParamWord(PID_ACC_ROLL);
// accOffset[Z] = getParamWord(PID_ACC_Z);
// Rough estimate. Hmm no nothing happens at calibration anyway.
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2);
// pressureMeasurementCount = 0;
delay_ms_Mess(100);
 
gyroOffset_writeToEEProm();
}
 
/*
534,18 → 539,6
/ ACC_OFFSET_CYCLES;
accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
}
// Save ACC neutral settings to eeprom
// setParamWord(PID_ACC_PITCH, accOffset[PITCH]);
// setParamWord(PID_ACC_ROLL, accOffset[ROLL]);
// setParamWord(PID_ACC_Z, accOffset[Z]);
 
accOffset_writeToEEProm();
 
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
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);
}
/branches/dongfang_FC_rewrite/analog.h
219,12 → 219,12
 
/*
* Air pressure.
* The sensor has a sensitivity of 46 mV/kPa.
* The sensor has a sensitivity of 45 mV/kPa.
* An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 11.95 * 10^-3 * h
* That is: dV = 46 mV * 11.95 * 10^-3 dh = 0.5497 mV / m.
* That is, with 2 * 1.024 / 3 steps per mV: 0.037 steps / m
* That is: dV = 45 mV * 11.95 * 10^-3 dh = 0.53775 mV / m.
* That is, with 1.024 / 3 steps per mV: 0.183552 steps / m
*/
#define AIRPRESSURE_SUMMATION_FACTOR 16
#define AIRPRESSURE_SUMMATION_FACTOR 54
#define AIRPRESSURE_FILTER 8
// Minimum A/D value before a range change is performed.
#define MIN_RAWPRESSURE (200 * 2)
261,12 → 261,17
void analog_update(void);
 
/*
* "Warm" calibration: Zero-offset gyros.
* Read gyro and acc.meter calibration from EEPROM.
*/
void analog_calibrate(void);
void analog_setNeutral(void);
 
/*
* "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
* Zero-offset gyros and write the calibration data to EEPROM.
*/
void analog_calibrateGyros(void);
 
/*
* Zero-offset accelerometers and write the calibration data to EEPROM.
*/
void analog_calibrateAcc(void);
#endif //_ANALOG_H
/branches/dongfang_FC_rewrite/attitude.c
179,7 → 179,7
correctionSum[PITCH] = correctionSum[ROLL] = 0;
 
// Calibrate hardware.
analog_calibrate();
analog_setNeutral();
 
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
/branches/dongfang_FC_rewrite/configuration.c
55,11 → 55,11
#include "sensors.h"
#include "rc.h"
 
int16_t variables[8] = {0,0,0,0,0,0,0,0};
int16_t variables[VARIABLE_COUNT];
paramset_t staticParams;
channelMap_t channelMap;
mixerMatrix_t mixerMatrix;
dynamicParam_t dynamicParams; // = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
dynamicParam_t dynamicParams;
uint8_t CPUType = ATMEGA644;
uint8_t boardRelease = 13;
uint8_t requiredMotors;
70,8 → 70,8
 
void configuration_applyVariablesToParams(void) {
uint8_t i;
#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}}
#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
#define SET_POT(b,a) { if (a<255) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a;}}
SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200);
SET_POT(dynamicParams.gyroI, staticParams.gyroI);
SET_POT(dynamicParams.gyroD, staticParams.gyroD);
104,11 → 104,11
}
 
const XLATION XLATIONS[] = {
// {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)},
{offsetof(paramset_t, heightSetting), offsetof(dynamicParam_t, heightSetting)},
};
 
const MMXLATION MMXLATIONS[] = {
// {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
{offsetof(paramset_t, heightD), offsetof(dynamicParam_t, heightD),0,100},
};
 
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
150,7 → 150,7
 
uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values
uint8_t CPUType = ATMEGA644;
if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P; // initial Values for 644P after reset
//if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P; // initial Values for 644P after reset
return CPUType;
}
 
202,9 → 202,8
staticParams.heightP = 10;
staticParams.heightD = 30;
staticParams.heightSetting = 251;
staticParams.heightMaxThrottleChange = 10;
staticParams.heightControlMaxThrottleChange = 10;
staticParams.heightSlewRate = 4;
staticParams.heightACCEffect = 30;
// Control
staticParams.stickP = 8;
225,7 → 224,7
staticParams.gyroI = 80;
staticParams.gyroD = 4;
// set by gyro code.
// set by gyro-specific code: gyro_setDefaults().
// staticParams.zerothOrderCorrection =
// staticParams.driftCompDivider =
// staticParams.driftCompLimit =
253,9 → 252,9
staticParams.emergencyFlightDuration = 30;
 
// Outputs
staticParams.outputFlash[0].bitmask = 0b01011111;
staticParams.outputFlash[0].bitmask = 1; //0b01011111;
staticParams.outputFlash[0].timing = 15;
staticParams.outputFlash[1].bitmask = 0b11110011;
staticParams.outputFlash[1].bitmask = 3; //0b11110011;
staticParams.outputFlash[1].timing = 15;
 
staticParams.outputFlags = 0;
273,9 → 272,7
}
 
staticParams.bitConfig =
CFG_GYRO_SATURATION_PREVENTION |
CFG_HEADING_HOLD;
staticParams.bitConfig2 = 0;
CFG_GYRO_SATURATION_PREVENTION | CFG_HEADING_HOLD;
 
memcpy(staticParams.name, "Default\0", 6);
}
/branches/dongfang_FC_rewrite/configuration.h
87,13 → 87,12
typedef struct {
// Global bitflags
uint8_t bitConfig; // see upper defines for bitcoding
uint8_t bitConfig2; // see upper defines for bitcoding
// Height Control
uint8_t heightP; // Value : 0-32
uint8_t heightD; // Value : 0-250
uint8_t heightSetting; // Value : 0-32
uint8_t heightMaxThrottleChange; // Value : 0-100
uint8_t heightControlMaxThrottleChange; // Value : 0-100
uint8_t heightSlewRate; // Value : 0-50
uint8_t heightACCEffect; // Value : 0-250
 
169,22 → 168,15
#define MKFLAG_RESERVE3 (1<<7)
 
// bit mask for staticParams.bitConfig
#define CFG_HEIGHT_CONTROL (1<<0)
#define CFG_HEIGHT_SWITCH (1<<1)
#define CFG_HEADING_HOLD (1<<2)
#define CFG_COMPASS_ACTIVE (1<<3)
#define CFG_COMPASS_FIX (1<<4)
#define CFG_GPS_ACTIVE (1<<5)
#define CFG_AXIS_COUPLING_ACTIVE (1<<6)
#define CFG_SIMPLE_HEIGHT_CONTROL (1<<0)
#define CFG_SIMPLE_HC_HOLD_SWITCH (1<<1)
#define CFG_HEADING_HOLD (1<<2)
#define CFG_COMPASS_ACTIVE (1<<3)
#define CFG_COMPASS_FIX (1<<4)
#define CFG_GPS_ACTIVE (1<<5)
#define CFG_AXIS_COUPLING_ACTIVE (1<<6)
#define CFG_GYRO_SATURATION_PREVENTION (1<<7)
 
// bit mask for staticParams.bitConfig2
#define CFG_LOOP_UP (1<<0)
#define CFG_LOOP_DOWN (1<<1)
#define CFG_LOOP_LEFT (1<<2)
#define CFG_LOOP_RIGHT (1<<3)
#define CFG_HEIGHT_3SWITCH (1<<4)
 
#define ATMEGA644 0
#define ATMEGA644P 1
#define SYSCLK F_CPU
203,9 → 195,11
#define MIX_ROLL 2
#define MIX_YAW 3
 
#define VARIABLE_COUNT 8
 
extern volatile uint8_t MKFlags;
extern uint8_t requiredMotors;
extern int16_t variables[8]; // The "Poti"s.
extern int16_t variables[VARIABLE_COUNT]; // The "Poti"s.
extern uint8_t boardRelease;
extern uint8_t CPUType;
 
/branches/dongfang_FC_rewrite/controlMixer.c
80,7 → 80,7
* (read: Custom MK RC project)
*/
uint8_t controlMixer_getArgument(void) {
return lastArgument;
return lastArgument;
}
 
/*
88,16 → 88,19
* than the R/C (read: Custom MK R/C project)
*/
uint8_t controlMixer_getCommand(void) {
return lastCommand;
return lastCommand;
}
 
uint8_t controlMixer_isCommandRepeated(void) {
return isCommandRepeated;
return isCommandRepeated;
}
 
void controlMixer_setNeutral() {
EC_setNeutral();
HC_setGround();
for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
variables[i] = 0;
}
EC_setNeutral();
HC_setGround();
}
 
/*
105,10 → 108,10
* No slew rate limitation.
*/
void controlMixer_initVariables(void) {
uint8_t i;
for (i = 0; i < 8; i++) {
variables[i] = RC_getVariable(i);
}
uint8_t i;
for (i=0; i < VARIABLE_COUNT; i++) {
variables[i] = RC_getVariable(i);
}
}
 
/*
116,44 → 119,44
* TODO: It assumes R/C as source. Not necessarily true.
*/
void controlMixer_updateVariables(void) {
uint8_t i;
int16_t targetvalue;
for (i = 0; i < 8; i++) {
targetvalue = RC_getVariable(i);
if (targetvalue < 0)
targetvalue = 0;
if (variables[i] < targetvalue && variables[i] < 255)
variables[i]++;
else if (variables[i] > 0 && variables[i] > targetvalue)
variables[i]--;
}
uint8_t i;
int16_t targetvalue;
for (i=0; i < VARIABLE_COUNT; i++) {
targetvalue = RC_getVariable(i);
if (targetvalue < 0)
targetvalue = 0;
if (variables[i] < targetvalue && variables[i] < 255)
variables[i]++;
else if (variables[i] > 0 && variables[i] > targetvalue)
variables[i]--;
}
}
 
uint8_t controlMixer_getSignalQuality(void) {
uint8_t rcQ = RC_getSignalQuality();
uint8_t ecQ = EC_getSignalQuality();
// This needs not be the only correct solution...
return rcQ > ecQ ? rcQ : ecQ;
uint8_t rcQ = RC_getSignalQuality();
uint8_t ecQ = EC_getSignalQuality();
// This needs not be the only correct solution...
return rcQ > ecQ ? rcQ : ecQ;
}
 
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
int16_t tmp = controls[index];
controls[index] = newValue;
tmp -= newValue;
tmp /= 2;
tmp = tmp * tmp;
// tmp += (newValue >= 0) ? newValue : -newValue;
if (controlActivity + (uint16_t)tmp >= controlActivity)
controlActivity += tmp;
else controlActivity = 0xffff;
int16_t tmp = controls[index];
controls[index] = newValue;
tmp -= newValue;
tmp /= 2;
tmp = tmp * tmp;
// tmp += (newValue >= 0) ? newValue : -newValue;
if (controlActivity + (uint16_t)tmp >= controlActivity)
controlActivity += tmp;
else controlActivity = 0xffff;
}
 
#define CADAMPING 10
void dampenControlActivity(void) {
uint32_t tmp = controlActivity;
tmp *= ((1<<CADAMPING)-1);
tmp >>= CADAMPING;
controlActivity = tmp;
uint32_t tmp = controlActivity;
tmp *= ((1<<CADAMPING)-1);
tmp >>= CADAMPING;
controlActivity = tmp;
}
 
/*
160,99 → 163,99
* Update the variables indicating stick position from the sum of R/C, GPS and external control.
*/
void controlMixer_update(void) {
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
// TODO: If no signal --> zero.
int16_t tempThrottle;
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
// TODO: If no signal --> zero.
int16_t tempThrottle;
// takes almost no time...
RC_update();
// takes almost no time...
EC_update();
// takes about 80 usec.
HC_update();
int16_t* RC_PRTY = RC_getPRTY();
int16_t* EC_PRTY = EC_getPRTY();
updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]);
updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]);
updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]);
dampenControlActivity();
 
// takes almost no time...
RC_update();
//debugOut.analog[14] = controlActivity/10;
tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]);
controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle);
// controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
controlMixer_updateVariables();
configuration_applyVariablesToParams();
//looping = RC_getLooping(looping);
} else { // Signal is not OK
// Could handle switch to emergency flight here.
// throttle is handled elsewhere.
// looping = 0;
}
// part1a end.
/* This is not really necessary with the dead-band feature on all sticks (see rc.c)
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (controlYaw > 2) controlYaw-= 2;
else if (controlYaw< -2) controlYaw += 2;
else controlYaw = 0;
}
*/
/*
* Record maxima
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]--;
}
*/
 
// takes almost no time...
EC_update();
 
// takes about 80 usec.
HC_update();
 
int16_t* RC_PRTY = RC_getPRTY();
int16_t* EC_PRTY = EC_getPRTY();
 
updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]);
updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]);
updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]);
dampenControlActivity();
 
//debugOut.analog[14] = controlActivity/10;
 
tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]);
controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle);
 
// controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
 
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
controlMixer_updateVariables();
configuration_applyVariablesToParams();
//looping = RC_getLooping(looping);
} else { // Signal is not OK
// Could handle switch to emergency flight here.
// throttle is handled elsewhere.
// looping = 0;
}
 
// part1a end.
 
/* This is not really necessary with the dead-band feature on all sticks (see rc.c)
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (controlYaw > 2) controlYaw-= 2;
else if (controlYaw< -2) controlYaw += 2;
else controlYaw = 0;
}
*/
 
/*
* Record maxima
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]--;
}
*/
 
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
: COMMAND_NONE;
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
: COMMAND_NONE;
 
if (rcCommand != COMMAND_NONE) {
isCommandRepeated = (lastCommand == rcCommand);
lastCommand = rcCommand;
lastArgument = RC_getArgument();
} else if (ecCommand != COMMAND_NONE) {
isCommandRepeated = (lastCommand == ecCommand);
lastCommand = ecCommand;
lastArgument = EC_getArgument();
} else {
// Both sources have no command, or one or both are out.
// Just set to false. There is no reason to check if the none-command was repeated anyway.
isCommandRepeated = 0;
lastCommand = COMMAND_NONE;
}
 
/*
if (isCommandRepeated)
DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED;
else
DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
if (rcCommand)
DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED;
else
DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED;
*/
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
: COMMAND_NONE;
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
: COMMAND_NONE;
if (rcCommand != COMMAND_NONE) {
isCommandRepeated = (lastCommand == rcCommand);
lastCommand = rcCommand;
lastArgument = RC_getArgument();
} else if (ecCommand != COMMAND_NONE) {
isCommandRepeated = (lastCommand == ecCommand);
lastCommand = ecCommand;
lastArgument = EC_getArgument();
} else {
// Both sources have no command, or one or both are out.
// Just set to false. There is no reason to check if the none-command was repeated anyway.
isCommandRepeated = 0;
lastCommand = COMMAND_NONE;
}
/*
if (isCommandRepeated)
DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED;
else
DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
if (rcCommand)
DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED;
else
DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED;
*/
}
 
// TODO: Integrate into command system.
uint8_t controlMixer_testCompassCalState(void) {
return RC_testCompassCalState();
return RC_testCompassCalState();
}
/branches/dongfang_FC_rewrite/eeprom.c
208,6 → 208,17
}
}
 
/***************************************************/
/* Sensor offsets */
/***************************************************/
uint8_t gyroOffset_readFromEEProm(void) {
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t));
}
 
void gyroOffset_writeToEEProm(void) {
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t));
}
 
uint8_t accOffset_readFromEEProm(void) {
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
}
216,7 → 227,6
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
}
 
 
/***************************************************/
/* Get active parameter set */
/***************************************************/
/branches/dongfang_FC_rewrite/heightControl.c
22,28 → 22,12
#define LATCH_TIME 40
 
int32_t groundPressure;
 
int32_t targetHeight;
int32_t rampedTargetHeight;
 
#define DEFAULT_HOVERTHROTTLE 50
int32_t stronglyFilteredHeightDiff = 0;
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
#define HOVERTHROTTLEFILTER 25
 
uint8_t heightRampingTimer = 0;
int32_t maxHeight;
int32_t maxHeightThisFlight;
int32_t iHeight;
/*
These parameters are free to take:
uint8_t HeightMinGas; // Value : 0-100
uint8_t HeightD; // Value : 0-250
uint8_t MaxHeight; // Value : 0-32
uint8_t HeightP; // Value : 0-32
uint8_t Height_Gain; // Value : 0-50
uint8_t Height_ACC_Effect; // Value : 0-250
*/
 
int32_t getHeight(void) {
return groundPressure - filteredAirPressure;
53,7 → 37,7
groundPressure = filteredAirPressure;
// This should also happen when height control is enabled in-flight.
rampedTargetHeight = getHeight();
maxHeight = 0;
maxHeightThisFlight = 0;
iHeight = 0;
}
 
61,12 → 45,11
int32_t height = getHeight();
static uint8_t setHeightLatch = 0;
if (height > maxHeight)
maxHeight = height;
if (height > maxHeightThisFlight)
maxHeightThisFlight = height;
if (staticParams.bitConfig & CFG_HEIGHT_SWITCH) {
if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
// If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
// DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
// Switch is ON
if (setHeightLatch <= LATCH_TIME) {
73,7 → 56,6
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;
// DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
}
// Time not yet reached.
setHeightLatch++;
81,20 → 63,28
} else {
// Switch is OFF.
setHeightLatch = 0;
// DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
}
} else {
// Switch is not activated; take the "max-height" as the target height.
// DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100;
targetHeight = (uint16_t) dynamicParams.heightSetting * 100;
}
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
heightRampingTimer = 0;
if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) {
rampedTargetHeight += staticParams.heightSlewRate;
} else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) {
rampedTargetHeight -= staticParams.heightSlewRate;
if (rampedTargetHeight < targetHeight) {
// climbing
if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
rampedTargetHeight += staticParams.heightSlewRate;
} else {
rampedTargetHeight = targetHeight;
}
} else if (rampedTargetHeight != targetHeight) {
// descending
if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
rampedTargetHeight -= staticParams.heightSlewRate;
} else {
rampedTargetHeight = targetHeight;
}
}
}
102,10 → 92,6
debugOut.analog[30] = height / 10;
}
 
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH
// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH
 
// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
uint16_t HC_getThrottle(uint16_t throttle) {
127,8 → 113,8
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
} else if (dHeight < 0) {
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
}
/*
138,19 → 124,17
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
*/
int16_t dThrottle = heightError * staticParams.heightP / 1000
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
* staticParams.heightD;
int16_t dThrottle = ((heightError * staticParams.heightP) << 10)
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD;
// the "minGas" is now a limit for how much up / down the throttle can be varied
if (dThrottle > staticParams.heightMaxThrottleChange)
dThrottle = staticParams.heightMaxThrottleChange;
else if (dThrottle < -staticParams.heightMaxThrottleChange)
dThrottle = -staticParams.heightMaxThrottleChange;
if (dThrottle > staticParams.heightControlMaxThrottleChange)
dThrottle = staticParams.heightControlMaxThrottleChange;
else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
dThrottle = -staticParams.heightControlMaxThrottleChange;
// TODO: Eliminate repetition.
if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) {
if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH)
if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
|| (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
// If switch is not in use --> Just apply height control.
// If switch is in use --> only apply height control when switch is also ON.
157,8 → 141,15
throttle += dThrottle;
}
}
 
/* Experiment: Find hover-throttle */
 
#define DEFAULT_HOVERTHROTTLE 50
int32_t stronglyFilteredHeightDiff = 0;
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
#define HOVERTHROTTLEFILTER 25
/* Experiment: Find hover-throttle */
stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
/branches/dongfang_FC_rewrite/main.c
103,8 → 103,7
timer0_init();
timer2_init();
usart0_Init();
if (CPUType == ATMEGA644P)
usart1_Init();
if (CPUType == ATMEGA644P);// usart1_Init();
RC_Init();
analog_init();
I2C_init();
/branches/dongfang_FC_rewrite/makefile
1,6 → 1,6
#--------------------------------------------------------------------
# MCU name
MCU = atmega644p
MCU = atmega644
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
137,7 → 137,7
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c
SRC += externalControl.c GPSControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c
SRC += eeprom.c uart1.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c
SRC += eeprom.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c
 
ifeq ($(EXT), MK3MAG)
SRC += mk3mag.c