Subversion Repositories NaviCtrl

Rev

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

Rev 480 Rev 482
Line 54... Line 54...
54 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
 54 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 55 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 56... Line 56...
56 
 
 56 
 
57 
#include <string.h>
 57 
#include <string.h>
- 
 
 58 
#include <math.h>
58 
#include <math.h>
 59 
#include <stdlib.h>
59 
#include "91x_lib.h"
 60 
#include "91x_lib.h"
60 
#include "led.h"
 61 
#include "led.h"
61 
#include "gps.h"
 62 
#include "gps.h"
62 
#include "uart1.h"
 63 
#include "uart1.h"
63 
#include "spi_slave.h"
 64 
#include "spi_slave.h"
64 
#include "compass.h"
 65 
#include "compass.h"
65 
#include "timer1.h"
 66 
#include "timer1.h"
66 
#include "timer2.h"
 67 
#include "timer2.h"
67 
#include "config.h"
 68 
#include "config.h"
68 
#include "main.h"
 - 
 
69 
#include "compass.h"
 69 
#include "main.h"
70 
#include "params.h"
 - 
 
71 
#include "stdlib.h"
 70 
#include "params.h"
72 
#include "settings.h"
 - 
 
Line 73... Line 71...
73 
#include "ncmag.h"
 71 
#include "settings.h"
74 
 
 72 
 
75 
#define SPI_RXSYNCBYTE1 0xAA
 73 
#define SPI_RXSYNCBYTE1 0xAA
76 
#define SPI_RXSYNCBYTE2 0x83
 74 
#define SPI_RXSYNCBYTE2 0x83
Line 584... Line 582...
584 
                                        FCCalibActive = 10;
 582 
                                        FCCalibActive = 10;
585 
                                        FC_is_Calibrated = 0;
 583 
                                        FC_is_Calibrated = 0;
586 
                                }
 584 
                                }
587 
                                else
 585 
                                else
588 
                                {
 586 
                                {
589 
                                        if(FCCalibActive)
 587 
                                        if(FCCalibActive)
590 
                                         {
 588 
                                        {
591 
                                            if(--FCCalibActive == 0)
 589 
                                                if(--FCCalibActive == 0)
592 
                                                 {
 590 
                                                {
593 
                                                  FC_is_Calibrated = 1;
 591 
                                                        FC_is_Calibrated = 1;
594 
                                                  ExtCompassOrientation = GetExtCompassOrientation();
 592 
                                                        Compass_Check();
595 
if(ExtCompassOrientation != Calibration.Version / 16) NCMAG_IsCalibrated = 0;
 - 
 
596 
                                                 }
 593 
                                                }
597 
                                         }
 594 
                                        }
598 
                                }
 595 
                                }
599 
                                if(FC.StatusFlags & FC_STATUS_START)
 596 
                                if(FC.StatusFlags & FC_STATUS_START)
600 
                             {
 597 
                                {
601 
                                   if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else
 598 
                                        if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else
602 
                                   HeadFreeStartAngle = GyroCompassCorrected;
 599 
                                        HeadFreeStartAngle = GyroCompassCorrected;
603 
                                 }
 600 
                                }
Line 604... Line 601...
604 
 
 601 
 
605 
                                 if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
 602 
                                if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
606 
                                  {
 603 
                                {
607 
                                   if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
 604 
                                        if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
608 
                                    {
 605 
                                        {
609 
                                         if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))       // nur bei ausreichender Distance -> 20m
 606 
                                                if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))        // nur bei ausreichender Distance -> 20m
610 
                                         {
 607 
                                                {
611 
                                          HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
 608 
                                                        HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
612 
                                         }
 609 
                                                }
613 
                                         else                                                                                            // Ansonsten die aktuelle Richtung übernehmen
 610 
                                                else                                                                                             // Ansonsten die aktuelle Richtung übernehmen
614 
                                          HeadFreeStartAngle = GyroCompassCorrected; // in 0.1°  
611 
                                                HeadFreeStartAngle = GyroCompassCorrected; // in 0.1°    
615 
                                        }
 612 
                                        }
- 
 
 613 
                                }
616 
                                }
 614 
 
617 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
 615 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
618 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[10];
 616 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[10];
619 
                                DebugOut.Analog[7] = FC.BAT_Voltage;
 617 
                                DebugOut.Analog[7] = FC.BAT_Voltage;
620 
                                DebugOut.Analog[5] = FC.StatusFlags;
 618 
                                DebugOut.Analog[5] = FC.StatusFlags;
Line 670... Line 668...
670 
                                break;
 668 
                                break;
671 
                        case SPI_FCCMD_PARAMETER2:
 669 
                        case SPI_FCCMD_PARAMETER2:
672 
                                CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255);
 670 
                                CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255);
673 
                                if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1];     // will be cleared in the SD-Logging
 671 
                                if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1];     // will be cleared in the SD-Logging
674 
                Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2];
 672 
                Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2];
675 
                                Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3];
 673 
                                Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3];
676 
                                break;
 674 
                                break;
677 
                        case SPI_FCCMD_STICK:
 675 
                        case SPI_FCCMD_STICK:
678 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
 676 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
679 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
 677 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
680 
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
 678 
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
Line 739... Line 737...
739 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
 737 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
740 
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
 738 
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
741 
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
 739 
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
742 
                                if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
 740 
                                if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
743 
                                {
 741 
                                {
744 
                                 FC.FromFC_DisableDeclination = 1;
 742 
                                        FC.FromFC_DisableDeclination = 1;
745 
                                 FC.FromFC_CompassOffset = 10 * (signed char) ((unsigned char) FromFlightCtrl.Param.Byte[7] - 128);
 743 
                                        FC.FromFC_CompassOffset = 10 * (s8) (FromFlightCtrl.Param.Byte[7] - 128);
746 
                                 GeoMagDec = 0;
 744 
                                        GeoMagDec = 0;
747 
                                }
 745 
                                }
748 
                                else
 746 
                                else
749 
                                {
 747 
                                {
750 
                                 FC.FromFC_DisableDeclination = 0;
 748 
                                        FC.FromFC_DisableDeclination = 0;
751 
                                 FC.FromFC_CompassOffset = 10 * (signed char) FromFlightCtrl.Param.Byte[7];
 749 
                                        FC.FromFC_CompassOffset = 10 * FromFlightCtrl.Param.sByte[7];
752 
                                }
 750 
                                }
753 
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
 751 
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
754 
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
 752 
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
755 
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
 753 
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
756 
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
 754 
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
Line 816... Line 814...
816 
                        SPI0_UpdateBuffer();
 814 
                        SPI0_UpdateBuffer();
817 
                        if (FC_Version.Major != 0xFF)  break;
 815 
                        if (FC_Version.Major != 0xFF)  break;
818 
                }while (!CheckDelay(timeout));
 816 
                }while (!CheckDelay(timeout));
819 
                UART1_PutString(".");
 817 
                UART1_PutString(".");
820 
                repeat++;
 818 
                repeat++;
821 
//              FCCalibActive = 1;
 819 
                FCCalibActive = 1;
822 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
 820 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
823 
        // if we got it
 821 
        // if we got it
824 
        if (FC_Version.Major != 0xFF)
 822 
        if (FC_Version.Major != 0xFF)
825 
        {
 823 
        {
826 
                sprintf(msg, " FC V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
 824 
                sprintf(msg, " FC V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
827 
                UART1_PutString(msg);
 825 
                UART1_PutString(msg);
828 
        }
 826 
        }
829 
        else UART1_PutString("\n\r not found!");
 827 
        else UART1_PutString("\n\r not found!");
830 
}
 828 
}
Line -... Line 829...
- 
 
 829 
 
- 
 
 830 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 831 
// + extended Current measurement -> 200 = 20A    201 = 21A    255 = 75A (20+55)
- 
 
 832 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 833 
u16 BL3_Current(u8 who) // in 0,1A
- 
 
 834 
{
- 
 
 835 
        if(Motor[who].Current <= 200) return((u16) Motor[who].Current);
- 
 
 836 
        else
- 
 
 837 
        {
- 
 
 838 
                if(Motor_Version[who] & MOTOR_STATE_BL30) return(200 + 10 * (u16) (Motor[who].Current - 200));
- 
 
 839 
                else return((u16) Motor[who].Current);
- 
 
 840 
        }