| 463,7 → 463,7 |
|---|
| else if(CanbusTimeOut == 1) |
| { |
| sprintf(ErrorMSG,"ERR: Canbus"); |
| CanbusInit(); |
| CanbusInit(); |
| newErrorCode = 39; |
| } |
| else |
| 524,7 → 524,7 |
|---|
| |
| u8 Polling(void) |
| { |
| static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process; |
| static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process, t20ms; |
| static u32 old_ms = 0; |
| |
| if(running) {/*DebugOut.Analog[]++;*/ return(1);}; |
| 532,6 → 532,8 |
|---|
| |
| if(CountMilliseconds != old_ms) // 1 ms |
| { |
| if(!t20ms--) { CalcNickServoValue(); t20ms = 20; }; |
| |
| if(UART1_BaudrateFallbackTimeout) |
| { |
| if(--UART1_BaudrateFallbackTimeout == 0) UART1_Configure(Uart1Baudrate); |
| 538,8 → 540,22 |
|---|
| } |
| |
| if(CanbusTimeOut >= 2) CanbusTimeOut--; |
| if(LaserCtrlTimeout) LaserCtrlTimeout--; else FromLaserCtrl.Distance = 0; |
| if(LaserCtrlTimeout) LaserCtrlTimeout--; |
| else |
| { |
| FromLaserCtrl.Distance = 0; |
| FromLaserCtrl.LaserStatus = 0; |
| }; |
| |
| if(GimbalCtrlTimeout) GimbalCtrlTimeout--; |
| else |
| { |
| FromGimbalCtrl.Nick = -1; |
| FromGimbalCtrl.Roll = -1; |
| FromGimbalCtrl.Yaw = -1; |
| FromGimbalCtrl.GimbalStatus = 0; |
| } |
| |
| if(CamCtrlTimeout) |
| { |
| if(--CamCtrlTimeout == 1) CamCtrlCharacter = '?'; |
| 556,6 → 572,7 |
|---|
| CalcHeadFree(); |
| if(CamCtrlTimeout > 1) CamCtrl_GetData(3); |
| if(LaserCtrlTimeout > 1) LaserCtrl_GetData(3); |
| if(GimbalCtrlTimeout > 1) GimbalCtrl_GetData(3); |
| if(UART_VersionInfo.HWMajor >= 30) ProcessCanBus(); |
| if(!CheckDelay(SPI0_Timeout)) TimeoutGPS_Process = 0; |
| else if(CountMilliseconds - SPI0_Timeout > 30000000L) SPI0_Timeout = CountMilliseconds; // avoid too long overflows |
| 612,6 → 629,7 |
|---|
| CountNewGpsDataIn5Sec = 25; |
| CountGpsProcessedIn5Sec = 0; |
| TimerCheckError = SetDelay(1000); |
| NickServoValue = 128 * 64; // Middle position |
| } |
| |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| 657,7 → 675,7 |
|---|
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Re-Init GPS-Module? |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV) |
| if((Parameter.GlobalConfig & FC_CFG_GPS_AKTIV) && (UART0_Muxer != UART0_MK3MAG)) |
| { |
| if(TryAgain_UBX_Setup) |
| { |
| 693,7 → 711,7 |
|---|
| CountGpsProcessedIn5Sec = 0; |
| CountNewGpsDataIn5Sec = FreqNewGpsData / 2; |
| ConfirmGpsUpdateRate(FreqNewGpsData); |
| } |
| } |
| } |
| oldFcFlags = FC.StatusFlags; |
| // if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected |
| 850,8 → 868,16 |
|---|
| if(I2CBus_LockBuffer(I2C0,3)) I2CBus_Transmission(I2C0, LASER_SLAVE_ADDRESS, &ToLaserCtrl, 4, &LaserCtrl_UpdateData, sizeof(FromLaserCtrl)); |
| } |
| else LaserCtrlTimeout = 0; // disable CamCtrl communication if external compass is connected |
| // +++++++++++++++++++++++++++++++++++++++ |
| |
| // +++++++++++++++++++++++++++++++++++++++ |
| // ++ check GimbalCtrl version (if connected) |
| if(Compass_I2CPort == NCMAG_PORT_INTERN) |
| { |
| if(I2CBus_LockBuffer(I2C0,3)) I2CBus_Transmission(I2C0, GIMBAL_SLAVE_ADDRESS, &ToGimbalCtrl, 4, &GimbalCtrl_UpdateData, sizeof(FromGimbalCtrl)); |
| } |
| else GimbalCtrlTimeout = 0; // disable CamCtrl communication if external compass is connected |
| // +++++++++++++++++++++++++++++++++++++++ |
| GPS_Init(); |
| |
| // ---------- Prepare the isr driven |
| 896,6 → 922,16 |
|---|
| } |
| else if(Compass_I2CPort == NCMAG_PORT_INTERN) UART1_PutString(" No LaserCtrl connected \r\n"); |
| |
| if(FromGimbalCtrl.GimbalStatus) |
| { |
| u8 msg[30]; |
| sprintf(msg, " GimbalCtrl found V%i.%02i ",1 + FromGimbalCtrl.Version / 100, FromGimbalCtrl.Version % 100); |
| UART1_PutString(msg); |
| if(!(FromGimbalCtrl.GimbalStatus & GIMBAL_DATA_OK)) UART1_PutString("... but no Gimbal data!\r\n"); |
| else UART1_PutString("okay\r\n"); |
| } |
| else if(Compass_I2CPort == NCMAG_PORT_INTERN) UART1_PutString(" No GimbalCtrl connected \r\n"); |
| |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| for (;;) // the endless main loop |
| { |
| 928,6 → 964,7 |
|---|
| SpeakNextWaypoint = 1; // Speak once as soon as the Points are active |
| NCFlags &= ~NC_FLAG_TARGET_REACHED; |
| BeepTime = 150; |
| SpeakHoTT = SPEAK_LOAD; |
| } |
| } |
| FromFC_LoadWP_List = 0; |
| 936,7 → 973,11 |
|---|
| if(FromFC_Save_SinglePoint) |
| { |
| WPL_Store.Index = FromFC_Save_SinglePoint; |
| if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150; |
| if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) |
| { |
| BeepTime = 150; |
| SpeakHoTT = SPEAK_SAVE; |
| } |
| FromFC_Save_SinglePoint = 0; |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 953,6 → 994,7 |
|---|
| SpeakNextWaypoint = 1; // Speak once as soon as the Points are active |
| NCFlags &= ~NC_FLAG_TARGET_REACHED; |
| BeepTime = 150; |
| SpeakHoTT = SPEAK_LOAD; |
| } |
| } |
| FromFC_Load_SinglePoint = 0; |