u8 UART0_UBXSendCFGMsg(u8* pData, u16 Len)
{
u32 timeout;
u8 retval = 0;
// if data are not a CFG MSG
if(pData[0]!= UBX_CLASS_CFG) return(retval);
// prepare rx msg filter
UbxMsg.Hdr.Class = UBX_CLASS_ACK;
UbxMsg.Hdr.Id = 0xFF;
UbxMsg.Hdr.Length = 0;
UbxMsg.ClassMask = 0xFF;
UbxMsg.IdMask = 0x00;
UbxMsg.Status = INVALID;
UART0_UBXSendMsg(pData, Len);
// check for acknowledge msg
timeout = SetDelay(100);
do
{
if(UbxMsg.Status == NEWDATA) break;
}while(!CheckDelay(timeout));
if(UbxMsg.Status == NEWDATA)
{ // 2 bytes payload
if((UbxMsg.Data[0] == pData[0]) && (UbxMsg.Data[1] == pData[1]) && (UbxMsg.Hdr.Length == 2)) retval = UbxMsg.Hdr.Id;
}
UbxMsg.Status = INVALID;
return(retval);
}
u8 UART0_GetUBXVersion(void)
{
u32 timeout;
u8 retval = 0xFF;
u8 ubxmsg[]={0x0A, 0x04, 0x00, 0x00}; //MON-VER
// prepare rx msg filter
UbxMsg.Hdr.Class = 0x0A;
UbxMsg.Hdr.Id = 0x04;
UbxMsg.Hdr.Length = 0;
UbxMsg.ClassMask = 0xFF;
UbxMsg.IdMask = 0xFF;
UbxMsg.Status = INVALID;
UART0_UBXSendMsg(ubxmsg, sizeof(ubxmsg));
// check for answer
timeout = SetDelay(100);
do
{
if(UbxMsg.Status == NEWDATA) break;
}while(!CheckDelay(timeout));
if((UbxMsg.Hdr.Length >= 40) && (UbxMsg.Status == NEWDATA))
{
UbxMsg.Data[4] = 0; //Only the fisrt 4 chsracters
UbxMsg.Data[39] = 0;
UART1_PutString(" V");
UART1_PutString((u8*)&UbxMsg.Data);
UART1_PutString(" HW:");
UART1_PutString((u8*)&UbxMsg.Data[30]);
retval = UbxMsg.Data[0]-'0'; // major sw version
}
UbxMsg.Status = INVALID;
return(retval);
}
void UART0_TransmitTxData(void)
{
if(DebugUART == UART0) return;
UART0_Transmit(); // output pending bytes in tx buffer
if(UART0_tx_buffer.Locked == TRUE) return;
else if(UART0_Request_ErrorMessage && (UART0_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&UART0_tx_buffer, 'E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
UART0_Request_ErrorMessage = FALSE;
}
else if(UART0_Request_VersionInfo && (UART0_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&UART0_tx_buffer, 'V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
UART0_Request_VersionInfo = FALSE;
}
else if(( ((UART0_NaviData_Interval >0) && CheckDelay(UART0_NaviData_Timer) ) || UART0_Request_NaviData) && (UART0_tx_buffer.Locked == FALSE))
{
NaviData.Errorcode = ErrorCode;
MKProtocol_CreateSerialFrame(&UART0_tx_buffer, 'O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
UART0_NaviData_Timer = SetDelay(UART0_NaviData_Interval);
UART0_Request_NaviData = FALSE;
}
UART0_Transmit(); // output pending bytes in tx buffer
}
u8 UART0_GetMKOSDVersion(void)
{
u32 timeout;
u8 msg[64];
u8 retval = 0;
MKOSD_VersionInfo.SWMajor = 0xFF;
MKOSD_VersionInfo.SWMinor = 0xFF;
MKOSD_VersionInfo.SWPatch = 0xFF;
if(UART0_Muxer != UART0_MKGPS) UART0_Connect_to_MKGPS(UART0_BAUD_RATE);
while(UART0_tx_buffer.Locked == TRUE) UART0_Transmit(); // output pending bytes in tx buffer;
MKProtocol_CreateSerialFrame(&UART0_tx_buffer, 'v', MKOSD_ADDRESS, 0); // request for version info
while(UART0_tx_buffer.Locked == TRUE) UART0_Transmit(); // output pending bytes in tx buffer;
timeout = SetDelay(500);
do
{
UART0_ProcessRxData();
if(MKOSD_VersionInfo.SWMajor != 0xFF) break;
}while(!CheckDelay(timeout));
if(MKOSD_VersionInfo.SWMajor != 0xFF)
{
sprintf(msg, "\n\r MK-OSD V%d.%d%c", MKOSD_VersionInfo.SWMajor, MKOSD_VersionInfo.SWMinor, 'a'+MKOSD_VersionInfo.SWPatch);
UART1_PutString(msg);
retval = 1;
}
//else UART1_PutString("\n\r No version information from MK-OSD.");
return(retval);
}
void LipoDetection(unsigned char print)
{
#define MAX_CELL_VOLTAGE 43 // max cell volatage for LiPO
unsigned int timer, cells;
if(print) printf("\n\rBatt:");
if(EE_Parameter.UnterspannungsWarnung < 50) // automatische Zellenerkennung
{
timer = SetDelay(500);
if(print) while (!CheckDelay(timer));
// up to 6s LiPo, less than 2s is technical impossible
for(cells = 2; cells < 7; cells++)
{
if(UBat < cells * MAX_CELL_VOLTAGE) break;
}
BattLowVoltageWarning = cells * EE_Parameter.UnterspannungsWarnung;
if(print)
{
Piep(cells, 200);
printf(" %d Cells ", cells);
}
}
else BattLowVoltageWarning = EE_Parameter.UnterspannungsWarnung;
if(print) printf(" Low warning level: %d.%d",BattLowVoltageWarning/10,BattLowVoltageWarning%10);
}
//----------------------------------------------------------------------------------------------------
// gobal logging handler
void Logging_Update(void)
{
static uint16_t logtimer = 0;
static logfilestate_t logstate = LOGFILE_IDLE;
if(SD_SWITCH) // a card is in slot
{
if(CheckDelay(logtimer))
{
logtimer = SetDelay(10); // faster makes no sense
// call the logger handlers if no error has occured
if(logstate != LOGFILE_ERROR) logstate = Logging_KML(LogCfg.KML_Interval);
if(logstate != LOGFILE_ERROR) logstate = Logging_GPX(LogCfg.GPX_Interval);
// a logging error has occured
if(logstate == LOGFILE_ERROR)
{
if(Fat16_IsValid()) // wait for reinizialization of fat16 from outside
{
Logging_Init(); // initialize the logs
logstate = LOGFILE_IDLE;
logtimer = SetDelay(10); // try next log in 10 mili sec
}
else
{ // retry in 5 seconds
logtimer = SetDelay(5000); // try again in 5 sec
}
} //EOF logfile error
} // EOF CheckDelay
}// EOF Card in Slot
}
//############################################################################
//启动代码,pwm为启动时的占空比
char Anwerfen(unsigned char pwm)
//############################################################################
{
unsigned long timer = 300,i;
DISABLE_SENSE_INT; //关掉模拟比较器中断
PWM = 5;
SetPWM();
Manuell();
// Delay_ms(200);
MinUpmPulse = SetDelay(300);
/*
* 延时300ms，期间不断检测电流，如果电流大于12A，关掉所有MOSFET，红灯闪10下
*/
while(!CheckDelay(MinUpmPulse))
{
FastADConvert();
if(Strom > 120)
{
STEUER_OFF; // Abschalten wegen Kurzschluss
RotBlink(10);
return(0); // 启动失败
}
}
PWM = pwm; // pwm = 10
while(1) // main starup program
{
for(i=0;i<timer; i++) // timer = 300
{
if(!UebertragungAbgeschlossen) SendUart(); //UebertragungAbgeschlossen用来表示串口当前是否正忙，0不忙，1忙
else DatenUebertragung();
Wait(100); // wait 100us
}
DebugAusgaben(); //将一些当前信息写到调试结构中去
FastADConvert();
if(Strom > 60)
{
STEUER_OFF; // Abschalten wegen Kurzschluss
RotBlink(10);
return(0);
}
timer-= timer/15+1; // actually, 14/15 of timer
if(timer < 25) { if(TEST_MANUELL) timer = 25; else return(1); } // timer is going to converge, utiil it is less than 25, it stops.
Manuell();
Phase++;
Phase %= 6;
AdConvert(); //检测shunt上电流
PWM = pwm;
SetPWM();
if(SENSE) //若模拟比较器中断位置标志，则此表达式为非零
{
PORTD ^= GRUEN;
}
}
}
//------------------------------------------------------------
// check for new GPS data
void GPS_Update(void)
{
static uint16_t GPS_Timeout = 0;
static uint16_t beep_rythm = 0;
switch(GPSData.Status)
{
case INVALID:
Error |= ERROR_GPS_RX_TIMEOUT;
GPS_ClearPosition(&(FollowMe.Position)); // clear followme position
break;
case PROCESSED:
// wait for timeout
if(CheckDelay(GPS_Timeout))
{
GPSData.Status = INVALID;
}
break;
case NEWDATA:
GPS_Timeout = SetDelay(GPS_TIMEOUT); // reset gps timeout
Error &= ~ERROR_GPS_RX_TIMEOUT; // clear possible error
beep_rythm++;
// update data in the follow me message
if((GPSData.SatFix & SATFIX_3D) && (GPSData.NumOfSats >= GPS_MINSATS))
{
GPS_CopyPosition(&(GPSData.Position),&(FollowMe.Position));
}
else
{
GPS_ClearPosition(&(FollowMe.Position)); // clear followme position
}
// NC like sound on bad gps signals
if(SysState == STATE_SEND_FOLLOWME)
{
if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
else if ((GPSData.NumOfSats < GPS_MINSATS) && !(beep_rythm % 5)) BeepTime = 10;
}
GPSData.Status = PROCESSED; // set to processed unlocks the buffer for new data
break;
default:
GPSData.Status = INVALID;
break;
}
}
// -----------------------------------------------------------
// the menu functions
// -----------------------------------------------------------
void Menu_Status(uint8_t key)
{ //0123456789ABCDEF
#if !defined (RECEIVER_SPEKTRUM_DX7EXP) && !defined (RECEIVER_SPEKTRUM_DX8EXP)
JetiBox_printfxy(0,0,"%2i.%1iV",UBat/10, UBat%10);
if(NaviDataOkay)
{
JetiBox_printfxy(6,0,"%3d%c %03dm%c",ErsatzKompassInGrad, 0xDF, GPSInfo.HomeDistance/10,NC_GPS_ModeCharacter);
}
else
{
JetiBox_printfxy(6,0,"Status");
}
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(NC_ErrorCode)
{
static unsigned int timer;
static char toggle = 1;
if(CheckDelay(timer)) { if(toggle) toggle = 0; else toggle = 1; timer = SetDelay(1500);};
if(toggle)
{
LIBFC_JetiBox_SetPos(0);
_printf_P(&LIBFC_JetiBox_Putchar, NC_ERROR_TEXT[NC_ErrorCode] , 0);
}
else
{
JetiBox_printfxy(6,0,"ERROR: %2d ",NC_ErrorCode);
// if(MotorenEin) JetiBeep = 'O';
}
}
else
if(ShowSettingNameTime)
{
LIBFC_JetiBox_Clear();
JetiBox_printfxy(0,1,"Set%d:%s ",ActiveParamSet,EE_Parameter.Name);
return; // nichts weiter ausgeben
}
#else
if(NC_ErrorCode) { JetiBox_printfxy(6,0,"ERROR: %2d ",NC_ErrorCode); if(MotorenEin) JetiBeep = 'S';};
#endif
JetiBox_printfxy(0,1,"%4i %2i:%02i",Capacity.UsedCapacity,FlugSekunden/60,FlugSekunden%60);
if(Parameter_GlobalConfig & CFG_HOEHENREGELUNG)
{
JetiBox_printfxy(10,1,"%4im%c", (int16_t)(HoehenWert/100),VarioCharacter);
}
#endif
}
unsigned char GetKeyboard(void)
{
static char taste1 = 0, taste2 = 0,taste3 = 0,taste4 = 0,taste5 = 0;
unsigned char ret = 0;
if(CheckDelay(KeyTimer))
{
if(_TASTE1) { if(taste1++ == 0 || taste1 == CNT_TASTE) ret |= KEY1; if(taste1 == CNT_TASTE) taste1 = CNT_TASTE-CNT_TASTE/3;} else taste1 = 0;
if(_TASTE2) { if(taste2++ == 0 || taste2 == CNT_TASTE) ret |= KEY2; if(taste2 == CNT_TASTE) taste2 = CNT_TASTE-CNT_TASTE/3;} else taste2 = 0;
if(_TASTE3) { if(taste3++ == 0 || taste3 == CNT_TASTE) ret |= KEY3; if(taste3 == CNT_TASTE) taste3 = CNT_TASTE-CNT_TASTE/3;} else taste3 = 0;
if(_TASTE4) { if(taste4++ == 0 || taste4 == CNT_TASTE) ret |= KEY4; if(taste4 == CNT_TASTE) taste4 = CNT_TASTE-CNT_TASTE/3;} else taste4 = 0;
if(_TASTE5) { if(taste5++ == 0 || taste5 == CNT_TASTE) ret |= KEY5; if(taste5 == CNT_TASTE) taste5 = CNT_TASTE-CNT_TASTE/3;} else taste5 = 0;
KeyTimer = SetDelay(KEY_DELAY_MS);
}
return(ret);
}
void SucheGyroOffset(void)
{
unsigned char i, ready = 0;
int timeout;
timeout = SetDelay(2000);
for(i=140; i != 0; i--)
{
if(ready == 3 && i > 10) i = 9;
ready = 0;
if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
I2C_Start(TWI_STATE_GYRO_OFFSET_TX);
if(AnalogOffsetNick < 10) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; AnalogOffsetNick = 245;};
if(AnalogOffsetRoll < 10) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; AnalogOffsetRoll = 245;};
if(AnalogOffsetGier < 10) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; AnalogOffsetGier = 245;};
while(twi_state) if(CheckDelay(timeout)) {printf("\n\r DAC or I2C ERROR! Check I2C, 3Vref, DAC and BL-Ctrl"); break;}
AdReady = 0;
ANALOG_ON;
while(!AdReady);
if(i<10) Delay_ms_Mess(10);
}
Delay_ms_Mess(70);
}
//############################################################################
//主程序
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);
}
//############################################################################
//Hauptprogramm
int main (void)
//############################################################################
{
char altPhase = 0;
int test = 0;
unsigned int Blink,TestschubTimer;
unsigned int Blink2,MittelstromTimer,DrehzahlMessTimer,MotorGestopptTimer;
DDRC = 0x08; //PC3输出，控制红色led 端口C 数据方向寄存器
PORTC = 0x08; //PC3输出高电平，红色led亮
DDRD = 0x3A; //10111010A-B-C-配置为输出，TXD,LED_GRN(PD7)配置为输出，MITTEL/RXD/INTO为输入
PORTD = 0x00;
DDRB = 0x0E;
PORTB = 0x31;
#if (MOTORADRESSE == 0)
PORTB |= (ADR1 + ADR2); // Pullups für Adresswahl
for(test=0;test<500;test++);
if(PINB & ADR1)
{
if (PINB & ADR2) MotorAdresse = 1;
else MotorAdresse = 2;
}
else
{
if (PINB & ADR2) MotorAdresse = 3;
else MotorAdresse = 4;
}
HwVersion = 11;
#else
MotorAdresse = MOTORADRESSE;
HwVersion = 10;
#endif
if(PIND & 0x80) {HwVersion = 12; IntRef = 0xc0;} //为1.2版本
DDRD = 0xBA;
UART_Init();
Timer0_Init();
sei();//Globale Interrupts Einschalten
// Am Blinken erkennt man die richtige Motoradresse
/*
for(test=0;test<5;test++)
{
if(test == MotorAdresse) PORTD |= GRUEN;
Delay_ms(150);
PORTD &= ~GRUEN;
Delay_ms(250);
}
Delay_ms(500);
*/
// UART_Init(); // war doppelt
PWM_Init();
InitIC2_Slave(0x50);
InitPPM();
Blink = SetDelay(101);
Blink2 = SetDelay(102);
MinUpmPulse = SetDelay(103);
MittelstromTimer = SetDelay(254);
DrehzahlMessTimer = SetDelay(1005);
TestschubTimer = SetDelay(1006);
while(!CheckDelay(MinUpmPulse))
{
if(SollwertErmittlung()) break; // 一旦发现有信号，立刻停止延时
}
GRN_ON;
PWM = 0;
SetPWM();
SFIOR = 0x08; // Analog Comperator ein
ADMUX = 1;
MinUpmPulse = SetDelay(10);
DebugOut.Analog[1] = 1; // 填写一些调试信号
PPM_Signal = 0;
if(!SollwertErmittlung()) MotorTon();
//MotorTon();
PORTB = 0x31; // Pullups wieder einschalten
// zum Test der Hardware; Motor dreht mit konstanter Drehzahl ohne Regelung
if(TEST_MANUELL) Anwerfen(TEST_MANUELL); // kommt von dort nicht wieder
while (1)
{
//ShowSense();
if(!TEST_SCHUB) PWM = SollwertErmittlung();
//I2C_TXBuffer = PWM; // Antwort über I2C-Bus
if(MANUELL_PWM) PWM = MANUELL_PWM;
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Phase != altPhase) // es gab eine Kommutierung im Interrupt
{
MotorGestoppt = 0;
ZeitFuerBerechnungen = 0; // direkt nach einer Kommutierung ist Zeit
MinUpmPulse = SetDelay(250); // Timeout, falls ein Motor stehen bleibt
altPhase = Phase; // 旧相位 = 新相位
}
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!PWM) // Sollwert == 0 在开机后主控板静默,并没有发任何I2C信号过来
{
MotorAnwerfen = 0; // kein Startversuch MotorAnwerfen means motor start
ZeitFuerBerechnungen = 0; // 指使一些操作的优先级用
// nach 1,5 Sekunden den Motor als gestoppt betrachten
if(CheckDelay(MotorGestopptTimer))
{
DISABLE_SENSE_INT;
MotorGestoppt = 1;
STEUER_OFF;
}
}
else
{
if(MotorGestoppt) MotorAnwerfen = 1; // Startversuch
MotorGestopptTimer = SetDelay(1500);
}
if(MotorGestoppt && !TEST_SCHUB) PWM = 0; //TEST_SCHUB 1为测试模式，0为正常模式
SetPWM();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!ZeitFuerBerechnungen++) //先!,后++
{
if(MotorGestoppt) // 马达停止
{
GRN_ON;
FastADConvert();
}
if(SIO_DEBUG)
{
DebugAusgaben(); // welche Werte sollen angezeigt werden?
if(!UebertragungAbgeschlossen) SendUart();
else DatenUebertragung();
}
// Berechnen des Mittleren Stroms zur (langsamen) Strombegrenzung
if(CheckDelay(MittelstromTimer)) //254ms
{
MittelstromTimer = SetDelay(50); // alle 50ms
if(Mittelstrom < Strom) Mittelstrom++;// Mittelwert des Stroms bilden 将mittelstorm平缓化
else if(Mittelstrom > Strom) Mittelstrom--;
if(Strom > MAX_STROM) MaxPWM -= MaxPWM / 32; //为什么电流大于最大电流的时候，PWM最大值要减小？
if((Mittelstrom > LIMIT_STROM))// Strom am Limit? 动态调整MaxPWM的值
{
if(MaxPWM) MaxPWM--;// dann die Maximale PWM herunterfahren
PORTC |= ROT;
}
else
{
if(MaxPWM < MAX_PWM) MaxPWM++;
}
}
if(CheckDelay(DrehzahlMessTimer)) // Ist-Drehzahl bestimmen 1005ms
{
DrehzahlMessTimer = SetDelay(10);
SIO_Drehzahl = CntKommutierungen;//(6 * CntKommutierungen) / (POLANZAHL / 2);
CntKommutierungen = 0;
// if(PPM_Timeout == 0) // keine PPM-Signale
ZeitZumAdWandeln = 1;
}
#if TEST_SCHUB == 1
{
if(CheckDelay(TestschubTimer))
{
TestschubTimer = SetDelay(1500);
switch(test)
{
case 0: PWM = 50; test++; break;
case 1: PWM = 130; test++; break;
case 2: PWM = 60; test++; break;
case 3: PWM = 140; test++; break;
case 4: PWM = 150; test = 0; break;
default: test = 0;
}
}
}
#endif
// Motor Stehen geblieben
if((CheckDelay(MinUpmPulse) && SIO_Drehzahl == 0) || MotorAnwerfen) // SIO_Drehzahl 为当前转速值 前面为判断意外停机
{
/* 下面三行代码给电机意外停转用*/
MotorGestoppt = 1;
DISABLE_SENSE_INT; // 取消比较中断
MinUpmPulse = SetDelay(100);
if(MotorAnwerfen)
{
PORTC &= ~ROT; //switch off red led
Strom_max = 0;
MotorAnwerfen = 0;
if(Anwerfen(10)) // 以PMW = 10触发启动程序，期间要换相32次,成功返回1
{
GRN_ON; //绿灯打开
MotorGestoppt = 0; //
Phase--; // Phase = 1
PWM = 1;
SetPWM();
SENSE_TOGGLE_INT; // ?????
ENABLE_SENSE_INT; // ????? 使能比较器中断
MinUpmPulse = SetDelay(20);
while(!CheckDelay(MinUpmPulse)); // kurz Synchronisieren
PWM = 15;
SetPWM();
MinUpmPulse = SetDelay(300);
while(!CheckDelay(MinUpmPulse)) // kurz Durchstarten
{
if(Strom > LIMIT_STROM/2)
{
STEUER_OFF; // Abschalten wegen Kurzschluss
RotBlink(10);
MotorAnwerfen = 1;
}
}
// Drehzahlmessung wieder aufsetzen
DrehzahlMessTimer = SetDelay(50);
altPhase = 7;
}
else if(SollwertErmittlung()) MotorAnwerfen = 1;
}
}
} // ZeitFuerBerechnungen
} // while(1) - Hauptschleife
}
void USB_TransmitTxData(void)
{
USB_Transmit(); // output pending bytes in tx buffer
if((USB_tx_buffer.Locked == TRUE)) return;
if(CheckDelay(USB_AboTimeOut))
{
USB_DebugData_Interval = 0;
USB_NaviData_Interval = 0;
USB_Data3D_Interval = 0;
USB_Display_Interval = 0;
}
if((USB_Request_DebugLabel != 0xFF) && (USB_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'A', NC_ADDRESS, 2, &USB_Request_DebugLabel, sizeof(USB_Request_DebugLabel), (u8 *) ANALOG_LABEL[USB_Request_DebugLabel], 16);
USB_Request_DebugLabel = 0xFF;
}
else if(USB_ConfirmFrame && (USB_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'B', NC_ADDRESS, 1, &USB_ConfirmFrame, sizeof(USB_ConfirmFrame));
USB_ConfirmFrame = 0;
}
else if( (( (USB_DebugData_Interval > 0) && CheckDelay(USB_DebugData_Timer)) || USB_Request_DebugData) && (USB_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'D', NC_ADDRESS, 1,(u8 *)&DebugOut, sizeof(DebugOut));
USB_DebugData_Timer = SetDelay(USB_DebugData_Interval);
USB_Request_DebugData = FALSE;
}
else if((( (USB_Data3D_Interval > 0) && CheckDelay(USB_Data3D_Timer) ) || USB_Request_Data3D) && (USB_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'C', NC_ADDRESS, 1,(u8 *)&Data3D, sizeof(Data3D));
USB_Data3D_Timer = SetDelay(USB_Data3D_Interval);
USB_Request_Data3D = FALSE;
}
else if(USB_Request_ExternalControl && (USB_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'G', NC_ADDRESS, 1, (u8 *)&ExternControl, sizeof(ExternControl));
USB_Request_ExternalControl = FALSE;
}
else if( (( (USB_Display_Interval > 0) && CheckDelay(USB_Display_Timer)) || USB_Request_Display) && (USB_tx_buffer.Locked == FALSE))
{
Menu_Update(USB_DisplayKeys);
USB_DisplayKeys = 0;
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'H', NC_ADDRESS, 1, (u8*)DisplayBuff, sizeof(DisplayBuff));
USB_Request_Display = FALSE;
}
else if(USB_Request_Display1 && (USB_tx_buffer.Locked == FALSE))
{
Menu_Update(0);
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'L', NC_ADDRESS, 3, (u8*)&MenuItem, sizeof(MenuItem), (u8*)&MaxMenuItem, sizeof(MaxMenuItem),(u8*)DisplayBuff, sizeof(DisplayBuff));
USB_Request_Display1 = FALSE;
}
else if(USB_Request_VersionInfo && (USB_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
USB_Request_VersionInfo = FALSE;
}
else if(( (USB_NaviData_Interval && CheckDelay(USB_NaviData_Timer) ) || USB_Request_NaviData) && (USB_tx_buffer.Locked == FALSE))
{
NaviData.Errorcode = ErrorCode;
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
USB_NaviData_Timer = SetDelay(USB_NaviData_Interval);
USB_Request_NaviData = FALSE;
}
else if(USB_Request_ErrorMessage && (USB_tx_buffer.Locked == FALSE))
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
USB_Request_ErrorMessage = FALSE;
}
USB_Transmit(); // output pending bytes in tx buffer
}
// -----------------------------------------------------------------------
void Delay_ms(uint16_t w)
{
unsigned int t_stop;
t_stop = SetDelay(w);
while (!CheckDelay(t_stop));
}
//----------------------------------------------------------------------------------------------------
// logs gps and state info to a gpx file
logfilestate_t Logging_GPX(uint16_t LogDelay)
{
static logfilestate_t logfilestate = LOGFILE_IDLE; // the current logfilestate
static int8_t* logfilename = NULL; // the pointer to the logfilename
static uint16_t logtimer = 0, flushtimer = 0; // the log update timer
static GPX_Document_t logfile; // the logfilehandle
// initialize if LogDelay os zero
if(!LogDelay)
{
switch(logfilestate)
{
case LOGFILE_OPENED:
GPX_DocumentClose(&logfile); // try to close it
break;
default:
break;
}
logfilestate = LOGFILE_IDLE;
logfilename = NULL;
GPX_DocumentInit(&logfile);
logtimer = SetDelay(0); // set logtimer to now
return logfilestate;
}
// no init
if(CheckDelay(logtimer))
{
logtimer = SetDelay(LogDelay); // standard interval
if(SysState == STATE_SEND_FOLLOWME)
{
switch(logfilestate)
{
case LOGFILE_IDLE:
case LOGFILE_CLOSED:
//if((GPSData.Status != INVALID) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.SatFix == SATFIX_3D) )
{
logfilestate = LOGFILE_START;
}
break;
case LOGFILE_START:
// find unused logfile name
do
{ // try to generate a new logfile name
logfilename = GenerateGPXLogFileName();
}while((logfilename != NULL) && fexist_(logfilename));
// if logfilename exist
if(logfilename != NULL)
{
// try to create the log file
if(GPX_DocumentOpen(logfilename, &logfile))
{
flushtimer = SetDelay(LOG_FLUSH_INTERVAL);
logfilestate = LOGFILE_OPENED; // goto next step
printf("\r\nOpening gpx-file: %s\r\n", logfilename);
}
else // could not be openend
{
logfilestate = LOGFILE_ERROR;
printf("\r\nError opening gpx-file: %s\r\n", logfilename);
logtimer = SetDelay(10); // try again in open logfile in 10 mili sec
}
}
else
{
logfilestate = LOGFILE_ERROR;
printf("\r\nError getting free gpx-file name\r\n");
}
// else retry in next loop
break;
case LOGFILE_OPENED:
// append new gps log data
if((GPSData.Status != INVALID) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.SatFix == SATFIX_3D))
{
if(!GPX_LoggGPSCoordinates(&logfile))
{ // error logging data
printf("\r\nError logging to gpx-file\r\n");
GPX_DocumentClose(&logfile);
logfilestate = LOGFILE_ERROR;
}
else // successful log
{
if(CheckDelay(flushtimer))
{
flushtimer = SetDelay(LOG_FLUSH_INTERVAL);
fflush_(logfile.file);
}
}
}
break;
case LOGFILE_ERROR:
break;
default:
logfilestate = LOGFILE_IDLE;
break;
}
} // EOF follow me on
else // follow me off
{ // close log file if opened
if(logfilestate == LOGFILE_OPENED)
{
if(GPX_DocumentClose(&logfile))
{
printf("\r\nClosing gpx-file\r\n");
logfilestate = LOGFILE_CLOSED;
}
else // could not be closed
{
printf("\r\nError closing gpx-file\r\n");
logfilestate = LOGFILE_ERROR;
}
}
} //EOF follow me off
} // EOF Check LogTimer
return logfilestate;
}