WebSVN - NaviCtrl - Diff - Rev 327 and 328



u32 TimerCheckError;
u8 ErrorCode = 0;
u16 BeepTime;
u8  NCFlags = 0;
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
u8 ErrorGpsFixLost = 0;

 
u8 ClearFCStatusFlags = 0;
u8 StopNavigation = 0;
Param_t Parameter;

 

//----------------------------------------------------------------------------------------------------
 
void CheckErrors(void)
{
    static no_error_delay = 0;

        s32 newErrorCode = 0;
        UART_VersionInfo.HardwareError[0] = 0;

 
        if(CheckDelay(SPI0_Timeout))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"no FC communication ");
                newErrorCode = 3;
                StopNavigation = 1;
                DebugOut.Status[0] &= ~0x01; // status of FC Present
                DebugOut.Status[0] &= ~0x02; // status of BL Present
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;

                LED_RED_ON;
                sprintf(ErrorMSG,"no compass communication ");
                //Reset I2CBus
                I2C1_Deinit();
                I2C1_Init();
                newErrorCode = 4;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
                DebugOut.Status[1] |= 0x08;
        }
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)

#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;
        }

 
        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(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))
        {
                LED_RED_ON;
        if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");
                else
                 {
                  sprintf(ErrorMSG,"no GPS communication ");
                  UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
              newErrorCode = 5;
                 }
                StopNavigation = 1;
//              UBX_Timeout = SetDelay(500);
        }
        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.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 // no error occured
 
 
        {
                StopNavigation = 0;
                LED_RED_OFF;
                if(no_error_delay) { no_error_delay--;  }
                else
                 {
                  sprintf(ErrorMSG,"No Error               ");
                  ErrorCode = 0;
                 }
        }
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04;
        else DebugOut.Status[1] &= ~0x04;
 
    if(newErrorCode)
         {
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // 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;

}
 
// the handler will be cyclic called by the timer 1 ISR

Rev 327	Rev 328
Line 87...	Line 87...
87	u32 TimerCheckError;	87	u32 TimerCheckError;
88	u8 ErrorCode = 0;	88	u8 ErrorCode = 0;
89	u16 BeepTime;	89	u16 BeepTime;
90	u8 NCFlags = 0;	90	u8 NCFlags = 0;
91	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg	91	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
-		92	u8 ErrorGpsFixLost = 0;
Line 92...	Line 93...
92		93
93	u8 ClearFCStatusFlags = 0;	94	u8 ClearFCStatusFlags = 0;
94	u8 StopNavigation = 0;	95	u8 StopNavigation = 0;
95	Param_t Parameter;	96	Param_t Parameter;
Line 131...	Line 132...
131		132
Line 132...	Line 133...
132	//----------------------------------------------------------------------------------------------------	133	//----------------------------------------------------------------------------------------------------
133		134
-		135	void CheckErrors(void)
-		136	{
134	void CheckErrors(void)	137	static no_error_delay = 0;
Line 135...	Line 138...
135	{	138	s32 newErrorCode = 0;
136	UART_VersionInfo.HardwareError[0] = 0;	139	UART_VersionInfo.HardwareError[0] = 0;
Line 143...	Line 146...
143		146
144	if(CheckDelay(SPI0_Timeout))	147	if(CheckDelay(SPI0_Timeout))
145	{	148	{
146	LED_RED_ON;	149	LED_RED_ON;
147	sprintf(ErrorMSG,"no FC communication ");	150	sprintf(ErrorMSG,"no FC communication ");
148	ErrorCode = 3;	151	newErrorCode = 3;
149	StopNavigation = 1;	152	StopNavigation = 1;
150	DebugOut.Status[0] &= ~0x01; // status of FC Present	153	DebugOut.Status[0] &= ~0x01; // status of FC Present
151	DebugOut.Status[0] &= ~0x02; // status of BL Present	154	DebugOut.Status[0] &= ~0x02; // status of BL Present
152	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_SPI_RX;	155	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_SPI_RX;
Line 156...	Line 159...
156	LED_RED_ON;	159	LED_RED_ON;
157	sprintf(ErrorMSG,"no compass communication ");	160	sprintf(ErrorMSG,"no compass communication ");
158	//Reset I2CBus	161	//Reset I2CBus
159	I2C1_Deinit();	162	I2C1_Deinit();
160	I2C1_Init();	163	I2C1_Init();
161	ErrorCode = 4;	164	newErrorCode = 4;
162	StopNavigation = 1;	165	StopNavigation = 1;
163	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_RX;	166	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_RX;
164	DebugOut.Status[1] \|= 0x08;	167	DebugOut.Status[1] \|= 0x08;
165	}	168	}
166	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)	169	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
Line 169...	Line 172...
169	#ifndef FOLLOW_ME	172	#ifndef FOLLOW_ME
170	sprintf(ErrorMSG,"FC not compatible ");	173	sprintf(ErrorMSG,"FC not compatible ");
171	#else	174	#else
172	sprintf(ErrorMSG,"! FollowMe only ! ");	175	sprintf(ErrorMSG,"! FollowMe only ! ");
173	#endif	176	#endif
174	ErrorCode = 1;	177	newErrorCode = 1;
175	StopNavigation = 1;	178	StopNavigation = 1;
176	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_FC_INCOMPATIBLE;	179	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_FC_INCOMPATIBLE;
177	}	180	}
Line 178...	Line 181...
178		181
179	else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)	182	else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
180	{	183	{
181	LED_RED_ON;	184	LED_RED_ON;
182	sprintf(ErrorMSG,"ERR: FC Nick Gyro");	185	sprintf(ErrorMSG,"ERR: FC Nick Gyro");
183	ErrorCode = 10;	186	newErrorCode = 10;
184	}	187	}
185	else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)	188	else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
186	{	189	{
187	LED_RED_ON;	190	LED_RED_ON;
188	sprintf(ErrorMSG,"ERR: FC Roll Gyro");	191	sprintf(ErrorMSG,"ERR: FC Roll Gyro");
189	ErrorCode = 11;	192	newErrorCode = 11;
190	}	193	}
191	else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)	194	else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
192	{	195	{
193	LED_RED_ON;	196	LED_RED_ON;
194	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");	197	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
195	ErrorCode = 12;	198	newErrorCode = 12;
196	}	199	}
197	else if(FC.Error[0] & FC_ERROR0_ACC_NICK)	200	else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
198	{	201	{
199	LED_RED_ON;	202	LED_RED_ON;
200	sprintf(ErrorMSG,"ERR: FC Nick ACC");	203	sprintf(ErrorMSG,"ERR: FC Nick ACC");
201	ErrorCode = 13;	204	newErrorCode = 13;
202	}	205	}
203	else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)	206	else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
204	{	207	{
205	LED_RED_ON;	208	LED_RED_ON;
206	sprintf(ErrorMSG,"ERR: FC Roll ACC");	209	sprintf(ErrorMSG,"ERR: FC Roll ACC");
207	ErrorCode = 14;	210	newErrorCode = 14;
208	}	211	}
209	else if(FC.Error[0] & FC_ERROR0_ACC_TOP)	212	else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
210	{	213	{
211	LED_RED_ON;	214	LED_RED_ON;
212	sprintf(ErrorMSG,"ERR:FC Z-ACC");	215	sprintf(ErrorMSG,"ERR:FC Z-ACC");
213	ErrorCode = 15;	216	newErrorCode = 15;
214	}	217	}
215	else if(FC.Error[0] & FC_ERROR0_PRESSURE)	218	else if(FC.Error[0] & FC_ERROR0_PRESSURE)
216	{	219	{
217	LED_RED_ON;	220	LED_RED_ON;
218	sprintf(ErrorMSG,"ERR:Pressure sensor");	221	sprintf(ErrorMSG,"ERR:Pressure sensor");
219	ErrorCode = 16;	222	newErrorCode = 16;
220	}	223	}
221	else if(FC.Error[1] & FC_ERROR1_I2C)	224	else if(FC.Error[1] & FC_ERROR1_I2C)
222	{	225	{
223	LED_RED_ON;	226	LED_RED_ON;
224	sprintf(ErrorMSG,"ERR:I2C FC to BL");	227	sprintf(ErrorMSG,"ERR:I2C FC to BL");
225	ErrorCode = 17;	228	newErrorCode = 17;
226	}	229	}
227	else if(FC.Error[1] & FC_ERROR1_BL_MISSING)	230	else if(FC.Error[1] & FC_ERROR1_BL_MISSING)
228	{	231	{
229	LED_RED_ON;	232	LED_RED_ON;
230	sprintf(ErrorMSG,"ERR: Bl Missing");	233	sprintf(ErrorMSG,"ERR: Bl Missing");
231	ErrorCode = 18;	234	newErrorCode = 18;
232	}	235	}
233	else if(FC.Error[1] & FC_ERROR1_MIXER)	236	else if(FC.Error[1] & FC_ERROR1_MIXER)
234	{	237	{
235	LED_RED_ON;	238	LED_RED_ON;
236	sprintf(ErrorMSG,"Mixer Error");	239	sprintf(ErrorMSG,"Mixer Error");
237	ErrorCode = 19;	240	newErrorCode = 19;
238	}	241	}
239	else if(CheckDelay(UBX_Timeout))	242	else if(CheckDelay(UBX_Timeout))
240	{	243	{
241	LED_RED_ON;	244	LED_RED_ON;
242	if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");	245	if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");
243	else	246	else
244	{	247	{
245	sprintf(ErrorMSG,"no GPS communication ");	248	sprintf(ErrorMSG,"no GPS communication ");
246	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_GPS_RX;	249	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_GPS_RX;
247	ErrorCode = 5;	250	newErrorCode = 5;
248	}	251	}
249	StopNavigation = 1;	252	StopNavigation = 1;
250	// UBX_Timeout = SetDelay(500);	253	// UBX_Timeout = SetDelay(500);
251	}	254	}
252	else if(Compass_Heading < 0)	255	else if(Compass_Heading < 0)
253	{	256	{
254	LED_RED_ON;	257	LED_RED_ON;
255	sprintf(ErrorMSG,"bad compass value ");	258	sprintf(ErrorMSG,"bad compass value ");
256	ErrorCode = 6;	259	newErrorCode = 6;
257	StopNavigation = 1;	260	StopNavigation = 1;
258	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;	261	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;
259	}	262	}
260	else if((FC.Error[1] & FC_ERROR1_SPI_RX))	263	else if((FC.Error[1] & FC_ERROR1_SPI_RX))
261	{	264	{
262	LED_RED_ON;	265	LED_RED_ON;
263	sprintf(ErrorMSG,"FC spi rx error ");	266	sprintf(ErrorMSG,"FC spi rx error ");
264	ErrorCode = 8;	267	newErrorCode = 8;
265	StopNavigation = 1;	268	StopNavigation = 1;
266	}	269	}
267	else if(FC.Error[0] & FC_ERROR0_CAREFREE)	270	else if(FC.Error[0] & FC_ERROR0_CAREFREE)
268	{	271	{
269	LED_RED_ON;	272	LED_RED_ON;
270	sprintf(ErrorMSG,"FC: Carefree Error");	273	sprintf(ErrorMSG,"FC: Carefree Error");
271	ErrorCode = 20;	274	newErrorCode = 20;
272	}	275	}
273	else if(FC.Error[1] & FC_ERROR1_PPM)	276	else if(FC.Error[1] & FC_ERROR1_PPM)
274	{	277	{
275	LED_RED_ON;	278	LED_RED_ON;
276	sprintf(ErrorMSG,"RC Signal lost ");	279	sprintf(ErrorMSG,"RC Signal lost ");
-		280	newErrorCode = 7;
-		281	}
-		282	else if(ErrorGpsFixLost)
-		283	{
-		284	LED_RED_ON;
-		285	sprintf(ErrorMSG,"GPS Fix lost ");
277	ErrorCode = 7;	286	newErrorCode = 21;
278	}	287	}
279	else // no error occured	288	else // no error occured
280	{	-
281	sprintf(ErrorMSG,"No Error ");	-
282	ErrorCode = 0;	289	{
283	StopNavigation = 0;	290	StopNavigation = 0;
-		291	LED_RED_OFF;
-		292	if(no_error_delay) { no_error_delay--; }
-		293	else
-		294	{
-		295	sprintf(ErrorMSG,"No Error ");
-		296	ErrorCode = 0;
284	LED_RED_OFF;	297	}
285	}	298	}
286	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 0x04;	299	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 0x04;
-		300	else DebugOut.Status[1] &= ~0x04;
-		301
-		302	if(newErrorCode)
-		303	{
-		304	if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // delay the errors if the motors are running
-		305	ErrorCode = newErrorCode;
-		306	}
-		307	FC.Error[0] = 0;
-		308	FC.Error[1] = 0;
-		309	FC.Error[2] = 0;
-		310	FC.Error[3] = 0;
-		311	FC.Error[4] = 0;
287	else DebugOut.Status[1] &= ~0x04;	312	ErrorGpsFixLost = 0;
Line 288...	Line 313...
288	}	313	}
289		314
290	// the handler will be cyclic called by the timer 1 ISR	315	// the handler will be cyclic called by the timer 1 ISR

Subversion Repositories NaviCtrl

(root)/tags/V2.00a/main.c @ 811 - Rev 327 → 328