/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 |