/beta/Code Redesign killagreg/analog.c |
---|
76,7 → 76,6 |
volatile uint8_t ADReady = 1; |
uint8_t DacOffsetGyroNick = 115, DacOffsetGyroRoll = 115, DacOffsetGyroYaw = 115; |
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
int8_t ExpandBaro = 0; |
uint8_t PressureSensorOffset; |
128,6 → 127,7 |
} |
SetParamByte(PID_PRESSURE_OFFSET, off); |
PressureSensorOffset = off; |
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && ((PressureSensorOffset < 10) || (PressureSensorOffset >= 245))) UART_VersionInfo.HardwareError[0] |= DEFECT_PRESSURE; |
AirPressure = AIR_PRESSURE_SCALE * (int32_t)AdAirPressure; // init IIR Filter |
Delay_ms_Mess(300); |
} |
138,8 → 138,6 |
uint8_t i, ready = 0; |
uint16_t timeout ; |
GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
timeout = SetDelay(2000); |
if(BoardRelease == 13) // the auto offset calibration is available only at board release 1.3 |
{ |
151,9 → 149,9 |
if(AdValueGyroRoll < 1020) DacOffsetGyroRoll--; else if(AdValueGyroRoll > 1030) DacOffsetGyroRoll++; else ready++; |
if(AdValueGyroYaw < 1020) DacOffsetGyroYaw-- ; else if(AdValueGyroYaw > 1030) DacOffsetGyroYaw++ ; else ready++; |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
if(DacOffsetGyroNick < 10) { GyroDefectNick = 1; DacOffsetGyroNick = 10;}; if(DacOffsetGyroNick > 245) { GyroDefectNick = 1; DacOffsetGyroNick = 245;}; |
if(DacOffsetGyroRoll < 10) { GyroDefectRoll = 1; DacOffsetGyroRoll = 10;}; if(DacOffsetGyroRoll > 245) { GyroDefectRoll = 1; DacOffsetGyroRoll = 245;}; |
if(DacOffsetGyroYaw < 10) { GyroDefectYaw = 1; DacOffsetGyroYaw = 10;}; if(DacOffsetGyroYaw > 245) { GyroDefectYaw = 1; DacOffsetGyroYaw = 245;}; |
if(DacOffsetGyroNick < 10) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_NICK; DacOffsetGyroNick = 10;}; if(DacOffsetGyroNick > 245) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_NICK; DacOffsetGyroNick = 245;}; |
if(DacOffsetGyroRoll < 10) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_ROLL; DacOffsetGyroRoll = 10;}; if(DacOffsetGyroRoll > 245) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_ROLL; DacOffsetGyroRoll = 245;}; |
if(DacOffsetGyroYaw < 10) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_YAW; DacOffsetGyroYaw = 10;}; if(DacOffsetGyroYaw > 245) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_YAW; DacOffsetGyroYaw = 245;}; |
while(twi_state) |
{ |
if(CheckDelay(timeout)) |
264,7 → 262,7 |
{ |
if(AdBiasAccTop < 750) |
{ |
Sub_AdBiasAccTop += 2; |
Sub_AdBiasAccTop += 5; |
if(ModelIsFlying < 500) Sub_AdBiasAccTop += 10; |
if(Sub_AdBiasAccTop > 100) |
{ |
277,7 → 275,7 |
{ |
if(AdBiasAccTop > 550) |
{ |
Sub_AdBiasAccTop -= 2; |
Sub_AdBiasAccTop -= 5; |
if(ModelIsFlying < 500) Sub_AdBiasAccTop -= 10; |
if(Sub_AdBiasAccTop < 100) |
{ |
354,7 → 352,7 |
ReadingHeight = (StartAirPressure - AirPressure); // change of air pressure to ground |
SumHeight -= SumHeight/SM_FILTER; |
SumHeight += ReadingHeight; |
ReadingVario = (15 * ReadingVario + 8 * (int16_t)(ReadingHeight - SumHeight/SM_FILTER))/16; |
ReadingVario = (31 * ReadingVario + 8 * (int16_t)(ReadingHeight - SumHeight/SM_FILTER))/32; |
tmpAirPressure /= 2; |
AirPressCount = AIR_PRESSURE_SCALE/2; |
} |
/beta/Code Redesign killagreg/eeprom.c |
---|
143,13 → 143,13 |
} |
ParamSet.Config0 = CFG0_AXIS_COUPLING_ACTIVE | CFG0_COMPASS_ACTIVE | CFG0_GPS_ACTIVE | CFG0_HEIGHT_SWITCH;//CFG0_HEIGHT_CONTROL | CFG0_COMPASS_FIX; |
ParamSet.Config1 = 0; |
ParamSet.Config2 = CFG2_HEIGHT_LIMIT;//|CFG2_SENSITIVE_RC; |
ParamSet.Config2 = CFG2_HEIGHT_LIMIT | CFG2_VARIO_BEEP;//|CFG2_SENSITIVE_RC; |
ParamSet.HeightMinGas = 30; |
ParamSet.MaxHeight = 255; |
ParamSet.HeightP = 10; |
ParamSet.HeightP = 15; |
ParamSet.HeightD = 30; |
ParamSet.Height_ACC_Effect = 30; |
ParamSet.Height_HoverBand = 5; |
ParamSet.Height_ACC_Effect = 0; |
ParamSet.Height_HoverBand = 8; |
ParamSet.Height_GPS_Z = 64; |
ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation) |
ParamSet.Height_Gain = 20; |
165,8 → 165,8 |
ParamSet.GyroYawI = 150; |
ParamSet.GyroStability = 6; // Value : 1-8 |
ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50 |
ParamSet.EmergencyGas = 35; |
ParamSet.EmergencyGasDuration = 60; |
ParamSet.EmergencyGas = 45; |
ParamSet.EmergencyGasDuration = 90; |
ParamSet.Receiver = RECEIVER_SPEKTRUM; |
ParamSet.IFactor = 32; |
ParamSet.UserParam1 = 0; |
251,13 → 251,13 |
} |
ParamSet.Config0 = CFG0_AXIS_COUPLING_ACTIVE | CFG0_COMPASS_ACTIVE | CFG0_GPS_ACTIVE | CFG0_HEIGHT_SWITCH;//CFG0_HEIGHT_CONTROL | CFG0_COMPASS_FIX; |
ParamSet.Config1 = 0; |
ParamSet.Config2 = CFG2_HEIGHT_LIMIT;//|CFG2_SENSITIVE_RC; |
ParamSet.Config2 = CFG2_HEIGHT_LIMIT | CFG2_VARIO_BEEP;//|CFG2_SENSITIVE_RC; |
ParamSet.HeightMinGas = 30; |
ParamSet.MaxHeight = 255; |
ParamSet.HeightP = 10; |
ParamSet.HeightP = 15; |
ParamSet.HeightD = 30; |
ParamSet.Height_ACC_Effect = 30; |
ParamSet.Height_HoverBand = 5; |
ParamSet.Height_ACC_Effect = 0; |
ParamSet.Height_HoverBand = 8; |
ParamSet.Height_GPS_Z = 64; |
ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation) |
ParamSet.Height_Gain = 15; |
273,8 → 273,8 |
ParamSet.GyroYawI = 120; |
ParamSet.GyroStability = 6; // Value : 1-8 |
ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50 |
ParamSet.EmergencyGas = 35; |
ParamSet.EmergencyGasDuration = 60; |
ParamSet.EmergencyGas = 45; |
ParamSet.EmergencyGasDuration = 90; |
ParamSet.Receiver = RECEIVER_SPEKTRUM; |
ParamSet.IFactor = 32; |
ParamSet.UserParam1 = 0; |
359,13 → 359,13 |
} |
ParamSet.Config0 = CFG0_AXIS_COUPLING_ACTIVE | CFG0_COMPASS_ACTIVE | CFG0_GPS_ACTIVE | CFG0_HEIGHT_SWITCH;//CFG0_HEIGHT_CONTROL | CFG0_COMPASS_FIX | CFG0_ROTARY_RATE_LIMITER; |
ParamSet.Config1 = 0; |
ParamSet.Config2 = CFG2_HEIGHT_LIMIT;//|CFG2_SENSITIVE_RC; |
ParamSet.Config2 = CFG2_HEIGHT_LIMIT | CFG2_VARIO_BEEP;//|CFG2_SENSITIVE_RC; |
ParamSet.HeightMinGas = 30; |
ParamSet.MaxHeight = 255; |
ParamSet.HeightP = 10; |
ParamSet.HeightP = 15; |
ParamSet.HeightD = 30; |
ParamSet.Height_ACC_Effect = 30; |
ParamSet.Height_HoverBand = 5; |
ParamSet.Height_ACC_Effect = 0; |
ParamSet.Height_HoverBand = 8; |
ParamSet.Height_GPS_Z = 64; |
ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation) |
ParamSet.Height_Gain = 15; |
381,8 → 381,8 |
ParamSet.GyroYawI = 120; |
ParamSet.GyroStability = 6; // Value : 1-8 |
ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50 |
ParamSet.EmergencyGas = 35; |
ParamSet.EmergencyGasDuration = 60; |
ParamSet.EmergencyGas = 45; |
ParamSet.EmergencyGasDuration = 90; |
ParamSet.Receiver = RECEIVER_SPEKTRUM; |
ParamSet.IFactor = 16; |
ParamSet.UserParam1 = 0; |
413,7 → 413,7 |
ParamSet.AxisCoupling2 = 80; |
ParamSet.AxisCouplingYawCorrection = 70; |
ParamSet.GyroAccTrim = 32; |
ParamSet.DynamicStability = 50; |
ParamSet.DynamicStability = 70; |
ParamSet.J16Bitmask = 95; |
ParamSet.J17Bitmask = 243; |
ParamSet.J16Bitmask_Warning = 0xAA; |
/beta/Code Redesign killagreg/fc.c |
---|
92,7 → 92,7 |
int16_t BiasHiResGyroNick = 0, BiasHiResGyroRoll = 0, AdBiasGyroYaw = 0; |
// accelerations |
int16_t AccNick, AccRoll, AccTop; |
int16_t AccNick, AccRoll;// AccTop; |
// neutral acceleration readings |
int16_t AdBiasAccNick = 0, AdBiasAccRoll = 0; |
194,6 → 194,10 |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR; |
DebugOut.Analog[12] = Motor[0].SetPoint; |
DebugOut.Analog[13] = Motor[1].SetPoint; |
DebugOut.Analog[14] = Motor[2].SetPoint; |
DebugOut.Analog[15] = Motor[3].SetPoint; |
//DebugOut.Analog[18] = ReadingVario; |
//DebugOut.Analog[19] = CompassCalState; |
DebugOut.Analog[20] = ServoNickValue; |
202,6 → 206,8 |
DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
DebugOut.Analog[30] = GPSStickNick; |
DebugOut.Analog[31] = GPSStickRoll; |
if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x01; |
else DebugOut.Status[1] &= ~0x01; |
} |
/************************************************************************/ |
229,6 → 235,8 |
//Servo_Off(); // disable servo output |
UART_VersionInfo.HardwareError[0] = 0; |
AdBiasAccNick = 0; |
AdBiasAccRoll = 0; |
AdBiasAccTop = 0; |
316,9 → 324,9 |
ReadingVario = 0; |
// reset acc averaging and integrals |
AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick; |
AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
AccTop = AdValueAccTop; |
AccNick = ACC_AMPLIFY * AdValueAccNick; |
AccRoll = ACC_AMPLIFY * AdValueAccRoll; |
//AccTop = AdValueAccTop; |
ReadingIntegralTop = AdValueAccTop * 1024; |
// and gyro readings |
364,6 → 372,13 |
//Servo_On(); //enable servo output |
RC_Quality = 100; |
if((BiasHiResGyroNick < 150 * 16) || (BiasHiResGyroNick > 850 * 16)) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_NICK; }; |
if((BiasHiResGyroRoll < 150 * 16) || (BiasHiResGyroRoll > 850 * 16)) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_ROLL; }; |
if((AdBiasGyroYaw < 150 * 2) || (AdBiasGyroYaw > 850 * 2 )) { UART_VersionInfo.HardwareError[0] |= DEFECT_G_YAW; }; |
if((AdBiasAccNick < 300 * 2) || (AdBiasAccNick > 750 * 2)) { UART_VersionInfo.HardwareError[0] |= DEFECT_A_NICK; }; |
if((AdBiasAccRoll < 300 * 2) || (AdBiasAccRoll > 750 * 2)) { UART_VersionInfo.HardwareError[0] |= DEFECT_A_ROLL; }; |
if((AdBiasAccTop < 512) || (AdBiasAccTop > 850 )) { UART_VersionInfo.HardwareError[0] |= DEFECT_A_TOP; }; |
} |
/************************************************************************/ |
416,13 → 431,13 |
// Acceleration Sensor |
// lowpass acc measurement and scale AccNick/AccRoll by a factor of ACC_AMPLIFY to have a better resolution |
AccNick = ((int32_t)AccNick * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L; |
AccRoll = ((int32_t)AccRoll * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L; |
AccTop = ((int32_t)AccTop * 3L + ((int32_t)AdValueAccTop)) / 4L; |
AccNick = (AccNick * 3 + ((ACC_AMPLIFY * AdValueAccNick))) / 4L; |
AccRoll = (AccRoll * 3 + ((ACC_AMPLIFY * AdValueAccRoll))) / 4L; |
//AccTop = (AccTop * 3 + (AdValueAccTop)) / 4L; |
// sum acc sensor readings for later averaging |
MeanAccNick += ACC_AMPLIFY * AdValueAccNick; |
MeanAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
MeanAccNick += ACC_AMPLIFY * (int32_t)AdValueAccNick; |
MeanAccRoll += ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
NaviAccNick += AdValueAccNick; |
NaviAccRoll += AdValueAccRoll; |
595,10 → 610,6 |
} |
if(MotorTest_Active) MotorTest_Active--; |
} |
DebugOut.Analog[12] = Motor[0].SetPoint; // Front |
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear |
DebugOut.Analog[14] = Motor[2].SetPoint; // Left |
DebugOut.Analog[15] = Motor[3].SetPoint; // Right |
//Start I2C Interrupt Mode |
I2C_Start(TWI_STATE_MOTOR_TX); |
} |
653,17 → 664,29 |
Ki = 10300 / ( FCParam.IFactor + 1 ); |
CHK_POTI(tmp,ParamSet.OrientationModeControl); |
if(tmp > 50 && NCDataOkay > 200) |
if(tmp > 50) |
{ |
#ifdef SWITCH_LEARNS_CAREFREE |
if(!CareFree) ControlHeading = (((int16_t) ParamSet.OrientationAngle * 15 + CompassHeading) % 360) / 2; |
#endif |
CareFree = 1; |
if(FromNaviCtrl.CompassHeading < 0) UART_VersionInfo.HardwareError[0] |= DEFECT_CAREFREE_ERR; |
else UART_VersionInfo.HardwareError[0] &= ~DEFECT_CAREFREE_ERR; |
} |
else CareFree = 0; |
if(CareFree) {if(FCParam.AxisCoupling1 < 210) FCParam.AxisCoupling1 += 30;} |
if((FromNaviCtrl.CompassHeading < 0) && (BeepModulation == 0xFFFF) && CareFree && (FCFlags & FCFLAG_MOTOR_RUN)) |
{ |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0xA400; |
CareFree = 0; |
} |
if(CareFree) |
{ |
if(FCParam.AxisCoupling1 < 210) FCParam.AxisCoupling1 += 30; |
} |
} |
} |
934,7 → 957,7 |
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_startmotors = 0; |
if(Calibration_Done) |
if(!UART_VersionInfo.HardwareError[0] && Calibration_Done) |
{ |
ModelIsFlying = 1; |
FCFlags |= (FCFLAG_MOTOR_RUN|FCFLAG_START); // set flag RUN and START |
1250,12 → 1273,12 |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralGyroNickError = (int32_t)(MeanIntegralGyroNick - (int32_t)MeanAccNick); |
IntegralGyroNickError = MeanIntegralGyroNick - MeanAccNick; |
CorrectionNick = IntegralGyroNickError / ParamSet.GyroAccTrim; |
AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralGyroRollError = (int32_t)(MeanIntegralGyroRoll - (int32_t)MeanAccRoll); |
IntegralGyroRollError = MeanIntegralGyroRoll - MeanAccRoll; |
CorrectionRoll = IntegralGyroRollError / ParamSet.GyroAccTrim; |
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER; |
1289,7 → 1312,6 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; |
if(IntegralGyroNickError > ERROR_LIMIT1) cnt = 4; |
CorrectionNick = 0; |
if((labs(MeanIntegralGyroNick_old - MeanIntegralGyroNick) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8)) |
{ |
if(IntegralGyroNickError > ERROR_LIMIT2) |
1331,7 → 1353,6 |
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; |
if(IntegralGyroRollError > ERROR_LIMIT1) cnt = 4; |
CorrectionRoll = 0; |
if((labs(MeanIntegralGyroRoll_old - MeanIntegralGyroRoll) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8)) |
{ |
if(IntegralGyroRollError > ERROR_LIMIT2) |
1544,16 → 1565,24 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) ) |
{ |
#define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.1s averaging |
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
int16_t CosAttitude; // for projection of hoover gas |
int16_t HCGas, HeightDeviation = 0; |
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
static int16_t FilterHCGas = 0; |
static int16_t HeightTrimming = 0; // rate for change of height setpoint |
static uint8_t HCActive = 0; |
// the states for the hover gas filter |
#define HOVER_GAS_STATE_NONE 0 |
#define HOVER_GAS_STATE_START 1 |
#define HOVER_GAS_STATE_RUNNING 2 |
static uint8_t HoverGasState = HOVER_GAS_STATE_NONE; |
#define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32.8s averaging |
static uint32_t HoverGasFilter = 0; // for averaging of GasMixFraction |
static int16_t StickGasHover = RC_GAS_OFFSET, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
static uint32_t HoverGasFilter = 0; |
static uint8_t delay = 100; |
#define BARO_LIMIT_MAX 0x01 |
1639,7 → 1668,6 |
HCActive = 1; |
} |
// calculate cos of nick and roll angle used for projection of the vertical hoover gas |
tmp_int1 = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); // nick angle in deg |
tmp_int2 = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); // roll angle in deg |
1650,6 → 1678,10 |
if(HCActive && !(FCFlags & FCFLAG_EMERGENCY_LANDING)) |
{ |
#define HC_TRIM_UP 0x01 |
#define HC_TRIM_DOWN 0x02 |
static uint8_t HeightTrimmingFlag = 0x00; |
if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
{ |
// Holgers original version |
1660,15 → 1692,12 |
HCGas = GasMixFraction; // take current stick gas as neutral point for the height control |
HeightTrimming = 0; |
HeightTrimmingFlag = 0x00; |
} |
else // alternative height control |
{ |
// PD-Control with respect to hover point |
// the setpoint will be fine adjusted with the gas stick position |
#define HC_TRIM_UP 0x01 |
#define HC_TRIM_DOWN 0x02 |
static uint8_t HeightTrimmingFlag = 0x00; |
#define HC_STICKTHRESHOLD 15 |
if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying |
1703,6 → 1732,10 |
HeightTrimming = 0; |
SetPointHeight = ReadingHeight; // update setpoint to current height |
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500; |
if((HoverGasState == HOVER_GAS_STATE_NONE) && (ReadingHeight > 50) ) |
{ |
HoverGasState = HOVER_GAS_STATE_START; // initiate long term filter |
} |
} |
VarioCharacter = '='; |
} |
1728,6 → 1761,7 |
SetPointHeight = ReadingHeight - 400; // setpoint should be 4 meters below actual height to avoid a take off |
if(ParamSet.Height_StickNeutralPoint) StickGasHover = ParamSet.Height_StickNeutralPoint; |
else StickGasHover = RC_GAS_OFFSET; |
HoverGas = GasMixFraction; // set HoverGas to StickGas |
} |
HCGas = HoverGas; // take hover gas (neutral point for PD controller) |
1736,10 → 1770,11 |
if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) ) |
{ |
int16_t GasReduction = 0; |
// from this point the Heigth Control Algorithm is identical for both versions |
if(BaroExpandActive) // baro range expanding active |
{ |
HCGas = HoverGas; // hooer while expanding baro adc range |
HCGas = HoverGas; // hover while expanding baro adc range |
HeightDeviation = 0; |
} // EOF // baro range expanding active |
else // valid data from air pressure sensor |
1748,34 → 1783,60 |
tmp_long1 = ReadingHeight - SetPointHeight; // positive when too high |
LIMIT_MIN_MAX(tmp_long1, -32767L, 32767L); // avoid overflow when casting to int16_t |
HeightDeviation = (int16_t)(tmp_long1); // positive when too high |
tmp_long1 = (tmp_long1 * (int32_t)FCParam.HeightP) / 16L; // p-part |
LIMIT_MIN_MAX(tmp_long1, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
HCGas -= tmp_long1; |
tmp_long1 = (tmp_long1 * (int32_t)FCParam.HeightP) / 32L; // p-part |
LIMIT_MIN_MAX(tmp_long1, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than 2 times the full range makes no sense |
GasReduction += tmp_long1; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int1 = ReadingVario / 8; |
LIMIT_MIN_MAX(tmp_int1, -181, 181); // avoid overflow when squared (181^2 = 32761) |
tmp_int2 = tmp_int1; |
LIMIT_MAX(tmp_int2,8); // limit quadratic part on upward movement to avoid to much gas reduction |
if(tmp_int1 > 0) tmp_int1 = tmp_int1 + (tmp_int2 * tmp_int2) / 4; |
else tmp_int1 = tmp_int1 - (tmp_int2 * tmp_int2) / 4; |
tmp_int1 = (tmp_int1 * (int32_t)FCParam.HeightD ) / 128L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int1,-64 * STICK_GAIN, 32 * STICK_GAIN); |
HCGas -= tmp_int1; |
LIMIT_MIN_MAX(tmp_int1, -127, 128); |
tmp_int1 = (tmp_int1 * (int32_t)FCParam.HeightD ) / 4L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int1,-64 * STICK_GAIN, 64 * STICK_GAIN); |
// reduce d-part when setvalue is changing |
if(HeightTrimmingFlag) tmp_int1 /= 4; |
GasReduction += tmp_int1; |
} // EOF no baro range expanding |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
tmp_long1 = ((ReadingIntegralTop / 128L) * (int32_t) FCParam.Height_ACC_Effect) / (128L / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_long1, -64 * STICK_GAIN, 32 * STICK_GAIN); |
HCGas -= tmp_long1; |
if(FCParam.Height_ACC_Effect) |
{ |
tmp_long1 = ((ReadingIntegralTop / 128L) * (int32_t) FCParam.Height_ACC_Effect) / (128L / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_long1, -32 * STICK_GAIN, 64 * STICK_GAIN); |
GasReduction += tmp_long1; |
} |
// ------------------------ D-Part 3: GpsZ ---------------------------------- |
tmp_int1 = ((int16_t)ParamSet.Height_GPS_Z * (int16_t)NCGpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int1, -64 * STICK_GAIN, 32 * STICK_GAIN); |
HCGas -= tmp_int1; |
LIMIT_MIN_MAX(tmp_int1, -32 * STICK_GAIN, 64 * STICK_GAIN); |
GasReduction += tmp_int1; |
// ----------------------- scale gas reduction relative to hover gas ------------------ |
GasReduction = (int32_t)((int32_t)GasReduction * HoverGas) / 512; |
HCGas -= GasReduction; // apply gas reduction |
// limit deviation from hover point within the target region |
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hover gas not zero |
// limit control output around the hover point |
if( (!HeightTrimming) && (HoverGas > 0) ) // height setpoint is not changed and hover gas not zero |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hover point |
#define DEVIATION_THRESHOLD 60 |
int16_t tmp; |
tmp = abs(HeightDeviation); |
if(tmp <= DEVIATION_THRESHOLD) // small deviation from setpoint |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
} |
else // larger deviation from setpoint |
{ |
// increase limit range proportional to deviation from setpoint |
tmp = (tmp - DEVIATION_THRESHOLD)/32; // substract threshold offset |
if(tmp > 15) tmp = 15; |
if(HeightDeviation > 0) // over setpoint |
{ // decrease lower gas limit |
tmp = (HoverGasMin * (16 - tmp)) / 16; // minimum a factor of 16 smaller |
LIMIT_MIN_MAX(HCGas, tmp, HoverGasMax); // limit gas around the hoover point |
} |
else |
{ // increase upper gas limit |
tmp = (HoverGasMax * (16 + tmp)) / 16; // maximum a factor of 2 higher |
LIMIT_MIN_MAX(HCGas, HoverGasMin, tmp); // limit gas around the hoover point |
} |
} |
} |
// strech control output by inverse attitude projection 1/cos |
1794,7 → 1855,7 |
LIMIT_MAX(FilterHCGas, GasMixFraction); |
} |
// set GasMixFraction to HeightControlGasFilter |
GasMixFraction = FilterHCGas; |
GasMixFraction = FilterHCGas + (GasMixFraction - HoverGas) / 4; |
} // EOF if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT)) |
}// EOF height control active |
else // HC not active |
1816,31 → 1877,28 |
// ----------------- Hover Gas Estimation -------------------------------- |
// Hover gas estimation by averaging gas control output on small z-velocities |
// this is done only if height contol option is selected in global config and aircraft is flying |
if((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_EMERGENCY_LANDING)) |
if((FCFlags & FCFLAG_FLY))// && !(FCFlags & FCFLAG_EMERGENCY_LANDING)) |
{ |
if(HoverGasFilter == 0) HoverGasFilter = HOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation |
if((HoverGasFilter == 0) || (HoverGasState == HOVER_GAS_STATE_START)) HoverGasFilter = HOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation |
if(HoverGasState == HOVER_GAS_STATE_START) HoverGasState = HOVER_GAS_STATE_RUNNING; |
tmp_long2 = (int32_t)GasMixFraction; // take current thrust |
tmp_long2 *= CosAttitude; // apply attitude projection |
tmp_long2 /= 8192; |
// average vertical projected thrust |
if(ModelIsFlying < 4000) // the first 8 seconds |
{ // reduce the time constant of averaging by factor of 16 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/16L); |
HoverGasFilter += 16L * tmp_long2; |
} |
else if(ModelIsFlying < 8000) // the first 16 seconds |
{ // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
HoverGasFilter += 4L * tmp_long2; |
} |
else //later |
if(abs(ReadingVario) < 100) // only on small vertical speed |
{ |
tmp_long2 = (int32_t)GasMixFraction; // take current thrust |
tmp_long2 *= CosAttitude; // apply attitude projection |
tmp_long2 /= 8192; |
// average vertical projected thrust |
if(ModelIsFlying < 2000) // the first 4 seconds |
{ // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/8L); |
HoverGasFilter += 8L * tmp_long2; |
} |
else if(ModelIsFlying < 4000) // the first 8 seconds |
{ // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
HoverGasFilter += 4L * tmp_long2; |
} |
else if(ModelIsFlying < 8000) // the first 16 seconds |
{ // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L); |
HoverGasFilter += 2L * tmp_long2; |
} |
else //later |
{ |
HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
HoverGasFilter += tmp_long2; |
1859,8 → 1917,13 |
HoverGasMax = 255 * STICK_GAIN; |
} |
} //EOF only on small vertical speed |
}// EOF ----------------- Hover Gas Estimation -------------------------------- |
}// EOF if(FCFlags & FCFLAG_FLY) |
else // not flying yet |
{ // reset hover gas filter |
HoverGasState = HOVER_GAS_STATE_NONE; |
HoverGasFilter = 0; |
HoverGas = 0; |
} |
}// EOF ParamSet.Config0 & CFG0_AIRPRESS_SENSOR |
// limit gas to parameter setting |
1908,10 → 1971,7 |
if(GyroIFactor) IPartNick += PPartNick - StickNick; // I-part for attitude control |
else IPartNick += DiffNick; // I-part for head holding |
LIMIT_MIN_MAX(IPartNick, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
if(FCParam.UserParam5 > 50) |
NickMixFraction = (ParamSet.GyroStability * DiffNick)/8 + (IPartNick / Ki); // PID-controller for nick |
else |
NickMixFraction = DiffNick + (IPartNick / Ki); // PID-controller for nick |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Axis |
1920,10 → 1980,7 |
if(GyroIFactor) IPartRoll += PPartRoll - StickRoll; // I-part for attitude control |
else IPartRoll += DiffRoll; // I-part for head holding |
LIMIT_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
if(FCParam.UserParam5 > 50) |
RollMixFraction = (ParamSet.GyroStability * DiffRoll)/8 + (IPartRoll / Ki); // PID-controller for roll |
else |
RollMixFraction = DiffRoll + (IPartRoll / Ki); // PID-controller for roll |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Limiter |
/beta/Code Redesign killagreg/jetimenu.c |
---|
31,11 → 31,19 |
JetiBox_printfxy(0,0,"%2i.%1iV",UBat/10, UBat%10); |
if(NCDataOkay) |
{ |
JetiBox_printfxy(6,0,"%03dm %03d%c", GPSInfo.HomeDistance/10,GPSInfo.HomeBearing, 0xDF); |
JetiBox_printfxy(6,0,"%3d%c %03dm",(int16_t)(YawGyroHeading / GYRO_DEG_FACTOR), 0xDF, GPSInfo.HomeDistance/10); |
//JetiBox_printfxy(6,0,"%03dm %03d%c", GPSInfo.HomeDistance/10,GPSInfo.HomeBearing, 0xDF); |
} |
else |
{ |
JetiBox_printfxy(6,0,"Status"); |
if(NCErrorCode) |
{ |
JetiBox_printfxy(6,0,"ERROR: %2d", NCErrorCode); |
} |
else |
{ |
JetiBox_printfxy(6,0,"Status") |
}; |
} |
JetiBox_printfxy(0,1,"%4i %2i:%02i",Capacity.UsedCapacity,FlightSeconds/60,FlightSeconds%60); |
if(ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) |
50,6 → 58,20 |
JetiBox_printfxy(0,1,"%4i %4i %4i", (int16_t)(IntegralGyroNick/1024), (int16_t)(IntegralGyroRoll/1024), (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR)); |
} |
void Menu_BLTemp(uint8_t key) |
{ //0123456789ABCDEF |
JetiBox_printfxy(0,0,"%3i %3i %3i %3i", Motor[0].Temperature, Motor[1].Temperature, Motor[2].Temperature, Motor[3].Temperature); |
JetiBox_printfxy(0,1,"%3i %3i %3i %3i", Motor[4].Temperature, Motor[5].Temperature, Motor[6].Temperature, Motor[7].Temperature); |
if(RequiredMotors <= 4) |
{ |
JetiBox_printfxy(0,1,"Temperatures "); |
} |
else if(RequiredMotors <= 6) |
{ |
JetiBox_printfxy(8,1,"\%cC ",0xDF); //°C |
} |
} |
void Menu_Battery(uint8_t key) |
{ //0123456789ABCDEF |
JetiBox_printfxy(0,0,"%2i.%1iV %3i.%1iA", UBat/10, UBat%10, Capacity.ActualCurrent/10, Capacity.ActualCurrent%10); |
61,27 → 83,26 |
{ |
if(NCDataOkay) |
{ |
JetiBox_printfxy(0,0,"%2um/s Sat:%2u",GPSInfo.Speed,GPSInfo.NumOfSats) |
JetiBox_printfxy(0,0,"Sat:%02d", GPSInfo.NumOfSats); |
switch (GPSInfo.SatFix) |
{ |
case SATFIX_NONE: |
JetiBox_printfxy(7,0,"NoFix"); |
case SATFIX_3D: |
JetiBox_printfxy(12,0,"3DFx"); |
break; |
case SATFIX_2D: |
JetiBox_printfxy(7,0,"2DFix"); |
JetiBox_printfxy(12,0,"2DFx"); |
break; |
case SATFIX_3D: |
JetiBox_printfxy(7,0,"3DFix"); |
break; |
case SATFIX_NONE: |
default: |
JetiBox_printfxy(7,0,"??Fix"); |
JetiBox_printfxy(12,0,"NoFx"); |
break; |
} |
if(GPSInfo.Flags & FLAG_DIFFSOLN) |
{ |
JetiBox_printfxy(9,0,"/DGPS"); |
JetiBox_printfxy(12,0,"DGPS"); |
} |
JetiBox_printfxy(0,1,"Home:%03dm %03d%c", GPSInfo.HomeDistance/10, GPSInfo.HomeBearing, 0xDF); |
JetiBox_printfxy(0,1,"Home:%3dm %3d%c", GPSInfo.HomeDistance/10, GPSInfo.HomeBearing, 0xDF); // 0xDF = ° |
} |
else |
{ //0123456789ABCDEF |
105,16 → 126,17 |
/* | |
3 - 0 - 1 - 2 - 3 - 0 |
4 - 0 - 1 - 2 - 3 - 4 - 0 |
*/ |
const MENU_ENTRY JetiBox_Menu[] PROGMEM= |
{ // l r u d pHandler |
{3, 1, 0, 0, &Menu_Status }, // 0 |
{4, 1, 0, 0, &Menu_Status }, // 0 |
{0, 2, 1, 1, &Menu_Attitude }, // 1 |
{1, 3, 2, 2, &Menu_Battery }, // 2 |
{2, 0, 3, 3, &Menu_PosInfo }, // 3 |
{1, 3, 2, 2, &Menu_BLTemp }, // 2 |
{2, 4, 3, 3, &Menu_Battery }, // 3 |
{3, 0, 4, 4, &Menu_PosInfo }, // 4 |
}; |
// ----------------------------------------------------------- |
/beta/Code Redesign killagreg/libfc1284.a |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
/beta/Code Redesign killagreg/main.c |
---|
154,7 → 154,7 |
int16_t main (void) |
{ |
uint16_t timer, pollingtimer; |
uint8_t i; |
uint8_t i, FoundMotors = 0; |
// disable interrupts global |
cli(); |
226,6 → 226,7 |
} |
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) |
{ |
FoundMotors++; |
printf("%d ",i+1); |
if(Motor[i].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) printf("(new) "); |
} |
238,6 → 239,9 |
} |
Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter |
} |
// if mismatch between mixer setup and hardware setup |
if(RequiredMotors < FoundMotors) UART_VersionInfo.HardwareError[1] |= DEFECT_MIXER_ERR; |
printf("\n\r==================================="); |
//if(ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) |
292,6 → 296,7 |
pollingtimer = SetDelay(250); |
Debug(ANSI_CLEAR "FC-Start!\n\rFlighttime: %d min", FlightMinutesTotal); // Note: this won't waste flash memory, if #DEBUG is not active |
DebugOut.Status[0] = 0x01 | 0x02; |
// begin of main loop |
while (1) |
315,11 → 320,16 |
SendMotorData(); // the flight control code |
RED_OFF; |
if(RC_Quality) RC_Quality--; |
if(RC_Quality) |
{ |
RC_Quality--; |
UART_VersionInfo.HardwareError[1] &= ~DEFECT_PPM_ERR; |
} |
else |
{ |
PPM_INPUT_ON; // if RC-Quality is lost, enable PPM input (could be disabled by a receiver on uart1) |
PPM_in[0] = 0; // set RSSI to zero on data timeout |
PPM_INPUT_ON; // if RC-Quality is lost, enable PPM input (could be disabled by a receiver on uart1) |
PPM_in[0] = 0; // set RSSI to zero on data timeout |
UART_VersionInfo.HardwareError[1] |= DEFECT_PPM_ERR; |
} |
if(!--I2CTimeout || MissingMotor) // try to reset the i2c if motor is missing or timeout |
331,6 → 341,8 |
I2CTimeout = 5; |
DebugOut.Analog[28]++; // I2C-Error |
FCFlags |= FCFLAG_I2C_ERR; |
UART_VersionInfo.HardwareError[1] |= DEFECT_I2C; |
DebugOut.Status[1] |= 0x02; // BL-Error-Status |
} |
if((BeepModulation == 0xFFFF) && (FCFlags & FCFLAG_MOTOR_RUN) ) |
{ |
356,6 → 368,20 |
static uint16_t counter_minute = 0; |
timer += 20; // every 20 ms |
if(MissingMotor) |
{ |
UART_VersionInfo.HardwareError[1] |= DEFECT_BL_MISSING; |
DebugOut.Status[1] |= 0x02; // BL-Error-Status |
} |
else |
{ |
UART_VersionInfo.HardwareError[1] &= ~DEFECT_BL_MISSING; |
if(I2CTimeout > 6) DebugOut.Status[1] &= ~0x02; // BL-Error-Status |
} |
if(I2CTimeout > 6) UART_VersionInfo.HardwareError[1] &= ~DEFECT_I2C; |
if(PcAccess) PcAccess--; |
else |
{ |
373,19 → 399,26 |
if(NCDataOkay > 200) |
{ |
NCDataOkay--; |
if(!BeepTime) FCFlags &= ~FCFLAG_SPI_RX_ERR; |
FCFlags &= ~FCFLAG_SPI_RX_ERR; |
UART_VersionInfo.HardwareError[1] &= ~DEFECT_SPI_RX_ERR; |
} |
else // no data from NC |
{ |
if(NC_Version.Compatible && BeepModulation == 0xFFFF) |
if(NC_Version.Compatible) |
{ |
BeepTime = 15000; |
BeepModulation = 0xA000; |
FCFlags |= FCFLAG_SPI_RX_ERR; |
UART_VersionInfo.HardwareError[1] |= DEFECT_SPI_RX_ERR; |
if((BeepModulation == 0xFFFF) && (FCFlags & FCFLAG_MOTOR_RUN) ) |
{ |
BeepTime = 15000; |
BeepModulation = 0xA800; |
} |
} |
// set gps control sticks neutral |
GPSStickNick = 0; |
GPSStickNick = 0; |
FCFlags |= FCFLAG_SPI_RX_ERR; |
FromNaviCtrl.CompassHeading = -1; |
NCDataOkay = 0; |
} |
#endif |
if(UBat < LowVoltageWarning) |
/beta/Code Redesign killagreg/makefile |
---|
5,12 → 5,12 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 79 |
VERSION_PATCH = 11 |
VERSION_MINOR = 80 |
VERSION_PATCH = 2 |
VERSION_SERIAL_MAJOR = 11 # Serial Protocol Major Version |
VERSION_SERIAL_MINOR = 0 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 13 # SPI Protocol Version |
NC_SPI_COMPATIBLE = 14 # SPI Protocol Version |
#------------------------------------------------------------------- |
#OPTIONS |
107,6 → 107,21 |
ifeq ($(VERSION_PATCH), 17) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)r_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 18) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)s_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 19) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)t_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 20) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)u_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 21) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)v_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 22) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)w_SVN$(REV) |
endif |
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
/beta/Code Redesign killagreg/menu.c |
---|
113,15 → 113,18 |
LCD_printfxy(0,0,"++ Flight-Ctrl ++"); |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",BoardRelease/10,BoardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a'); |
LCD_printfxy(0,2,"Setting: %d %s", GetActiveParamSet(), Mixer.Name); |
if(I2CTimeout < 6) |
if(UART_VersionInfo.HardwareError[0]) LCD_printfxy(0,3,"Hardware Error 1:%d !!",UART_VersionInfo.HardwareError[0]) |
else |
if (MissingMotor) |
{ |
LCD_printfxy(0,3,"I2C Error!"); |
} |
else if (MissingMotor) |
{ |
LCD_printfxy(0,3,"Missing BL-Ctrl:%d", MissingMotor); |
} |
else LCD_printfxy(0,3,"(c) Holger Buss"); |
else |
if(UART_VersionInfo.HardwareError[1] & DEFECT_MIXER_ERR) LCD_printfxy(0,3,"Mixer Error!") |
else |
//if(UART_VersionInfo.HardwareError[1]) LCD_printfxy(0,3,"Error 2:%d !!",UART_VersionInfo.HardwareError[1]) |
//else |
if(I2CTimeout < 6) LCD_printfxy(0,3,"I2C ERROR!!!") |
break; |
case 1:// Height Control Menu Item |
if(ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) |
/beta/Code Redesign killagreg/spi.c |
---|
164,12 → 164,12 |
uint8_t SPITransferCompleted, SPI_ChkSum; |
uint8_t SPI_RxDataValid = 0; |
uint8_t NCDataOkay = 0; |
uint8_t NCDataOkay = 250; |
uint8_t NCSerialDataOkay = 0; |
int8_t NCGpsZ = 0; |
int8_t NCRotate_C = 32, NCRotate_S = 0; |
uint8_t NCErrorCode = 0; |
uint8_t SPI_CommandSequence[] = {SPI_FCCMD_STICK, SPI_FCCMD_USER, SPI_FCCMD_PARAMETER1, SPI_FCCMD_STICK, SPI_FCCMD_MISC, SPI_FCCMD_VERSION, SPI_FCCMD_STICK, SPI_FCCMD_SERVOS, SPI_FCCMD_ACCU}; |
uint8_t SPI_CommandCounter = 0; |
303,6 → 303,9 |
ToNaviCtrl.Param.Byte[2] = VERSION_PATCH; |
ToNaviCtrl.Param.Byte[3] = NC_SPI_COMPATIBLE; |
ToNaviCtrl.Param.Byte[4] = BoardRelease; |
ToNaviCtrl.Param.Byte[5] = UART_VersionInfo.HardwareError[0]; |
ToNaviCtrl.Param.Byte[6] = UART_VersionInfo.HardwareError[1]; |
ToNaviCtrl.Param.Byte[7] = UART_VersionInfo.HardwareError[2]; |
break; |
case SPI_FCCMD_SERVOS: |
361,6 → 364,9 |
NC_Version.Patch = FromNaviCtrl.Param.Byte[2]; |
NC_Version.Compatible = FromNaviCtrl.Param.Byte[3]; |
NC_Version.Hardware = FromNaviCtrl.Param.Byte[4]; |
DebugOut.Status[0] |= FromNaviCtrl.Param.Byte[5]; |
NCErrorCode = FromNaviCtrl.Param.Byte[6]; |
DebugOut.Status[1] = (DebugOut.Status[1] & (0x01|0x02)) | (FromNaviCtrl.Param.Byte[6] & (0x04 | 0x08)); |
break; |
case SPI_NCCMD_GPSINFO: |
367,6 → 373,7 |
GPSInfo.Flags = FromNaviCtrl.Param.Byte[0]; |
GPSInfo.NumOfSats = FromNaviCtrl.Param.Byte[1]; |
GPSInfo.SatFix = FromNaviCtrl.Param.Byte[2]; |
GPSInfo.Speed = FromNaviCtrl.Param.Byte[3]; |
GPSInfo.HomeDistance = FromNaviCtrl.Param.Int[2]; |
GPSInfo.HomeBearing = FromNaviCtrl.Param.sInt[3]; |
break; |
/beta/Code Redesign killagreg/spi.h |
---|
106,6 → 106,7 |
uint8_t Flags; // Status Flags |
uint8_t NumOfSats; // number of satelites |
uint8_t SatFix; // type of satfix |
uint8_t Speed; // ground speed in m/s |
uint16_t HomeDistance; // distance to Home in dm |
int16_t HomeBearing; // bearing to home in deg |
} __attribute__((packed)) GPSInfo_t; |
116,6 → 117,7 |
extern uint8_t NCSerialDataOkay; |
extern int8_t NCGpsZ; |
extern int8_t NCRotate_C, NCRotate_S; |
extern uint8_t NCErrorCode; |
void SPI_MasterInit(void); |
/beta/Code Redesign killagreg/uart0.c |
---|
524,6 → 524,8 |
memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer) - 1); |
MixerTable_WriteToEEProm(); |
tempchar1 = 1; |
// ?? the bit is cleared here but the error check is only done once during startup |
UART_VersionInfo.HardwareError[1] &= ~DEFECT_MIXER_ERR; |
} |
else |
{ |
/beta/Code Redesign killagreg/uart0.h |
---|
30,8 → 30,8 |
typedef struct |
{ |
uint8_t Digital[2]; |
uint16_t Analog[32]; // Debugvalues |
uint8_t Status[2]; // system state flags |
uint16_t Analog[32]; // debug values |
} __attribute__((packed)) DebugOut_t; |
extern DebugOut_t DebugOut; |
38,11 → 38,11 |
typedef struct |
{ |
int16_t AngleNick; // in 0.1 deg |
int16_t AngleRoll; // in 0.1 deg |
int16_t Heading; // in 0.1 deg |
int8_t Centroid[3]; |
uint8_t reserve[5]; |
int16_t AngleNick; // in 0.1 deg |
int16_t AngleRoll; // in 0.1 deg |
int16_t Heading; // in 0.1 deg |
int8_t Centroid[3]; |
uint8_t reserve[5]; |
} __attribute__((packed)) Data3D_t; |
63,6 → 63,22 |
extern ExternControl_t ExternControl; |
// bitmask for UART_VersionInfo_t.HardwareError[0] |
#define DEFECT_G_NICK 0x01 |
#define DEFECT_G_ROLL 0x02 |
#define DEFECT_G_YAW 0x04 |
#define DEFECT_A_NICK 0x08 |
#define DEFECT_A_ROLL 0x10 |
#define DEFECT_A_TOP 0x20 |
#define DEFECT_PRESSURE 0x40 |
#define DEFECT_CAREFREE_ERR 0x80 |
// bitmask for UART_VersionInfo_t.HardwareError[1] |
#define DEFECT_I2C 0x01 |
#define DEFECT_BL_MISSING 0x02 |
#define DEFECT_SPI_RX_ERR 0x04 |
#define DEFECT_PPM_ERR 0x08 |
#define DEFECT_MIXER_ERR 0x10 |
typedef struct |
{ |
uint8_t SWMajor; |
70,8 → 86,9 |
uint8_t ProtoMajor; |
uint8_t ProtoMinor; |
uint8_t SWPatch; |
uint8_t Reserved[5]; |
} __attribute__((packed)) UART_VersionInfo_t; |
uint8_t HardwareError[5]; |
} __attribute__((packed)) UART_VersionInfo_t; |
extern UART_VersionInfo_t UART_VersionInfo; |
#endif //_UART0_H |
/beta/Code Redesign killagreg/version.txt |
---|
381,44 → 381,34 |
- Einführung eines Vario-Zeichens (+/-/ ) auf der Jetibox |
- BL-Timeout beim Start erhöht |
0.79b H. Buss + G.Stobrawa 20.4.2010 |
0.80a H. Buss + G.Stobrawa 20.5.2010 - 22.7.2010 |
- Motoren Starten nicht ohne Kalibrierung |
- Checksummen gesicherte Datenablage im EEProm |
- statt 8 nun 11 Bit Auflösung der Lageregekung |
- MotorSmooth()-Routine entfernt |
- Unterstützung der BL2.0-Regler |
0.79c H. Buss + G.Stobrawa 20.5.2010 |
- Unterstützung der BL2.0-Regler Konfiguration via MK-Tool |
- Erste Version von "Head-Free" |
- GPS-Login-Time auf 2 reduziert |
0.79d H. Buss 31.5.2010 |
- statt 8 nun 10 Bit Auflösung der Lageregekung |
- Unterstützung der BL2.0-Regler Konfiguration via MK-Tool |
- Parametersätze werden per I2C an die BL-Regler durchgereicht |
- "Care-Free" implementiert |
- Freie Belegung der "Vorne"-Richtung |
0.79e H.Buss 05.6.2010 |
- MotorSmooth auf UserParameter5 |
0.79f H.Buss 07.5.2010 |
- Bug im Lageregler -> zurück auf 10Bit Auflösung |
0.79j S.Pendsa & H.Buss 22.06.2010 |
- RECEIVER_SPEKTRUM_EXP |
0.79k H.Buss |
- nur wenn Motoren laufen: Beepen, wenn Carefree ohne Navi oder ohne gültigen MK3Mag-Wert |
- nur wenn Motoren laufen: Beepen wenn NC plötzlich ausfällt |
- #define RECEIVER_SPEKTRUM_EXP eingeführt |
- #define SWITCH_LEARNS_CAREFREE eingeführt |
- Piepen, wenn NC-Kommunikation wegfällt |
- Da kann man mit UserParameter5 wieder das Verhalten der 0.79b herstellen |
0.79l H.Buss |
- Schwerpunktanzeige in den 3D-Daten |
- Gyro Stability |
- EEPROM-Kompatibilität auf 84 |
- Checksummen gesicherte Datenablage im EEProm |
- JetiMenü: bis acht Temperaturen der BL-Regler |
- GPSInfo.Speed im Jeti-Display |
- Compass-Richtung nun im Jeti-menü |
Anpassunge n bzgl. V0.79l |
G.Stobrawa 26.6.2010 |
0.80b H. Buss 23.7.2010 |
- Fehlerdiagnose implementiert |
0.80c H. Buss 23.7.2010 |
- ErrorCode aufs Jeti-Display |
Anpassungen bzgl. V0.80c |
G.Stobrawa 25.7.2010 |
- Code stärker modularisiert und restrukturiert |
- viele Kommentare zur Erklärug eingefügt |
- konsequent englische Variablennamen |
425,7 → 415,7 |
- Makefile: EXT=NAVICTRL Unterstützung der SPI Communikation zum Naviboard |
- Makefile: EXT=MK3MAG Unerstützung des MK3MAG/CMPS03 direkt an FC und Conrad UBLOX Modul |
- Makefile: EXT=MK3MAG Unerstützung des MK3MAG/CMPS03 direkt an FC |
Weiter Detail siehe Readme.txt im Verzeichnis Hex-Files. |