/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!! */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software Nutzungsbedingungen (english version: see below)
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
// + des Mitverschuldens offen.
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
// + Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software LICENSING TERMS
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
// + The Software may only be used with the Licensor's products.
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
// + agreement shall be the property of the Licensor.
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
// + features that can be used to identify the program may not be altered or defaced by the customer.
// + The customer shall be responsible for taking reasonable precautions
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
// + #### END OF LICENSING TERMS ####
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//#define MCLK96MHZ
const unsigned long _Main_Crystal
= 25000;
//#include <stdio.h>
#include <string.h>
#include "91x_lib.h"
#include "led.h"
#include "uart0.h"
#include "uart1.h"
#include "uart2.h"
#include "gps.h"
#include "i2c.h"
#include "compass.h"
#include "ncmag.h"
#include "timer1.h"
#include "timer2.h"
#include "analog.h"
#include "spi_slave.h"
#include "fat16.h"
#include "sdc.h"
#include "logging.h"
#include "params.h"
#include "settings.h"
#include "config.h"
#include "main.h"
#include "debug.h"
#include "eeprom.h"
#include "ssc.h"
#include "sdc.h"
#include "uart1.h"
#include "canbus.h"
#include "triggerlog.h"
#include "CamCtrl.h"
#ifdef FOLLOW_ME
u8 TransmitAlsoToFC
= 0;
#endif
u32 TimerCheckError
;
u8 ErrorCode
= 0;
u16 BeepTime
;
u8 NCFlags
= 0;
s16 GeoMagDec
= 0; // local magnetic declination in 0.1 deg
u8 ErrorGpsFixLost
= 0;
u8 FromFC_LoadWP_List
= 0, FromFC_Load_SinglePoint
= 0, FromFC_Save_SinglePoint
= 0;
u8 ToFC_MaxWpListIndex
= 4;
u8 ClearFCStatusFlags
= 0;
u8 StopNavigation
= 0;
volatile u32 PollingTimeout
= 10000;
Param_t Parameter
;
Partner_t Partner
;
volatile FC_t FC
;
volatile u32 SPIWatchDog
= 15000; // stop Navigation if this goes to zero
volatile u32 SD_WatchDog
= 15000; // stop Logging if this goes to zero
u32 CountGpsProcessedIn5Sec
= 50,CountNewGpsDataIn5Sec
= 25, FreqGpsNavProcessed
= 0, FreqNewGpsData
= 0;
u8 NewWPL_Name
= 0;
u32 MaxWP_Radius_in_m
= 0;
s8 ErrorMSG
[25];
s8 PartnerErrorMSG
[25] = " --- \0";
u32 TimeSinceMotorStart
= 0;
u8 IamMaster
= NOTHING
; // for Master/Slave Redundance
//----------------------------------------------------------------------------------------------------
void SCU_Config
(void)
{
/* configure PLL and set it as master clock source */
SCU_MCLKSourceConfig
(SCU_MCLK_OSC
); // set master clock source to external oscillator clock (25MHz) before diabling the PLL
SCU_PLLCmd
(DISABLE
); // now disable the PLL
#ifdef MCLK96MHZ
SCU_BRCLKDivisorConfig
(SCU_BRCLK_Div2
); // set BRCLK to MCLK/2 = 48MHz
SCU_PCLKDivisorConfig
(SCU_PCLK_Div4
); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock)
SCU_RCLKDivisorConfig
(SCU_RCLK_Div2
); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
SCU_HCLKDivisorConfig
(SCU_HCLK_Div2
); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
SCU_PLLFactorsConfig
(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
#else
SCU_BRCLKDivisorConfig
(SCU_BRCLK_Div1
); // set BRCLK to MCLK = 48MHz
SCU_PCLKDivisorConfig
(SCU_PCLK_Div2
); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)
SCU_RCLKDivisorConfig
(SCU_RCLK_Div1
); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
SCU_HCLKDivisorConfig
(SCU_HCLK_Div1
); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
SCU_PLLFactorsConfig
(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
#endif
SCU_PLLCmd
(ENABLE
); // Enable PLL (is disabled by SCU_PLLFactorsConfig)
SCU_MCLKSourceConfig
(SCU_MCLK_PLL
); // set master clock source to PLL
}
//----------------------------------------------------------------------------------------------------
void GetNaviCtrlVersion
(void)
{
u8 msg
[25];
sprintf(msg
,"\n\r NaviCtrl HW: V%d.%d SW: V%d.%02d%c", UART_VersionInfo.
HWMajor/10, UART_VersionInfo.
HWMajor%10, VERSION_MAJOR
, VERSION_MINOR
, 'a'+ VERSION_PATCH
);
UART1_PutString
(msg
);
}
//----------------------------------------------------------------------------------------------------
void CheckErrors
(void)
{
static s32 no_error_delay
= 0;
s32 newErrorCode
= 0;
UART_VersionInfo.
HardwareError[0] = 0;
if(CheckDelay
(I2CBus
(Compass_I2CPort
)->Timeout
) || (Compass_Heading
< 0)) DebugOut.
StatusRed |= AMPEL_COMPASS
;
else DebugOut.
StatusRed &= ~AMPEL_COMPASS
; // MK3Mag green status
if((FC.
Error[1] & FC_ERROR1_I2C
) || (FC.
Error[1] & FC_ERROR1_BL_MISSING
)) DebugOut.
StatusRed |= AMPEL_BL
;
else DebugOut.
StatusRed &= ~AMPEL_BL
; // BL-Ctrl green status
if(UART_VersionInfo.
HardwareError[0] || UART_VersionInfo.
HardwareError[1]) DebugOut.
StatusRed |= AMPEL_NC
;
else DebugOut.
StatusRed &= ~AMPEL_NC
;
if(CheckDelay
(I2CBus
(Compass_I2CPort
)->Timeout
))
{
LED_RED_ON
;
sprintf(ErrorMSG
,"no compass communica");
//Reset Compass communication
if(Compass_I2CPort
== NCMAG_PORT_EXTERN
) I2CBus_Init
(I2C0
); else I2CBus_Init
(I2C1
);
Compass_Init
();
newErrorCode
= 4;
StopNavigation
= 1;
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_COMPASS_RX
;
DebugOut.
StatusRed |= AMPEL_COMPASS
;
}
else if(CompassValueErrorCount
> 30)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"compass sensor error");
newErrorCode
= 34;
StopNavigation
= 1;
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_COMPASS_VALUE
;
if(Compass_I2CPort
== NCMAG_PORT_EXTERN
) I2CBus_Init
(I2C0
); else I2CBus_Init
(I2C1
);
Compass_Init
();
}
else if((FCCalibActive
|| CompassCalState
) && FC_Version.
Hardware)
{
sprintf(ErrorMSG
,"Calibrate... ");
newErrorCode
= 0;
ErrorCode
= 0;
no_error_delay
= 1;
}
else if(CheckDelay
(SPI0_Timeout
))
{
LED_RED_ON
;
sprintf(ErrorMSG
,"no FC communication ");
newErrorCode
= 3;
StopNavigation
= 1;
DebugOut.
StatusGreen &= ~AMPEL_FC
; // status of FC Present
DebugOut.
StatusGreen &= ~AMPEL_BL
; // status of BL Present
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_SPI_RX
;
}
else if(FC_Version.
Compatible != FC_SPI_COMPATIBLE
)
{
LED_RED_ON
;
#ifndef FOLLOW_ME
sprintf(ErrorMSG
,"FC not compatible ");
#else
sprintf(ErrorMSG
,"! FollowMe only ! ");
#endif
newErrorCode
= 1;
StopNavigation
= 1;
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE
;
DebugOut.
StatusRed |= AMPEL_NC
;
}
else if(FC.
Error[0] & FC_ERROR0_GYRO_NICK
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR: FC Nick Gyro");
newErrorCode
= 10;
}
else if(FC.
Error[0] & FC_ERROR0_GYRO_ROLL
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR: FC Roll Gyro");
newErrorCode
= 11;
}
else if(FC.
Error[0] & FC_ERROR0_GYRO_YAW
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR: FC Yaw Gyro");
newErrorCode
= 12;
}
else if(FC.
Error[0] & FC_ERROR0_ACC_NICK
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR: FC Nick ACC");
newErrorCode
= 13;
}
else if(FC.
Error[0] & FC_ERROR0_ACC_ROLL
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR: FC Roll ACC");
newErrorCode
= 14;
}
else if(FC.
Error[0] & FC_ERROR0_ACC_TOP
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:FC Z-ACC");
newErrorCode
= 15;
}
else if(NC_To_FC_Flags
& NC_TO_FC_FLYING_RANGE
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:Flying range!");
newErrorCode
= 28;
}
else if(FC.
Error[0] & FC_ERROR0_PRESSURE
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:Pressure sensor");
newErrorCode
= 16;
}
else if(FC.
Error[1] & FC_ERROR1_I2C
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:I2C FC to BL");
newErrorCode
= 17;
}
else if(FC.
Error[1] & FC_ERROR1_BL_MISSING
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR: Bl Missing");
newErrorCode
= 18;
}
else if(FC.
Error[1] & FC_ERROR1_MIXER
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"Mixer Error");
newErrorCode
= 19;
}
else if(CheckDelay
(UBX_Timeout
) && Parameter.
GlobalConfig & FC_CFG_GPS_AKTIV
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"no GPS communication");
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_GPS_RX
;
UART_VersionInfo.
Flags &= ~NC_VERSION_FLAG_GPS_PRESENT
;
newErrorCode
= 5;
StopNavigation
= 1;
}
else if(Compass_Heading
< 0 && NCMAG_Present
&& !NCMAG_IsCalibrated
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"compass not calibr.");
newErrorCode
= 31;
StopNavigation
= 1;
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_COMPASS_VALUE
;
}
else if(Compass_Heading
< 0)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"bad compass value ");
newErrorCode
= 6;
StopNavigation
= 1;
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_COMPASS_VALUE
;
}
else if((FC.
Error[1] & FC_ERROR1_SPI_RX
))
{
LED_RED_ON
;
sprintf(ErrorMSG
,"FC spi rx error ");
newErrorCode
= 8;
StopNavigation
= 1;
}
else if(FC.
Error[0] & FC_ERROR0_CAREFREE
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"FC: Carefree Error");
newErrorCode
= 20;
}
else if(FC.
BAT_Voltage < 45)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:Power Supply");
newErrorCode
= 41;
}
else
if(FC.
Error[1] & FC_ERROR1_RC_VOLTAGE
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR: 5V RC-Supply");
newErrorCode
= 40;
}
else if(FC.
Error[1] & FC_ERROR1_PPM
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"RC Signal lost ");
newErrorCode
= 7;
}
else if(ErrorGpsFixLost
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"GPS Fix lost ");
newErrorCode
= 21;
}
else if(ErrorDisturbedEarthMagnetField
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"Magnet error ");
newErrorCode
= 22;
DebugOut.
StatusRed |= AMPEL_COMPASS
| AMPEL_NC
;
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_COMPASS_VALUE
;
}
else if(((ErrorCheck_BL_MinOfMaxPWM
== 40 && (TimeSinceMotorStart
> 3)) || (ErrorCheck_BL_MinOfMaxPWM
== 39)) && !ErrorCode
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:Motor restart ");
newErrorCode
= 23;
DebugOut.
StatusRed |= AMPEL_BL
;
}
else if(BL_MinOfMaxPWM
&& BL_MinOfMaxPWM
< 30 && !ErrorCode
)
{
u16 i
;
for(i
= 0; i
< 16; i
++) if(Motor
[i
].
MaxPWM == BL_MinOfMaxPWM
) break; // find the motor number
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:BL%2d Test:%2d ",i
+1,BL_MinOfMaxPWM
);
newErrorCode
= 32;
DebugOut.
StatusRed |= AMPEL_BL
;
}
else if(ErrorCheck_BL_MinOfMaxPWM
< 248 && (TimeSinceMotorStart
> 3) && !ErrorCode
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:BL Limitation ");
newErrorCode
= 24;
DebugOut.
StatusRed |= AMPEL_BL
;
}
else if((NCFlags
& NC_FLAG_RANGE_LIMIT
) && (FC.
StatusFlags & FC_STATUS_FLY
) && !ErrorCode
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:GPS WP range ");
newErrorCode
= 25;
DebugOut.
StatusRed |= AMPEL_NC
;
}
else if((!SD_SWITCH
|| (SDCardInfo.
Valid == 0)) && Parameter.
GlobalConfig3 & CFG3_NO_SDCARD_NO_START
&& !(FC.
StatusFlags & FC_STATUS_FLY
))
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:No SD-Card ");
newErrorCode
= 26;
DebugOut.
StatusRed |= AMPEL_NC
;
}
else if((SD_LoggingError
|| (SD_WatchDog
< 2000 && SD_WatchDog
!= 0)) && Parameter.
GlobalConfig3 & CFG3_NO_SDCARD_NO_START
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:SD Logging abort");
newErrorCode
= 27;
DebugOut.
StatusRed |= AMPEL_NC
;
SD_LoggingError
= 0;
}
else if(((AbsoluteFlyingAltitude
) && (NaviData.
Altimeter / 20 >= AbsoluteFlyingAltitude
)) && (FC.
StatusFlags & FC_STATUS_FLY
))
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ERR:Max Altitude ");
newErrorCode
= 29;
DebugOut.
StatusRed |= AMPEL_NC
;
}
else if(Parameter.
GlobalConfig3 & CFG3_NO_GPSFIX_NO_START
&& !(NCFlags
& NC_FLAG_GPS_OK
) && ((FC.
StatusFlags & (FC_STATUS_START
| FC_STATUS_MOTOR_RUN
)) || (FC.
StickGas < -50 && FC.
StickYaw < -50)))
{
LED_RED_ON
;
sprintf(ErrorMSG
,"No GPS Fix ");
newErrorCode
= 30;
}
else if((FreqNewGpsData
<= 35 || FreqNewGpsData
> 60) && TimeSinceMotorStart
> 15)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"GPS Update rate ");
newErrorCode
= 38;
}
else if(NC_GPS_ModeCharacter
== 'F')
{
sprintf(ErrorMSG
,"FAILSAFE pos.! ");
newErrorCode
= 35;
}
else if(FC.
StatusFlags3 & FC_STATUS3_REDUNDANCE_ERROR
)
{
sprintf(ErrorMSG
,"ERR:Redundancy ");
newErrorCode
= 36;
}
else if(FC.
StatusFlags3 & FC_STATUS3_REDUNDANCE_TEST
)
{
sprintf(ErrorMSG
,"Redundancy test ");
newErrorCode
= 37;
}
else if(CanbusTimeOut
== 1)
{
sprintf(ErrorMSG
,"ERR: Canbus");
CanbusInit
();
newErrorCode
= 39;
}
else
if(FC.
Error[1] & FC_ERROR1_ACC_NOT_CAL
)
{
LED_RED_ON
;
sprintf(ErrorMSG
,"ACC not calib.");
newErrorCode
= 42;
}
else // no error occured
{
StopNavigation
= 0;
LED_RED_OFF
;
if(no_error_delay
) { no_error_delay
--; }
else
{
sprintf(ErrorMSG
,"No Error ");
ErrorCode
= 0;
}
}
if(newErrorCode
)
{
if(FC.
StatusFlags & FC_STATUS_MOTOR_RUN
) no_error_delay
= 8; // delay the errors if the motors are running
ErrorCode
= newErrorCode
;
}
FC.
Error[0] = 0;
FC.
Error[1] = 0;
FC.
Error[2] = 0;
FC.
Error[3] = 0;
FC.
Error[4] = 0;
ErrorGpsFixLost
= 0;
ErrorCheck_BL_MinOfMaxPWM
= BL_MinOfMaxPWM
;
}
unsigned char RAM_Checksum
(unsigned char* pBuffer
, u16 len
)
{
u8 crc
= 0xAA;
u16 i
;
for(i
=0; i
<len
; i
++) crc
+= pBuffer
[i
];
return crc
;
}
u8 Polling
(void)
{
static u8 running
= 0, oldFcFlags
= 0, count5sec
, TimeoutGPS_Process
;
static u32 old_ms
= 0;
if(running
) {/*DebugOut.Analog[]++;*/ return(1);};
running
= 1;
if(CountMilliseconds
!= old_ms
) // 1 ms
{
if(CanbusTimeOut
>= 2) CanbusTimeOut
--;
if(CamCtrlTimeout
)
{
if(--CamCtrlTimeout
== 1) CamCtrlCharacter
= '?';
else if(CamCtrlTimeout
== 10000) CamCtrlTimeout
= 0; // this is used during NC startup-Time phase
}
else
{
if(TrigLogging.
CountExternal) CamCtrlCharacter
= TrigLogging.
CountExternal % 10 + '0';
FromCamCtrl.
CamStatus = 0;
};
old_ms
= CountMilliseconds
;
Compass_Update
(); // update compass communication
Analog_Update
(); // get new ADC values
CalcHeadFree
();
if(CamCtrlTimeout
> 1) CamCtrl_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
if(++TimeoutGPS_Process
>= 25)
{
GPS_Navigation
(&GPSData
, &(ToFlightCtrl.
GPSStick)); // process the GPS data even if the FC is not connected
TimeoutGPS_Process
= 0;
}
}
SPI0_UpdateBuffer
(); // also calls the GPS-functions
UART0_ProcessRxData
(); // GPS process request
UART0_TransmitTxData
(); // GPS send answer
UART1_ProcessRxData
(); // PC process request
UART1_TransmitTxData
(); // PC send answer
UART2_TransmitTxData
(); // FC send answer
if(!(FC.
StatusFlags & FC_STATUS_MOTOR_RUN
)) TimeSinceMotorStart
= 0;
if(HugeBlockFromFC.
WhoAmI == 1) // contains eeprom-Data
{
u8 crc1
, crc2
;
crc1
= HugeBlockFromFC.
Data[sizeof(EE_Parameter
)-1]; // contains the EE_Parameter.crc
crc2
= RAM_Checksum
((u8
*)(&HugeBlockFromFC.
Data), sizeof(EE_Parameter
)-1); // calculates the current crc
if(crc1
== crc2
)
{
memcpy(&EE_Parameter
, (u8
*) HugeBlockFromFC.
Data, sizeof(EE_Parameter
));
RequestConfigFromFC
= 0;
}
HugeBlockFromFC.
WhoAmI = 0;
}
if(FCCalibActive
)
{
count5sec
= 0;
FreqNewGpsData
= 50;
CountNewGpsDataIn5Sec
= 25;
CountGpsProcessedIn5Sec
= 0;
TimerCheckError
= SetDelay
(1000);
}
// ---------------- Error Check Timing ----------------------------
if(CheckDelay
(TimerCheckError
) || (FC.
StatusFlags & FC_STATUS_START
&& !(oldFcFlags
& FC_STATUS_START
))) // Timer or FY wants to start
{
if(CheckDelay
(TimerCheckError
))
{
if(!(FC.
StatusFlags & FC_STATUS_FLY
)) // do not change the Bit during flight
{
if(!BLITZ_CONNECTED
) BlitzSchuhConnected
= 1;
else BlitzSchuhConnected
= 0;
}
TimerCheckError
= SetDelay
(1000);
if(CompassValueErrorCount
) CompassValueErrorCount
--;
if(++count5sec
== 5)
{
FreqGpsNavProcessed
= CountGpsProcessedIn5Sec
* 2; //400 = 40Hz
FreqNewGpsData
= CountNewGpsDataIn5Sec
; // 50 = 5,0Hz
count5sec
= 0;
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
if(!CheckDelay
(SPI0_Timeout
) || (DebugUART
== UART1
)) CheckErrors
();
if(FC.
StatusFlags & FC_STATUS_FLY
)
{
NaviData.
FlyingTime++; // we want to count the battery-time
TimeSinceMotorStart
++;
}
if(EE_Parameter.
Revision == 0) RequestConfigFromFC
= 1;
if(SerialLinkOkay
) SerialLinkOkay
--;
if(SerialLinkOkay
< 250 - 6) NCFlags
|= NC_FLAG_NOSERIALLINK
; // 6 seconds timeout for serial communication
else NCFlags
&= ~NC_FLAG_NOSERIALLINK
;
if(StopNavigation
&& (Parameter.
NaviGpsModeControl >= 50) && (Parameter.
GlobalConfig & FC_CFG_GPS_AKTIV
)) BeepTime
= 1000;
}
running
= 0;
// if(!PollingTimeout) DebugOut.Analog[17]++;
return(0);
}
// the handler will be cyclic called by the timer 1 ISR
// used is for critical timing parts that normaly would handled
// within the main loop that could block longer at logging activities
void EXTIT3_IRQHandler
(void) // 1ms - Takt
{
IENABLE
;
VIC_ITCmd
(EXTIT3_ITLine
,DISABLE
); // disable irq
if(PollingTimeout
== 0)
{
PollingTimeout
= 5;
//if(Polling() == 0) DebugOut.Analog[]++;
Polling
();
}
VIC_SWITCmd
(EXTIT3_ITLine
,DISABLE
); // clear pending bit
VIC_ITCmd
(EXTIT3_ITLine
, ENABLE
); // enable irq
IDISABLE
;
VIC1
->VAR
= 0xFF; // write any value to VIC0 Vector address register
}
//----------------------------------------------------------------------------------------------------
int main
(void)
{
// static u32 ftimer =0;
// static u8 fstate = 0;
// static File_t* f = NULL;
/* Configure the system clocks */
SCU_Config
();
/* init VIC (Vectored Interrupt Controller) */
SCU_AHBPeriphClockConfig
(__VIC
,ENABLE
); // enable AHB bus clock for VIC
SCU_AHBPeriphReset
(__VIC
, DISABLE
); // disable reset state for VIC
VIC_DeInit
(); // deinitializes the VIC module registers to their default reset values.
VIC_InitDefaultVectors
();
// initialize timer 1 for System Clock and delay rountines
TIMER1_Init
();
// initialize the LEDs (needs Timer 1)
Led_Init
();
// initialize the debug UART1
UART1_Init
();
UART1_PutString
("\r\n---------------------------------------------");
// initialize timer 2 for servo outputs
//TIMER2_Init();
// initialize UART2 to FLIGHTCTRL
UART2_Init
();
// initialize UART0 (to MKGPS or MK3MAG)
UART0_Init
();
// initialize adc
Analog_Init
();
// initialize SPI0 to FC
SPI0_Init
();
// initialize i2c busses (needs Timer 1)
InitCamCtrl
();
I2CBus_Init
(I2C0
);
I2CBus_Init
(I2C1
);
// initialize fat16 partition on sd card (needs Timer 1)
Fat16_Init
();
// initialize NC params
NCParams_Init
();
// initialize the settings
Settings_Init
();
// initialize logging (needs settings)
Logging_Init
();
//UART_VersionInfo.HWMajor = 30;
LED_GRN_ON
;
TimerCheckError
= SetDelay
(3000);
UART1_PutString
("\r\n++++++++++++++++++++++++++++++++++++++++++");
UART1_PutString
("\n\r Version information:");
GetNaviCtrlVersion
();
DebugOut.
StatusGreen = AMPEL_NC
| AMPEL_COMPASS
; // NC and MK3Mag
DebugOut.
StatusRed = 0x00;
UART1_PutString
("\r\n++++++++++++++++++++++++++++++++++++++++++");
Compass_Init
();
#ifdef FOLLOW_ME
TransmitAlsoToFC
= 1;
UART1_PutString
("\r\n++++++++++++++++++++++++++++++++++++++++++");
UART1_PutString
("\n\r FOLLOW-ME Transmitter only!");
UART1_PutString
("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");
TransmitAlsoToFC
= 0;
#else
SPI0_GetFlightCtrlVersion
();
if(FC_Version.
Compatible != FC_SPI_COMPATIBLE
)
{
UART1_PutString
("\n\r Flight-Ctrl not compatible\n\r");
LED_RED_ON
;
}
#endif
if(IamMaster
== SLAVE
) UART1_PutString
(" SLAVE\r\n");
if(IamMaster
== MASTER
) UART1_PutString
(" MASTER\r\n");
// +++++++++++++++++++++++++++++++++++++++
// ++ check CamCtrl version (if connected)
if(Compass_I2CPort
== NCMAG_PORT_INTERN
)
{
if(I2CBus_LockBuffer
(I2C0
,3)) I2CBus_Transmission
(I2C0
, CAM_SLAVE_ADDRESS
, &ToCamCtrl
, 4, &CamCtrl_UpdateData
, sizeof(FromCamCtrl
));
}
else CamCtrlTimeout
= 0; // disable CamCtrl communication if external compass is connected
// +++++++++++++++++++++++++++++++++++++++
if(UBX_Setup
() == 0) UBX_Setup
(); // inits the GPS-Module via ubx -> try twice
GPS_Init
();
// ---------- Prepare the isr driven
// set to absolute lowest priority
VIC_Config
(EXTIT3_ITLine
, VIC_IRQ
, PRIORITY_SW
);
// enable interrupts
VIC_ITCmd
(EXTIT3_ITLine
, ENABLE
);
Debug_OK
("START");
UART1_PutString
("\r\n");
fifo_purge
(&UART1_rx_fifo
); // flush the whole fifo init buffer
LED_GRN_ON
;
LED_RED_OFF
;
Settings_GetParamValue
(PID_SEND_NMEA
,(u16
*) &NMEA_Interval
);
UART1_PutString
("\r\n");
CompassValueErrorCount
= 0;
I2CBus
(Compass_I2CPort
)->Timeout
= SetDelay
(3000);
// Intilizes the Canbus
if(UART_VersionInfo.
HWMajor >= 30)
{
EXT2_Init
(); // External Output EXT2
CanbusInit
();
}
if(FromCamCtrl.
CamStatus)
{
u8 msg
[30];
sprintf(msg
, " CamCtrl found V%i.%02i \r\n",1 + FromCamCtrl.
Version / 100, FromCamCtrl.
Version % 100);
UART1_PutString
(msg
);
}
else UART1_PutString
(" No CamCtrl connected \r\n");
// ++++++++++++++++++++++++++++++++++++++++++++++
for (;;) // the endless main loop
{
PollingTimeout
= 5;
Polling
();
// ++++++++++++++++++++++++++++++++++++++++++++++
if(FromFC_LoadWP_List
)
{
WPL_Store.
Index = (FromFC_LoadWP_List
& ~
0x80);
if(WPL_Store.
Index <= ToFC_MaxWpListIndex
)
{
if(PointList_ReadFromFile
(&WPL_Store
) == WPL_OK
)
{
if(FromFC_LoadWP_List
& 0x80)// -> load relative
{
u32 angle
;
angle
= (360 + GyroCompassCorrected
/10 + Parameter.
OrientationAngle * 15) % 360;
if(NCFlags
& NC_FLAG_FREE
|| NaviData.
TargetPositionDeviation.
Distance > 7*10)
{ // take actual position
if(!PointList_Move
(1,&(GPSData.
Position),angle
)) PointList_Clear
(); // try to move wp-list so that 1st entry matches the current position
}
else
{ // take last target position
if(!PointList_Move
(1, &(NaviData.
TargetPosition),angle
)) PointList_Clear
(); // try to move wp-list so that 1st entry matches the current position
}
}
if(FC.
StatusFlags & FC_STATUS_FLY
) PointList_WPActive
(TRUE
);
GPS_pWaypoint
= PointList_WPBegin
(); // updates POI index
SpeakWaypointRached
= 1; // Speak once when the last Point is reached
SpeakNextWaypoint
= 1; // Speak once as soon as the Points are active
NCFlags
&= ~NC_FLAG_TARGET_REACHED
;
BeepTime
= 150;
}
}
FromFC_LoadWP_List
= 0;
}
// ++++++++++++++++++++++++++++++++++++++++++++++
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;
FromFC_Save_SinglePoint
= 0;
}
// ++++++++++++++++++++++++++++++++++++++++++++++
if(FromFC_Load_SinglePoint
)
{
WPL_Store.
Index = FromFC_Load_SinglePoint
;
if(WPL_Store.
Index <= ToFC_MaxWpListIndex
)
{
if(PointList_LoadSinglePoint
(&WPL_Store
) == WPL_OK
)
{
if(FC.
StatusFlags & FC_STATUS_FLY
) PointList_WPActive
(TRUE
);
GPS_pWaypoint
= PointList_WPBegin
(); // updates POI index
SpeakWaypointRached
= 1; // Speak once when the last Point is reached
SpeakNextWaypoint
= 1; // Speak once as soon as the Points are active
NCFlags
&= ~NC_FLAG_TARGET_REACHED
;
BeepTime
= 150;
}
}
FromFC_Load_SinglePoint
= 0;
}
// ++++++++++++++++++++++++++++++++++++++++++++++
// ---------------- Logging ---------------------------------------
if(SD_WatchDog
)
{
SD_WatchDog
= 30000;
if(SDCardInfo.
Valid == 1) Logging_Update
(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1
else
{
ToFC_MaxWpListIndex
= 0;
if(FC.
StatusFlags & FC_STATUS_START
) SD_LoggingError
= 100;
}
if(!SD_WatchDog
) UART1_PutString
("\n\rSD-Watchdog - Logging aborted\n\r");
}
/*
if(CheckDelay(ftimer))
{
static s8 filename[35];
static u8 i = 0;
s8 dbgmsg[40];
ftimer = SetDelay(100);
if(FC.Poti[3]>100 && fstate == 0)
{
fstate = 1;
sprintf(filename, "/toast/toasta/toast%02i.txt",i++);
}
else if(FC.Poti[3]<100 && fstate == 2)
{
fstate = 3;
}
switch(fstate)
{
case 1:
sprintf(dbgmsg,"\r\nStart writing file: %s", filename);
Debug(dbgmsg);
f = fopen_(filename, 'a');
if(f== NULL) Fat16_Init();
fstate = 2;
break;
case 2:
fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f);
break;
case 3:
sprintf(dbgmsg,"\r\nClosing file: %s", filename);
Debug(dbgmsg);
fclose_(f);
fstate = 0;
break;
default:
break;
}
}
*/
}
}
//DebugOut.Analog[]