void SucheLuftruckOffset(void)
{
unsigned int off;
ExpandBaro = 0;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
{
unsigned char off2;
OCR0A = 150;
off2 = GetParamByte(PID_PRESSURE_OFFSET);
if(off2 < 230) off2 += 10;
OCR0B = off2;
Delay_ms_Mess(100);
if(MessLuftdruck > DESIRED_H_ADC) off2 = 240;
for(; off2 >= 5; off2 -= 5)
{
OCR0B = off2;
Delay_ms_Mess(50);
printf("*");
if(MessLuftdruck > DESIRED_H_ADC) break;
}
SetParamByte(PID_PRESSURE_OFFSET, off2);
if(off2 >= 15) off = 140; else off = 0;
for(; off < 250;off++)
{
OCR0A = off;
Delay_ms_Mess(50);
printf(".");
if(MessLuftdruck < DESIRED_H_ADC) break;
}
DruckOffsetSetting = off;
}
#else
off = GetParamByte(PID_PRESSURE_OFFSET);
if(off > 20) off -= 10;
OCR0A = off;
Delay_ms_Mess(100);
if(MessLuftdruck < DESIRED_H_ADC) off = 0;
for(; off < 250;off++)
{
OCR0A = off;
Delay_ms_Mess(50);
printf(".");
if(MessLuftdruck < DESIRED_H_ADC) break;
}
DruckOffsetSetting = off;
SetParamByte(PID_PRESSURE_OFFSET, off);
#endif
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && (DruckOffsetSetting < 10 || DruckOffsetSetting >= 245)) VersionInfo.HardwareError[0] |= FC_ERROR0_PRESSURE;
OCR0A = off;
Delay_ms_Mess(300);
}
void SucheLuftruckOffset(void)
{
unsigned int off;
ExpandBaro = 0;
CalcExpandBaroStep();
off = GetParamByte(PID_PRESSURE_OFFSET);
if(off < 240) off += 10;
OCR0A = off;
OCR0B = 255-off;
Delay_ms_Mess(150);
if(MessLuftdruck > DESIRED_H_ADC) off = 240;
for(; off > 5; off--)
{
OCR0A = off;
OCR0B = 255-off;
Delay_ms_Mess(100);
printf(".");
if(MessLuftdruck > DESIRED_H_ADC) break;
}
DruckOffsetSetting = off;
SetParamByte(PID_PRESSURE_OFFSET, off);
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && (DruckOffsetSetting < 10 || DruckOffsetSetting >= 230)) VersionInfo.HardwareError[0] |= FC_ERROR0_PRESSURE;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + correction of the altitude error in higher altitudes
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
CalAthmospheare = 15; // re-claibrated from 16 to 15 at 2.09 -> the baro-Altimeter was about 7% too high
if(ACC_AltitudeControl)
{
if(PlatinenVersion < 23) { if(off < 140) CalAthmospheare += (160 - off) / 26; }
// else { if(off < 170) CalAthmospheare += (188 - off) / 19; }
else { if(off < 170) CalAthmospheare += (188 - off) / 15; } // rescaled at 2.09
}
Luftdruck = MessLuftdruck * CalAthmospheare;
#endif
Delay_ms_Mess(300);
}
void ParamSet_Init(void)
{
uint8_t channel_backup = 0, bad_params = 0, ee_default = 0,i;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(PlatinenVersion != GetParamByte(PID_HARDWARE_VERSION))
{
if(PlatinenVersion == 22 && GetParamByte(PID_HARDWARE_VERSION) == 21 && !(PIND & 0x10)) SetParamByte(PID_EE_REVISION,0); // reset the Settings if the Version changed to V2.2
SetParamByte(PID_HARDWARE_VERSION,PlatinenVersion); // Remember the Version number
wdt_enable(WDTO_15MS); // Reset-Commando
printf("\n\r--> Hardware Version Byte Changed <--");
while(1);
}
#endif
if((EEPARAM_REVISION) != GetParamByte(PID_EE_REVISION))
{
ee_default = 1; // software update or forced by mktool
}
// 1st check for a valid channel backup in eeprom
i = EEProm_Checksum(EEPROM_ADR_CHANNELS, sizeof(EE_Parameter.Kanalbelegung));
if(i == eeprom_read_byte((uint8_t*)(EEPROM_ADR_CHANNELS + sizeof(EE_Parameter.Kanalbelegung)))) channel_backup = 1;
// parameter check
// check all 5 parameter settings
for (i = 1;i < 6; i++)
{
if(ee_default || !ParamSet_ReadFromEEProm(i)) // could not read paramset from eeprom
{
bad_params = 1;
printf("\n\rGenerating default Parameter Set %d",i);
switch(i)
{
case 1:
ParamSet_DefaultSet1(); // Fill ParamSet Structure to default parameter set 1 (Sport)
break;
case 2:
ParamSet_DefaultSet2(); // Normal
break;
default:
ParamSet_DefaultSet3(); // Easy
break;
}
if(channel_backup) // if we have an channel mapping backup in eeprom
{ // restore it from eeprom
eeprom_read_block((void *)EE_Parameter.Kanalbelegung, (void*)(EEPROM_ADR_CHANNELS), sizeof(EE_Parameter.Kanalbelegung));
}
else
{ // use default mapping
ParamSet_DefaultStickMapping();
}
ParamSet_WriteToEEProm(i);
}
}
if(bad_params) // at least one of the parameter settings were invalid
{
// default-Setting is parameter set 3
SetActiveParamSet(3);
}
// read active parameter set to ParamSet stucture
i = GetActiveParamSet();
ParamSet_ReadFromEEProm(i);
printf("\n\rUsing Parameter Set %d", i);
// load mixer table
if(GetParamByte(PID_EE_REVISION) == 0xff || !MixerTable_ReadFromEEProm() )
{
printf("\n\rGenerating default Mixer Table");
MixerTable_Default(); // Quadro
MixerTable_WriteToEEProm();
}
if(ee_default) SetParamByte(PID_EE_REVISION, (EEPARAM_REVISION));
// determine motornumber
RequiredMotors = 0;
for(i = 0; i < 16; i++)
{
if(Mixer.Motor[i][MIX_GAS] > 0) RequiredMotors++;
}
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors);
PrintLine();// ("\n\r===================================");
}
//############################################################################
//Hauptprogramm
int main (void)
//############################################################################
{
unsigned int timer,i,timer2 = 0, timerPolling;
DDRB = 0x00;
PORTB = 0x00;
for(timer = 0; timer < 1000; timer++); // verzögern
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
PlatinenVersion = 21;
#else
if(PINB & 0x01)
{
if(PINB & 0x02) PlatinenVersion = 13;
else PlatinenVersion = 11;
}
else
{
if(PINB & 0x02) PlatinenVersion = 20;
else PlatinenVersion = 10;
}
#endif
DDRC = 0x81; // SCL
DDRC |=0x40; // HEF4017 Reset
PORTC = 0xff; // Pullup SDA
DDRB = 0x1B; // LEDs und Druckoffset
PORTB = 0x01; // LED_Rot
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5
PORTD = 0x47; // LED
HEF4017R_ON;
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
WDTCSR = 0;
beeptime = 2500;
StickGier = 0; PPM_in[K_GAS] = 0; StickRoll = 0; StickNick = 0;
if(PlatinenVersion >= 20) GIER_GRAD_FAKTOR = 1220; else GIER_GRAD_FAKTOR = 1291; // unterschiedlich für ME und ENC
ROT_OFF;
Timer_Init();
TIMER2_Init();
UART_Init();
rc_sum_init();
ADC_Init();
I2C_Init(1);
SPI_MasterInit();
Capacity_Init();
LIBFC_Init();
GRN_ON;
sei();
ParamSet_Init();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Check connected BL-Ctrls
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Check connected BL-Ctrls
BLFlags |= BLFLAG_READ_VERSION;
motor_read = 0; // read the first I2C-Data
SendMotorData();
timer = SetDelay(500);
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
printf("\n\rFound BL-Ctrl: ");
timer = SetDelay(4000);
for(i=0; i < MAX_MOTORS; i++)
{
SendMotorData();
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
if(Mixer.Motor[i][0] > 0) // wait max 4 sec for the BL-Ctrls to wake up
{
while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) )
{
SendMotorData();
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
}
}
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
{
printf("%d",i+1);
FoundMotors++;
// if(Motor[i].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) printf("(new) ");
}
}
for(i=0; i < MAX_MOTORS; i++)
{
if(!(Motor[i].State & MOTOR_STATE_PRESENT_MASK) && Mixer.Motor[i][0] > 0)
{
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1);
ServoActive = 2; // just in case the FC would be used as camera-stabilizer
}
Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter
}
printf("\n\r===================================");
if(RequiredMotors < FoundMotors) VersionInfo.HardwareError[1] |= FC_ERROR1_MIXER;
//if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
{
printf("\n\rCalibrating pressure sensor..");
timer = SetDelay(1000);
SucheLuftruckOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
SetNeutral(0);
ROT_OFF;
beeptime = 2000;
ExternControl.Digital[0] = 0x55;
FlugMinuten = (unsigned int)GetParamByte(PID_FLIGHT_MINUTES) * 256 + (unsigned int)GetParamByte(PID_FLIGHT_MINUTES + 1);
FlugMinutenGesamt = (unsigned int)GetParamByte(PID_FLIGHT_MINUTES_TOTAL) * 256 + (unsigned int)GetParamByte(PID_FLIGHT_MINUTES_TOTAL + 1);
if((FlugMinutenGesamt == 0xFFFF) || (FlugMinuten == 0xFFFF))
{
FlugMinuten = 0;
FlugMinutenGesamt = 0;
}
printf("\n\rFlight-time %u min Total:%u min", FlugMinuten, FlugMinutenGesamt);
printf("\n\rControl: ");
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Normal (ACC-Mode)");
LcdClear();
I2CTimeout = 5000;
WinkelOut.Orientation = 1;
LipoDetection(1);
LIBFC_ReceiverInit(EE_Parameter.Receiver);
printf("\n\r===================================\n\r");
//SpektrumBinding();
timer = SetDelay(2000);
timerPolling = SetDelay(250);
Debug(ANSI_CLEAR "FC-Start!\n\rFlugzeit: %d min", FlugMinutenGesamt); // Note: this won't waste flash memory, if #DEBUG is not active
DebugOut.Status[0] = 0x01 | 0x02;
JetiBeep = 0;
while (1)
{
if (JetiUpdateModeActive) while (1);
if(CheckDelay(timerPolling))
{
timerPolling = SetDelay(100);
LIBFC_Polling();
}
if(UpdateMotor && AdReady) // ReglerIntervall
{
UpdateMotor=0;
if(WinkelOut.CalcState) CalMk3Mag();
else MotorRegler();
SendMotorData();
ROT_OFF;
if(SenderOkay) { SenderOkay--; VersionInfo.HardwareError[1] &= ~FC_ERROR1_PPM; }
else
{
TIMSK1 |= _BV(ICIE1); // enable PPM-Input
PPM_in[0] = 0; // set RSSI to zero on data timeout
VersionInfo.HardwareError[1] |= FC_ERROR1_PPM;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160;
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!--I2CTimeout || MissingMotor)
{
if(!I2CTimeout)
{
I2C_Reset();
I2CTimeout = 5;
DebugOut.Analog[28]++; // I2C-Error
VersionInfo.HardwareError[1] |= FC_ERROR1_I2C;
DebugOut.Status[1] |= 0x02; // BL-Error-Status
}
if((BeepMuster == 0xffff) && MotorenEin)
{
beeptime = 10000;
BeepMuster = 0x0080;
}
}
else
{
ROT_OFF;
if(!beeptime)
{
VersionInfo.HardwareError[1] &= ~FC_ERROR1_I2C;
}
}
if(!UpdateMotor)
{
if(CalculateServoSignals) CalculateServo();
DatenUebertragung();
BearbeiteRxDaten();
if(CheckDelay(timer))
{
static unsigned char second;
timer += 20; // 20 ms interval
if(MissingMotor)
{
VersionInfo.HardwareError[1] |= FC_ERROR1_BL_MISSING;
DebugOut.Status[1] |= 0x02; // BL-Error-Status
}
else
{
VersionInfo.HardwareError[1] &= ~FC_ERROR1_BL_MISSING;
if(I2CTimeout > 6) DebugOut.Status[1] &= ~0x02; // BL-Error-Status
}
if(I2CTimeout > 6) VersionInfo.HardwareError[1] &= ~FC_ERROR1_I2C;
if(PcZugriff) PcZugriff--;
else
{
ExternControl.Config = 0;
ExternStickNick = 0;
ExternStickRoll = 0;
ExternStickGier = 0;
if(BeepMuster == 0xffff && SenderOkay == 0)
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if(NaviDataOkay > 200)
{
NaviDataOkay--;
VersionInfo.HardwareError[1] &= ~FC_ERROR1_SPI_RX;
}
else
{
if(NC_Version.Compatible)
{
VersionInfo.HardwareError[1] |= FC_ERROR1_SPI_RX;
if(BeepMuster == 0xffff && MotorenEin)
{
beeptime = 15000;
BeepMuster = 0xA800;
}
}
GPS_Nick = 0;
GPS_Roll = 0;
//if(!beeptime)
FromNaviCtrl.CompassValue = -1;
NaviDataOkay = 0;
}
if(UBat < BattLowVoltageWarning)
{
FC_StatusFlags |= FC_STATUS_LOWBAT;
if(BeepMuster == 0xffff)
{
beeptime = 6000;
BeepMuster = 0x0300;
}
}
else if(!beeptime) FC_StatusFlags &= ~FC_STATUS_LOWBAT;
SPI_StartTransmitPacket();
SendSPI = 4;
if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden
else
if(++second == 49)
{
second = 0;
FlugSekunden++;
}
if(++timer2 == 2930) // eine Minute
{
timer2 = 0;
FlugMinuten++;
FlugMinutenGesamt++;
SetParamByte(PID_FLIGHT_MINUTES,FlugMinuten / 256);
SetParamByte(PID_FLIGHT_MINUTES+1,FlugMinuten % 256);
SetParamByte(PID_FLIGHT_MINUTES_TOTAL,FlugMinutenGesamt / 256);
SetParamByte(PID_FLIGHT_MINUTES_TOTAL+1,FlugMinutenGesamt % 256);
timer = SetDelay(20); // falls "timer += 20;" mal nicht geht
}
}
LED_Update();
Capacity_Update();
} //else DebugOut.Analog[26]++;
}
if(!SendSPI) { SPI_TransmitByte(); }
}
return (1);
}