Subversion Repositories NaviCtrl

Rev

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

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