Subversion Repositories NaviCtrl

Rev

Rev 587 | Rev 598 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 587 Rev 588
Line 350... Line 350...
350 
                        case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
 350 
                        case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
351 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
 351 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
352 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
 352 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
353 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
 353 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
354 
                                ToFlightCtrl.Param.Byte[3]      = (u8) Kalman_Kompass;
 354 
                                ToFlightCtrl.Param.Byte[3]      = (u8) Kalman_Kompass;
355 
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
 355 
                                ToFlightCtrl.Param.Byte[4] = 0; // siehe bitcodiert unten
356 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
 356 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
357 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
 357 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
358 
                                ToFlightCtrl.Param.Byte[7] = GPS_Aid_StickMultiplikator;
 358 
                                ToFlightCtrl.Param.Byte[7] = GPS_Aid_StickMultiplikator;
359 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
 359 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
360 
                                {
 360 
                                {
361 
//                                      if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.OrientationAngle * 15)) % 360;  // the FC uses the uncorrected comnpass value
 361 
//                                      if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.OrientationAngle * 15)) % 360;  // the FC uses the uncorrected comnpass value
362 
                                        if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.CamOrientation * 15)) % 360;  // the FC uses the uncorrected comnpass value
 362 
                                        if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.CamOrientation * 15)) % 360;  // the FC uses the uncorrected comnpass value
363 
                                        else CAM_Orientation.Azimuth = -1;
 363 
                                        else CAM_Orientation.Azimuth = -1;
- 
 
 364 
                                        if(CAM_Orientation.UpdateMask & FORCE_AZIMUTH_ROTATION) ToFlightCtrl.Param.sByte[4] = 0x01;     // allows Yawing without CareFree (Yawing at Coming Home)
364 
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
 365 
                                        CAM_Orientation.UpdateMask &= ~(CAM_UPDATE_AZIMUTH | FORCE_AZIMUTH_ROTATION);
365 
                                }
 366 
                                }
366 
                                else
 367 
                                else
367 
                                {
 368 
                                {
368 
                                        ToFlightCtrl.Param.sInt[4] = -1;
 369 
                                        ToFlightCtrl.Param.sInt[4] = -1;
369 
                                }
 370 
                                }
Line 817... Line 818...
817 
                        case SPI_FCCMD_SLOW3:  // slow!
 818 
                        case SPI_FCCMD_SLOW3:  // slow!
818 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[0];
 819 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[0];
819 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[1];
 820 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[1];
820 
                                Parameter.DescendRange = FromFlightCtrl.Param.Byte[2];
 821 
                                Parameter.DescendRange = FromFlightCtrl.Param.Byte[2];
821 
                                Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[3];
 822 
                                Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[3];
822 
// 4
 - 
 
- 
 
 823 
                                ServoParams.CompInvert = FromFlightCtrl.Param.Byte[4];
- 
 
 824 
                                Parameter.HomeYawMode = ((ServoParams.CompInvert & 0x18) >> 3);
823 
// 5
 825 
// 5
824 
// 6
826 
// 6
825 
// 7
 827 
// 7
826 
// 8
 828 
// 8
827 
// 9
 829 
// 9
Line 835... Line 837...
835 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
 837 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
836 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
 838 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
837 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
 839 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
838 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
 840 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
839 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
 841 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
840 
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
 - 
 
841 
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
 - 
 
842 
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
 - 
 
843 
                // every time we got new data from the FC via SPI call the navigation routine
 842 
                // every time we got new data from the FC via SPI call the navigation routine
844 
                // and update GPSStick that are returned to FC
 843 
                // and update GPSStick that are returned to FC
845 
                SPI_RxBuffer_Request = 0;
 844 
                SPI_RxBuffer_Request = 0;
846 
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
 845 
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
847 
                ClearFCStatusFlags = 1;
 846 
                ClearFCStatusFlags = 1;