Rev 199 | Rev 204 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 199 | Rev 202 | ||
---|---|---|---|
Line 96... | Line 96... | ||
96 | s32 Kalman_K = 32; |
96 | s32 Kalman_K = 32; |
97 | s32 Kalman_MaxDrift = 5 * 16; |
97 | s32 Kalman_MaxDrift = 5 * 16; |
98 | s32 Kalman_MaxFusion = 64; |
98 | s32 Kalman_MaxFusion = 64; |
99 | s32 ToFcGpsZ = 0; |
99 | s32 ToFcGpsZ = 0; |
Line 100... | Line 100... | ||
100 | 100 | ||
101 | u8 SPI_CommandSequence[] = { SPI_KALMAN }; |
101 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN}; |
Line 102... | Line 102... | ||
102 | u8 SPI_CommandCounter = 0; |
102 | u8 SPI_CommandCounter = 0; |
Line 103... | Line 103... | ||
103 | 103 | ||
Line 244... | Line 244... | ||
244 | SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low; |
244 | SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low; |
245 | SSP_InitStructure.SSP_ClockRate = 0; |
245 | SSP_InitStructure.SSP_ClockRate = 0; |
Line 246... | Line 246... | ||
246 | 246 | ||
247 | SSP_Init(SSP0, &SSP_InitStructure); |
247 | SSP_Init(SSP0, &SSP_InitStructure); |
248 | SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE); |
248 | SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE); |
249 | 249 | ||
Line 250... | Line 250... | ||
250 | fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue)); |
250 | fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue)); |
251 | 251 | ||
252 | SSP_Cmd(SSP0, ENABLE); |
252 | SSP_Cmd(SSP0, ENABLE); |
Line 267... | Line 267... | ||
267 | //------------------------------------------------------ |
267 | //------------------------------------------------------ |
268 | void SPI0_UpdateBuffer(void) |
268 | void SPI0_UpdateBuffer(void) |
269 | { |
269 | { |
270 | static u32 timeout = 0; |
270 | static u32 timeout = 0; |
271 | static u8 counter = 50; |
271 | static u8 counter = 50; |
272 | 272 | ||
273 | if (SPI_RxBuffer_Request) |
273 | if (SPI_RxBuffer_Request) |
274 | { |
274 | { |
275 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
275 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
276 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
276 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
277 | ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
277 | ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
Line 284... | Line 284... | ||
284 | // restart command cycle at the end |
284 | // restart command cycle at the end |
285 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
285 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
Line 286... | Line 286... | ||
286 | 286 | ||
287 | switch (ToFlightCtrl.Command) |
287 | switch (ToFlightCtrl.Command) |
288 | { |
288 | { |
289 | case SPI_KALMAN: |
289 | case SPI_NCCMD_KALMAN: |
290 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
290 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
291 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
291 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
292 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
292 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
293 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
293 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
294 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
294 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
Line -... | Line 295... | ||
- | 295 | break; |
|
- | 296 | ||
- | 297 | case SPI_NCCMD_VERSION: |
|
- | 298 | ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR; |
|
- | 299 | ToFlightCtrl.Param.Byte[1] = VERSION_MINOR; |
|
- | 300 | ToFlightCtrl.Param.Byte[2] = VERSION_MINOR; |
|
- | 301 | ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE; |
|
- | 302 | ToFlightCtrl.Param.Byte[4] = 0; |
|
295 | break; |
303 | break; |
296 | 304 | ||
297 | default: |
305 | default: |
298 | break; |
306 | break; |
Line 299... | Line 307... | ||
299 | } |
307 | } |
300 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
308 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
301 | 309 | ||
302 | 310 | ||
303 | switch(FromFlightCtrl.Command) |
311 | switch(FromFlightCtrl.Command) |
304 | { |
312 | { |
305 | case SPI_CMD_USER: |
313 | case SPI_FCCMD_USER: |
306 | Parameter.User1 = FromFlightCtrl.Param.Byte[0]; |
314 | Parameter.User1 = FromFlightCtrl.Param.Byte[0]; |
Line 324... | Line 332... | ||
324 | break; |
332 | break; |
Line 325... | Line 333... | ||
325 | 333 | ||
326 | #define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; } |
334 | #define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; } |
Line 327... | Line 335... | ||
327 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); } |
335 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); } |
328 | 336 | ||
329 | case SPI_CMD_PARAMETER1: |
337 | case SPI_FCCMD_PARAMETER1: |
330 | CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255); |
338 | CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255); |
331 | CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255); |
339 | CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255); |
332 | CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255); |
340 | CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255); |
Line 339... | Line 347... | ||
339 | CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255); |
347 | CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255); |
340 | CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255); |
348 | CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255); |
341 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
349 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
342 | break; |
350 | break; |
Line 343... | Line 351... | ||
343 | 351 | ||
344 | case SPI_CMD_STICK: |
352 | case SPI_FCCMD_STICK: |
345 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
353 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
346 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
354 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
347 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |
355 | FC.StickRoll = FromFlightCtrl.Param.sByte[2]; |
348 | FC.StickNick = FromFlightCtrl.Param.sByte[3]; |
356 | FC.StickNick = FromFlightCtrl.Param.sByte[3]; |
Line 351... | Line 359... | ||
351 | FC.Poti[2] = FromFlightCtrl.Param.Byte[6]; |
359 | FC.Poti[2] = FromFlightCtrl.Param.Byte[6]; |
352 | FC.Poti[3] = FromFlightCtrl.Param.Byte[7]; |
360 | FC.Poti[3] = FromFlightCtrl.Param.Byte[7]; |
353 | FC.Poti[4] = FromFlightCtrl.Param.Byte[8]; |
361 | FC.Poti[4] = FromFlightCtrl.Param.Byte[8]; |
354 | FC.Poti[5] = FromFlightCtrl.Param.Byte[9]; |
362 | FC.Poti[5] = FromFlightCtrl.Param.Byte[9]; |
355 | FC.Poti[6] = FromFlightCtrl.Param.Byte[10]; |
363 | FC.Poti[6] = FromFlightCtrl.Param.Byte[10]; |
356 | FC.Poti[7] = FromFlightCtrl.Param.Byte[11]; |
364 | FC.Poti[7] = FromFlightCtrl.Param.Byte[11]; |
357 | break; |
365 | break; |
Line 358... | Line 366... | ||
358 | 366 | ||
359 | case SPI_CMD_MISC: |
367 | case SPI_FCCMD_MISC: |
360 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
368 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
361 | { // put only new CompassCalState into queue to send via I2C |
369 | { // put only new CompassCalState into queue to send via I2C |
362 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
370 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
363 | fifo_put(&CompassCalcStateFiFo, CompassCalState); |
371 | fifo_put(&CompassCalcStateFiFo, CompassCalState); |
Line 372... | Line 380... | ||
372 | FC.RC_Quality = FromFlightCtrl.Param.Byte[9]; |
380 | FC.RC_Quality = FromFlightCtrl.Param.Byte[9]; |
373 | FC.RC_RSSI = FromFlightCtrl.Param.Byte[10]; |
381 | FC.RC_RSSI = FromFlightCtrl.Param.Byte[10]; |
374 | NaviData.Gas = (FC.UBat * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning; |
382 | NaviData.Gas = (FC.UBat * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning; |
375 | break; |
383 | break; |
Line 376... | Line 384... | ||
376 | 384 | ||
377 | case SPI_CMD_SERVOS: |
385 | case SPI_FCCMD_SERVOS: |
378 | ServoParams.Refresh = FromFlightCtrl.Param.Byte[0]; |
386 | ServoParams.Refresh = FromFlightCtrl.Param.Byte[0]; |
379 | ServoParams.CompInvert = FromFlightCtrl.Param.Byte[1]; |
387 | ServoParams.CompInvert = FromFlightCtrl.Param.Byte[1]; |
380 | ServoParams.NickControl = FromFlightCtrl.Param.Byte[2]; |
388 | ServoParams.NickControl = FromFlightCtrl.Param.Byte[2]; |
381 | ServoParams.NickComp = FromFlightCtrl.Param.Byte[3]; |
389 | ServoParams.NickComp = FromFlightCtrl.Param.Byte[3]; |
Line 385... | Line 393... | ||
385 | ServoParams.RollComp = FromFlightCtrl.Param.Byte[7]; |
393 | ServoParams.RollComp = FromFlightCtrl.Param.Byte[7]; |
386 | ServoParams.RollMin = FromFlightCtrl.Param.Byte[8]; |
394 | ServoParams.RollMin = FromFlightCtrl.Param.Byte[8]; |
387 | ServoParams.RollMax = FromFlightCtrl.Param.Byte[9]; |
395 | ServoParams.RollMax = FromFlightCtrl.Param.Byte[9]; |
388 | break; |
396 | break; |
Line 389... | Line 397... | ||
389 | 397 | ||
390 | case SPI_CMD_VERSION: |
398 | case SPI_FCCMD_VERSION: |
391 | FC_Version.Major = FromFlightCtrl.Param.Byte[0]; |
399 | FC_Version.Major = FromFlightCtrl.Param.Byte[0]; |
392 | FC_Version.Minor = FromFlightCtrl.Param.Byte[1]; |
400 | FC_Version.Minor = FromFlightCtrl.Param.Byte[1]; |
393 | FC_Version.Patch = FromFlightCtrl.Param.Byte[2]; |
401 | FC_Version.Patch = FromFlightCtrl.Param.Byte[2]; |
394 | FC_Version.Compatible = FromFlightCtrl.Param.Byte[3]; |
402 | FC_Version.Compatible = FromFlightCtrl.Param.Byte[3]; |
Line 400... | Line 408... | ||
400 | } |
408 | } |
Line 401... | Line 409... | ||
401 | 409 | ||
402 | // every time we got new data from the FC via SPI call the navigation routine |
410 | // every time we got new data from the FC via SPI call the navigation routine |
403 | GPS_Navigation(); |
411 | GPS_Navigation(); |
404 | ClearFCFlags = 1; |
412 | ClearFCFlags = 1; |
405 | 413 | ||
406 | if(counter) |
414 | if(counter) |
407 | { |
415 | { |
408 | counter--; // count down to enable servo |
416 | counter--; // count down to enable servo |
409 | if(!counter) TIMER2_Init(); // enable Servo Output |
417 | if(!counter) TIMER2_Init(); // enable Servo Output |
Line 410... | Line 418... | ||
410 | } |
418 | } |
411 | 419 | ||
Line 420... | Line 428... | ||
420 | Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg |
428 | Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg |
421 | Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg |
429 | Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg |
422 | Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg |
430 | Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg |
423 | } // EOF if(SPI_RxBuffer_Request) |
431 | } // EOF if(SPI_RxBuffer_Request) |
424 | else // no new SPI data |
432 | else // no new SPI data |
425 | { |
433 | { |
426 | if(CheckDelay(timeout) && (counter == 0)) |
434 | if(CheckDelay(timeout) && (counter == 0)) |
427 | { |
435 | { |
428 | TIMER2_Deinit(); // disable Servo Output |
436 | TIMER2_Deinit(); // disable Servo Output |
429 | counter = 50; // reset counter for enabling Servo Output |
437 | counter = 50; // reset counter for enabling Servo Output |
430 | } |
438 | } |