138,7 → 138,7 |
// MK flags |
uint16_t ModelIsFlying = 0; |
|
volatile uint8_t FC_StatusFlags = 0; |
volatile uint8_t FC_StatusFlags = 0, FC_StatusFlags2 = 0; |
volatile uint8_t FC_ErrorFlags = 0; |
|
int8_t VarioCharacter = ' '; |
212,7 → 212,7 |
DebugOut.Analog[20] = ServoNickValue; |
DebugOut.Analog[22] = Capacity.ActualCurrent; |
DebugOut.Analog[23] = Capacity.UsedCapacity; |
DebugOut.Analog[24] = SetPointHeight/5; |
//DebugOut.Analog[24] = SetPointHeight/5; |
DebugOut.Analog[27] = CompassCourse; |
DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
DebugOut.Analog[30] = GPSStickNick; |
614,10 → 614,6 |
{ |
Motor[i].SetPoint = MotorTest[i]; |
Motor[i].SetPointLowerBits = 0; |
/* |
Motor[i].SetPoint = MotorTest[i] / 4; |
Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
*/ |
} |
} |
if(MotorTest_Active) MotorTest_Active--; |
702,36 → 698,46 |
Ki = 10300 / ( FCParam.IFactor + 1 ); |
|
CHK_POTI(tmp,ParamSet.OrientationModeControl); |
if(tmp > 50) |
if(tmp > 50) // care free should be set on |
{ |
#ifdef SWITCH_LEARNS_CAREFREE |
if(!CareFree) ControlHeading = (((int16_t) ParamSet.OrientationAngle * 15 + CompassHeading) % 360) / 2; |
#endif |
CareFree = 1; |
if(CareFreeOld == 0 && CareFree) BeepTime = 1000; |
if(CareFreeOld == 1 && !CareFree) BeepTime = 200; |
CareFreeOld = CareFree; |
if(FromNaviCtrl.CompassHeading < 0) UART_VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; |
else UART_VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; |
// check dependencies like valid compass heading |
if((FromNaviCtrl.CompassHeading < 0) && (FC_StatusFlags & FC_STATUS_MOTOR_RUN)) |
{ |
// indicate orientation lost |
if(BeepModulation == 0xFFFF) |
{ |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0xA400; |
} |
// and disable care free |
CareFree = 0; |
FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE; |
UART_VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; |
} |
else // everthing ok |
{ |
#ifdef SWITCH_LEARNS_CAREFREE |
if(!CareFree) ControlHeading = (((int16_t) ParamSet.OrientationAngle * 15 + CompassHeading) % 360) / 2; |
#endif |
CareFree = 1; |
if(FCParam.AxisCoupling1 < 210) FCParam.AxisCoupling1 += 30; |
FC_StatusFlags2 |= FC_STATUS2_CAREFREE; |
UART_VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; // reset error flag |
} |
} |
else |
else // care free off |
{ |
CareFree = 0; |
CareFreeOld = 0; |
FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE; |
UART_VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; // reset error flag |
} |
|
if((FromNaviCtrl.CompassHeading < 0) && (BeepModulation == 0xFFFF) && CareFree && (FC_StatusFlags & FC_STATUS_MOTOR_RUN)) |
// accoustic signal when CareFree state transition |
if(CareFree != CareFreeOld) |
{ |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0xA400; |
CareFree = 0; |
if(CareFree) BeepTime = 1500; // has been enabled |
else BeepTime = 200; // has been disabled |
CareFreeOld = CareFree; // resync |
} |
|
if(CareFree) |
{ |
if(FCParam.AxisCoupling1 < 210) FCParam.AxisCoupling1 += 30; |
} |
|
} |
} |
|
1512,42 → 1518,10 |
if(!UpdateCompassCourse) |
{ |
r = ((540 + CompassCourse - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180; |
DebugOut.Analog[19] = r; |
v = r * (FCParam.CompassYawEffect/2); |
if(abs(r) > 20) v *= 2; // over 20° course deviation -> twice as fast |
CompassYawSetPoint = v / 16; |
} |
else CompassYawSetPoint = 0; |
DebugOut.Analog[18] = CompassFusion; |
DebugOut.Analog[25] = CompassYawSetPoint; |
|
/* |
w = (w * FCParam.CompassYawEffect) / 32; |
w = FCParam.CompassYawEffect - w; |
if(w >= 0) |
{ |
if(!BadCompassHeading) |
{ |
v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
// calc course deviation |
r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * FCParam.CompassYawEffect; |
if (v > w) v = w; |
else if (v < -w) v = -w; |
ReadingIntegralGyroYaw += v; |
} |
else |
{ // wait a while |
BadCompassHeading--; |
} |
} |
else |
{ // ignore compass at extreme attitudes for a while |
BadCompassHeading = 500; |
} |
*/ |
} |
} // EOF if(CalculateCompassTimer-- == 1) |
else |
1731,6 → 1705,8 |
|
if(HCActive && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
{ |
FC_StatusFlags2 |= FC_STATUS2_ALTITUDE_CONTROL; |
|
if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
{ |
// Holgers original version |
1762,6 → 1738,7 |
FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
HeightTrimming += abs(StickGas - (StickGasHover + HC_STICKTHRESHOLD)); |
VarioCharacter = '+'; |
WaypointTrimming= 0; |
} // gas stick is below hover point |
else if(StickGas < (StickGasHover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN)) |
{ |
1773,6 → 1750,7 |
FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
HeightTrimming -= abs(StickGas - (StickGasHover - HC_STICKTHRESHOLD)); |
VarioCharacter = '-'; |
WaypointTrimming= 0; |
} |
else // gas stick in hover range |
{ |
1783,6 → 1761,11 |
HeightTrimming += NCAltitudeSpeed; |
WaypointTrimming = 10; |
VarioCharacter = '^'; |
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
{ |
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
SetPointHeight = ReadingHeight; // update setpoint to current height |
} |
} |
else if(NCAltitudeSpeed && (NCSetPointHeight < SetPointHeight)) |
{ |
1790,6 → 1773,11 |
HeightTrimming -= NCAltitudeSpeed; |
WaypointTrimming = -10; |
VarioCharacter = 'v'; |
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
{ |
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
SetPointHeight = ReadingHeight; // update setpoint to current height |
} |
} |
else if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
{ |
1951,6 → 1939,7 |
FilterHCGas = GasMixFraction; // init filter for HCGas witch current gas mix fraction |
// set undefined state to indicate vario off |
FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
FC_StatusFlags2 &= ~FC_STATUS2_ALTITUDE_CONTROL; |
} // EOF HC not active |
|
// ----------------- Hover Gas Estimation -------------------------------- |
1975,7 → 1964,7 |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
HoverGasFilter += 4L * tmp_long2; |
} |
else if(abs(ReadingVario) < 100) // later on, only on small vertical speed |
else if((abs(ReadingVario) < 100) && ( abs(ReadingHeight - SetPointHeight) < 256)) // later on, only on small vertical speed, and less setpoit deviation |
{ |
HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
HoverGasFilter += tmp_long2; |