648,7 → 648,7 |
/************************************************************************/ |
void MotorControl(void) |
{ |
int16_t tmp_int, tmp_int2; |
int16_t tmp_int1, tmp_int2; |
int32_t tmp_long, tmp_long2; |
|
// Mixer Fractions that are combined for Motor Control |
668,15 → 668,11 |
static uint16_t UpdateCompassCourse = 0; |
// high resolution motor values for smoothing of PID motor outputs |
static int16_t MotorValue[MAX_MOTORS]; |
static uint8_t LoadHandler = 5; |
uint8_t i; |
|
if(--LoadHandler == 0) LoadHandler = 5; // distribution of proccessor load |
|
Mean(); |
GRN_ON; |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is bad |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1327,11 → 1323,11 |
} |
} |
// exponential stick sensitivity in yawring rate |
tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
tmp_int += (ParamSet.StickYawP * StickYaw) / 4; |
SetPointYaw = tmp_int; |
tmp_int1 = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
tmp_int1 += (ParamSet.StickYawP * StickYaw) / 4; |
SetPointYaw = tmp_int1; |
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
ReadingIntegralGyroYaw -= tmp_int; |
ReadingIntegralGyroYaw -= tmp_int1; |
// limit the effect |
LIMIT_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000) |
|
1508,18 → 1504,17 |
} |
GasMixFraction *= STICK_GAIN; // scale GasMixFraction to enlarge resolution in the motor mixer |
|
// if airpressure sensor is enabled |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Airpressure sensor is enabled |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) ) |
{ |
static int16_t FilterHCGas = 0; |
#define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.1s averaging |
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
|
if(LoadHandler == 1) // only every 5th run |
{ // 2ms * 5 = 10ms update |
#define HOOVER_GAS_AVERAGE 1024L // 1024 * 10ms = 10s averaging |
#define HC_GAS_AVERAGE 2 // 2 * 10ms= 20 ms averaging |
|
int16_t CosAttitude; // for projection of hoover gas |
int16_t HCGas, HeightDeviation; |
static int16_t FilterHCGas = 0; |
static int16_t HeightTrimming = 0; // rate for change of height setpoint |
static uint8_t HCActive = 0; |
static int16_t StickGasHoover = RC_GAS_OFFSET, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
1553,7 → 1548,7 |
ExpandBaro -= 1; |
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
BeepTime = 300; |
BaroExpandActive = 50; |
BaroExpandActive = 250; |
} |
else |
{ |
1568,7 → 1563,7 |
ExpandBaro += 1; |
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
BeepTime = 300; |
BaroExpandActive = 50; |
BaroExpandActive = 250; |
} |
else |
{ |
1582,7 → 1577,6 |
}// EOF --------- barometer range expansion ------------------ |
|
|
|
// if height control is activated by an rc channel |
if(ParamSet.Config0 & CFG0_HEIGHT_SWITCH) |
{ // check if parameter is less than activation threshold |
1590,7 → 1584,6 |
{ //height control not active |
if(!delay--) |
{ |
|
SetPointHeight = ReadingHeight; // update SetPoint with current reading |
HCActive = 0; // disable height control |
delay = 1; |
1599,7 → 1592,7 |
else |
{ //height control is activated |
HCActive = 1; // enable height control |
delay = 40; |
delay = 200; |
} |
} |
else // no switchable height control |
1613,9 → 1606,9 |
|
|
// calculate cos of nick and roll angle used for projection of the vertical hoover gas |
tmp_int = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); // nick angle in deg |
tmp_int1 = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); // nick angle in deg |
tmp_int2 = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); // roll angle in deg |
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); |
CosAttitude = (int16_t)ihypot(tmp_int1, tmp_int2); |
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
|
1623,7 → 1616,7 |
{ |
if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
{ |
// Holger original version |
// Holgers original version |
// start of height control algorithm |
// the height control is only an attenuation of the actual gas stick. |
// I.e. it will work only if the gas stick is higher than the hover gas |
1675,12 → 1668,11 |
} |
} |
// trim height set point if needed |
if(abs(HeightTrimming) > 256) |
if(abs(HeightTrimming) > 512) |
{ |
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/256; // move setpoint |
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/((5 * 512) / 2); // move setpoint |
HeightTrimming = 0; |
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75; |
/* |
//update hoover gas stick value when setpoint is shifted |
if(!ParamSet.Height_StickNeutralPoint) |
{ |
1687,7 → 1679,7 |
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning; |
LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down |
}*/ |
} |
} // EOF trimming height set point |
} //if MKFlags & MKFLAG_FLY |
else // not flying but height control is already active |
1712,20 → 1704,20 |
{ |
// ------------------------- P-Part ---------------------------- |
HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when too high |
tmp_int = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part |
HCGas -= tmp_int; |
tmp_int1 = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part |
HCGas -= tmp_int1; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int = ReadingVario / 8; |
if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
if(tmp_int > 0) tmp_int = ReadingVario + (tmp_int * tmp_int) / 4; |
else tmp_int = ReadingVario - (tmp_int * tmp_int) / 4; |
tmp_int = (FCParam.HeightD * (int32_t)(tmp_int)) / 128L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int, -127, 255); |
HCGas -= tmp_int; |
tmp_int1 = ReadingVario / 8; |
if(tmp_int1 > 8) tmp_int1 = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
if(tmp_int1 > 0) tmp_int1 = ReadingVario + (tmp_int1 * tmp_int1) / 4; |
else tmp_int1 = ReadingVario - (tmp_int1 * tmp_int1) / 4; |
tmp_int1 = (FCParam.HeightD * (int32_t)(tmp_int1)) / 128L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int1, -127, 255); |
HCGas -= tmp_int1; |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_int, -127, 255); |
HCGas -= tmp_int; |
tmp_int1 = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_int1, -127, 255); |
HCGas -= tmp_int1; |
|
// limit deviation from hoover point within the target region |
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1735,9 → 1727,9 |
} // EOF no baro range expanding |
|
// ------------------------ D-Part 3: GpsZ ---------------------------------- |
tmp_int = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int, -127, 255); |
HCGas -= tmp_int; |
tmp_int1 = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int1, -127, 255); |
HCGas -= tmp_int1; |
|
// strech control output by inverse attitude projection 1/cos |
tmp_long2 = (int32_t)HCGas; |
1821,11 → 1813,7 |
} |
} //EOF only on small vertical speed |
}// EOF ----------------- Hoover Gas Estimation -------------------------------- |
} // EOF if(LoadBalancer == 1) |
else |
{ // if no update in the height control block then take last value |
GasMixFraction = FilterHCGas; |
} |
|
}// EOF ParamSet.Config0 & CFG0_AIRPRESS_SENSOR |
|
// limit gas to parameter setting |
1861,8 → 1849,8 |
{ |
LIMIT_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
tmp_int = ParamSet.GasMax * STICK_GAIN; |
LIMIT_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction)); |
tmp_int1 = ParamSet.GasMax * STICK_GAIN; |
LIMIT_MIN_MAX(YawMixFraction, -(tmp_int1 - GasMixFraction), (tmp_int1 - GasMixFraction)); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Axis |
1885,9 → 1873,9 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Limiter |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64; |
LIMIT_MIN_MAX(NickMixFraction, -tmp_int, tmp_int); |
LIMIT_MIN_MAX(RollMixFraction, -tmp_int, tmp_int); |
tmp_int1 = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64; |
LIMIT_MIN_MAX(NickMixFraction, -tmp_int1, tmp_int1); |
LIMIT_MIN_MAX(RollMixFraction, -tmp_int1, tmp_int1); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |