Rev 514 | Rev 516 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 514 | Rev 515 | ||
---|---|---|---|
Line 300... | Line 300... | ||
300 | unsigned char Byte[4]; |
300 | unsigned char Byte[4]; |
301 | unsigned int Int[2]; |
301 | unsigned int Int[2]; |
302 | unsigned long Long; |
302 | unsigned long Long; |
303 | } Temp; |
303 | } Temp; |
304 | */ |
304 | */ |
305 | SPIWatchDog = 3500; // stop communication to FC after this timeout |
305 | SPIWatchDog = 3500; // stop communication to FC after this timeout |
306 | if(SPI_RxBuffer_Request) |
306 | if(SPI_RxBuffer_Request) |
307 | { |
307 | { |
308 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
308 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
309 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
309 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
310 | ToFlightCtrl.CompassHeading = Compass_Heading; |
310 | ToFlightCtrl.CompassHeading = Compass_Heading; |
Line 315... | Line 315... | ||
315 | ToFlightCtrl.MagVecY = MagVector.Y; |
315 | ToFlightCtrl.MagVecY = MagVector.Y; |
316 | ToFlightCtrl.MagVecZ = MagVector.Z; |
316 | ToFlightCtrl.MagVecZ = MagVector.Z; |
317 | // ToFlightCtrl.NCStatus = 0; |
317 | // ToFlightCtrl.NCStatus = 0; |
318 | // cycle spi commands |
318 | // cycle spi commands |
319 | if(ErrorCode != last_error_code && enable_injecting) |
319 | if(ErrorCode != last_error_code && enable_injecting) |
320 | { |
320 | { |
321 | ToFlightCtrl.Command = SPI_NCCMD_VERSION; |
321 | ToFlightCtrl.Command = SPI_NCCMD_VERSION; |
322 | last_error_code = ErrorCode; |
322 | last_error_code = ErrorCode; |
323 | enable_injecting = 0; |
323 | enable_injecting = 0; |
324 | } |
324 | } |
325 | else |
325 | else |
326 | if(FC_WP_EventChannel != last_wp_event && enable_injecting) |
326 | if(FC_WP_EventChannel != last_wp_event && enable_injecting) |
327 | { |
327 | { |
328 | ToFlightCtrl.Command = SPI_NCCMD_GPSINFO; |
328 | ToFlightCtrl.Command = SPI_NCCMD_GPSINFO; |
329 | last_wp_event = FC_WP_EventChannel; |
329 | last_wp_event = FC_WP_EventChannel; |
330 | enable_injecting = 0; |
330 | enable_injecting = 0; |
331 | } |
331 | } |
332 | else |
332 | else |
333 | { |
333 | { |
334 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
334 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
335 | // restart command cycle at the end |
335 | // restart command cycle at the end |
336 | if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
336 | if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
337 | if(ToFlightCtrl.Command == SPI_NCCMD_KALMAN) enable_injecting = 1; |
337 | if(ToFlightCtrl.Command == SPI_NCCMD_KALMAN) enable_injecting = 1; |
338 | } |
338 | } |
Line 339... | Line 339... | ||
339 | 339 | ||
340 | #define FLAG_GPS_AID 0x01 |
340 | #define FLAG_GPS_AID 0x01 |
341 | switch (ToFlightCtrl.Command) |
341 | switch (ToFlightCtrl.Command) |
342 | { |
342 | { |
Line 346... | Line 346... | ||
346 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
346 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
347 | ToFlightCtrl.Param.Byte[3] = (u8) Kalman_Kompass; |
347 | ToFlightCtrl.Param.Byte[3] = (u8) Kalman_Kompass; |
348 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
348 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
349 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
349 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
350 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
350 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
351 | ToFlightCtrl.Param.Byte[7] = GPS_Aid_StickMultiplikator; |
351 | ToFlightCtrl.Param.Byte[7] = GPS_Aid_StickMultiplikator; |
352 | if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) |
352 | if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) |
353 | { |
353 | { |
354 | if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + 720 - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10)) % 360; // the FC uses the uncorrected comnpass value |
354 | if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + 720 - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10)) % 360; // the FC uses the uncorrected comnpass value |
355 | else CAM_Orientation.Azimuth = -1; |
355 | else CAM_Orientation.Azimuth = -1; |
356 | CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH; |
356 | CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH; |
Line 373... | Line 373... | ||
373 | ToFlightCtrl.Param.sInt[5] = POI_KameraNick; |
373 | ToFlightCtrl.Param.sInt[5] = POI_KameraNick; |
374 | break; |
374 | break; |
Line 375... | Line 375... | ||
375 | 375 | ||
376 | case SPI_NCCMD_VERSION: |
376 | case SPI_NCCMD_VERSION: |
377 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
377 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
378 | //+ higher than the maximum allowed altitude |
378 | //+ higher than the maximum allowed altitude |
379 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
379 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
380 | ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR; |
380 | ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR; |
381 | ToFlightCtrl.Param.Byte[1] = VERSION_MINOR; |
381 | ToFlightCtrl.Param.Byte[1] = VERSION_MINOR; |
382 | ToFlightCtrl.Param.Byte[2] = VERSION_PATCH; |
382 | ToFlightCtrl.Param.Byte[2] = VERSION_PATCH; |
Line 448... | Line 448... | ||
448 | { |
448 | { |
449 | ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint; |
449 | ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint; |
450 | } |
450 | } |
451 | break; |
451 | break; |
452 | case SPI_NCCMD_HOTT_INFO: |
452 | case SPI_NCCMD_HOTT_INFO: |
453 | if(NewWPL_Name) hott_index = 100; |
453 | if(NewWPL_Name) hott_index = 100; |
454 | switch(hott_index++) |
454 | switch(hott_index++) |
455 | { |
455 | { |
456 | case 0: |
456 | case 0: |
457 | //Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden |
457 | //Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden |
458 | //53.28 5788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
458 | //53.28 5788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
Line 562... | Line 562... | ||
562 | NewWPL_Name = 0; |
562 | NewWPL_Name = 0; |
563 | hott_index = 0; |
563 | hott_index = 0; |
564 | break; |
564 | break; |
565 | default: |
565 | default: |
566 | ToFlightCtrl.Param.Byte[0] = 255; |
566 | ToFlightCtrl.Param.Byte[0] = 255; |
567 | hott_index = 0; |
567 | hott_index = 0; |
568 | break; |
568 | break; |
569 | } |
569 | } |
570 | break; |
570 | break; |
571 | default: |
571 | default: |
572 | break; |
572 | break; |
573 | // 0 = 0,1 |
573 | // 0 = 0,1 |
Line 612... | Line 612... | ||
612 | FC_is_Calibrated = 1; |
612 | FC_is_Calibrated = 1; |
613 | Compass_Check(); |
613 | Compass_Check(); |
614 | } |
614 | } |
615 | } |
615 | } |
616 | } |
616 | } |
617 | if(FC.StatusFlags & FC_STATUS_START) |
617 | if(FC.StatusFlags & FC_STATUS_START) |
618 | { |
618 | { |
619 | // if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
619 | // if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else |
620 | if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600; |
620 | if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600; |
621 | else HeadFreeStartAngle = GyroCompassCorrected; |
621 | else HeadFreeStartAngle = GyroCompassCorrected; |
622 | } |
622 | } |
Line 623... | Line 623... | ||
623 | 623 | ||
624 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
624 | if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
Line 665... | Line 665... | ||
665 | Motor[FromFlightCtrl.Param.Byte[6]].MaxPWM = FromFlightCtrl.Param.Byte[8]; |
665 | Motor[FromFlightCtrl.Param.Byte[6]].MaxPWM = FromFlightCtrl.Param.Byte[8]; |
666 | Motor[FromFlightCtrl.Param.Byte[6]].State = FromFlightCtrl.Param.Byte[9]; |
666 | Motor[FromFlightCtrl.Param.Byte[6]].State = FromFlightCtrl.Param.Byte[9]; |
667 | Motor[FromFlightCtrl.Param.Byte[6]].Temperature = FromFlightCtrl.Param.Byte[10]; |
667 | Motor[FromFlightCtrl.Param.Byte[6]].Temperature = FromFlightCtrl.Param.Byte[10]; |
668 | Motor[FromFlightCtrl.Param.Byte[6]].Current = FromFlightCtrl.Param.Byte[11]; |
668 | Motor[FromFlightCtrl.Param.Byte[6]].Current = FromFlightCtrl.Param.Byte[11]; |
669 | if(FromFC_VarioCharacter == '+' || FromFC_VarioCharacter == '-') // manual setpoint clears the NC-Parameter command |
669 | if(FromFC_VarioCharacter == '+' || FromFC_VarioCharacter == '-') // manual setpoint clears the NC-Parameter command |
670 | { |
670 | { |
671 | NCParams_ClearValue(NCPARAMS_ALTITUDE_RATE); |
671 | NCParams_ClearValue(NCPARAMS_ALTITUDE_RATE); |
672 | } |
672 | } |
673 | NaviData.UBat = FC.BAT_Voltage; |
673 | NaviData.UBat = FC.BAT_Voltage; |
674 | NaviData.Current = FC.BAT_Current; |
674 | NaviData.Current = FC.BAT_Current; |
675 | NaviData.UsedCapacity = FC.BAT_UsedCapacity; |
675 | NaviData.UsedCapacity = FC.BAT_UsedCapacity; |
Line 676... | Line 676... | ||
676 | 676 | ||
Line 690... | Line 690... | ||
690 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
690 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
691 | break; |
691 | break; |
692 | case SPI_FCCMD_PARAMETER2: |
692 | case SPI_FCCMD_PARAMETER2: |
693 | CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255); |
693 | CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255); |
694 | if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1]; // will be cleared in the SD-Logging |
694 | if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1]; // will be cleared in the SD-Logging |
695 | Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2]; |
695 | Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2]; |
696 | Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3]; |
696 | Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3]; |
697 | Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4]; |
697 | Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4]; |
698 | if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5]; |
698 | if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5]; |
699 | if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SinglePoint = FromFlightCtrl.Param.Byte[6]; |
699 | if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SinglePoint = FromFlightCtrl.Param.Byte[6]; |
700 | if(FromFlightCtrl.Param.Byte[7]) FromFC_Save_SinglePoint = FromFlightCtrl.Param.Byte[7]; |
700 | if(FromFlightCtrl.Param.Byte[7]) FromFC_Save_SinglePoint = FromFlightCtrl.Param.Byte[7]; |
Line 714... | Line 714... | ||
714 | FC.Poti[3] = FromFlightCtrl.Param.Byte[7]; |
714 | FC.Poti[3] = FromFlightCtrl.Param.Byte[7]; |
715 | FC.Poti[4] = FromFlightCtrl.Param.Byte[8]; |
715 | FC.Poti[4] = FromFlightCtrl.Param.Byte[8]; |
716 | FC.Poti[5] = FromFlightCtrl.Param.Byte[9]; |
716 | FC.Poti[5] = FromFlightCtrl.Param.Byte[9]; |
717 | FC.Poti[6] = FromFlightCtrl.Param.Byte[10]; |
717 | FC.Poti[6] = FromFlightCtrl.Param.Byte[10]; |
718 | FC.Poti[7] = FromFlightCtrl.Param.Byte[11]; |
718 | FC.Poti[7] = FromFlightCtrl.Param.Byte[11]; |
719 | CHK_POTI_MM(WaypointAcceleration,WaypointAccelerationSetting,0,255); // that could be a Poti-Value |
719 | CHK_POTI_MM(WaypointAcceleration,WaypointAccelerationSetting,0,255); // that could be a Poti-Value |
720 | break; |
720 | break; |
Line 721... | Line 721... | ||
721 | 721 | ||
722 | case SPI_FCCMD_MISC: |
722 | case SPI_FCCMD_MISC: |
723 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
723 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |