//############################################################################
//主程序
int main (void)
//############################################################################
{
unsigned int timer;
//unsigned int timer2 = 0;
DDRB = 0x00;
PORTB = 0x00;
for(timer = 0; timer < 1000; timer++);
if(PINB & 0x01) PlatinenVersion = 11;
else PlatinenVersion = 10;
DDRC = 0x81; // SCL
PORTC = 0xff; // Pullup SDA
DDRB = 0x1B; // LEDs und offset
PORTB = 0x01; // LED_Rot
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5
DDRD |=0x80; // J7
PORTD = 0xF7; // LED
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
WDTCSR = 0;
beeptime = 2000;
StickGier = 0;
PPM_in[K_GAS] = 0;
StickRoll = 0;
StickNick = 0;
ROT_OFF;
Timer_Init();
UART_Init();
rc_sum_init();
ADC_Init();
i2c_init();
SPI_MasterInit();
sei();
VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
VersionInfo.Nebenversion = VERSION_NEBENVERSION;
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_HAUPTVERSION, VERSION_NEBENVERSION,VERSION_INDEX + 'a');
printf("\n\r==============================");
GRN_ON;
#define EE_DATENREVISION 69 // wird angepasst, wenn sich die EEPROM-Daten ge鋘dert haben
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)
{
printf("\n\rInit. EEPROM: Generiere Default-Parameter...");
DefaultKonstanten1();
for (unsigned char i=0; i<6; i++)
{
if(i==2) DefaultKonstanten2(); // 相机
if(i==3) DefaultKonstanten3(); // 初学者
if(i>3) DefaultKonstanten2(); // 相机
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
}
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);
}
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
{
printf("\n\rACC nicht abgeglichen!");
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
{
printf("\n\rAbgleich Luftdrucksensor..");
timer = SetDelay(1000);
SucheLuftruckOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
SetNeutral();
ROT_OFF;
beeptime = 2000;
ExternControl.Digital[0] = 0x55;
printf("\n\rSteuerung: ");
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Neutral");
printf("\n\n\r");
LcdClear();
I2CTimeout = 5000;
while (1)
{
if(UpdateMotor) // 间隙调整
{
SPI_TransmitByte(); //#
UpdateMotor=0;
MotorRegler();
SendMotorData();
ROT_OFF;
if(PcZugriff) PcZugriff--;
else
{
DubWiseKeys[0] = 0;
DubWiseKeys[1] = 0;
ExternControl.Config = 0;
ExternStickNick = 0;
ExternStickRoll = 0;
ExternStickGier = 0;
}
if(SenderOkay) SenderOkay--;
if(!I2CTimeout)
{
I2CTimeout = 5;
i2c_reset();
if((BeepMuster == 0xffff) && MotorenEin)
{
beeptime = 10000;
BeepMuster = 0x0080;
}
}
else
{
I2CTimeout--;
ROT_OFF;
}
if(SIO_DEBUG && !UpdateMotor)
{
DatenUebertragung();
BearbeiteRxDaten();
}
else BearbeiteRxDaten();
if(CheckDelay(timer))
{
if(UBat < EE_Parameter.UnterspannungsWarnung)
{
if(BeepMuster == 0xffff)
{
beeptime = 6000;
BeepMuster = 0x0300;
}
}
SPI_StartTransmitPacket();//#
timer = SetDelay(100);
}
}
}
return (1);
}
//############################################################################
//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);
}
