Subversion Repositories NaviCtrl

#include "crc16.h"

#define ABO_TIMEOUT 8000 // disable abo after 8 seconds

u32 UART1_AboTimeOut = 0;

NaviData_Volatile_t NaviData_Volatile;

NaviData_WP_t NaviData_WP;

NaviData_Deviation_t NaviData_Deviation;

NaviData_Home_t NaviData_Home;

NaviData_Target_t NaviData_Target;

u32 UART1_NaviData_Interval = 0;        // in ms

u32 NMEA_Interval = 0;// in ms

u8 CalculateDebugLableCrc(void)

{

        u16 i;

        u8 crc = 0;

        for(i=0;i<sizeof(ANALOG_LABEL);i++) crc += ANALOG_LABEL[0][i];

        return(crc);

                                        UART1_Request_Display = TRUE;

                                        if(UART1_Display_Interval) UART1_Request_Display = TRUE;

                        case 'o': // request for navigation information

                                UART1_NaviData_Interval = (u32) SerialMsg.pData[0] * 10;

}

u16 TransmitNavigationData(u16 MaxBytesPerSecond) // returns the minumum pause time in ms

{

static u8 state = 0, count_flags = 2, count_target = 3, count_home = 4, count_wp = 5 , count_tiny = 6;

static u16 CRC_Home = 0, CRC_Target = 0, CRC_Flags = 0, CRC_Wp = 0;

u16 pause, sent = 0, crc_home = 0xff, crc_target = 0xff, crc_flags = 0xff, crc_wp = 0xff;

while(!sent)

 {

                switch(state++)

                {

                 case 0:

                 case 4:

// belegt 35 ASCII-Zeichen

                                NaviData_Flags.Index = 11;

                                NaviData_Flags.ActualLongitude = NaviData.CurrentPosition.Longitude;

                                NaviData_Flags.ActualLatitude = NaviData.CurrentPosition.Latitude;

                                NaviData_Flags.Altimeter = NaviData.Altimeter;

                                NaviData_Flags.GroundSpeed = NaviData.GroundSpeed / 10;

                                NaviData_Flags.OSDStatusFlags = (FC.StatusFlags & OSD_FLAG_MASK1) | (FC.StatusFlags2 & OSD_FLAG_MASK2);

                                NaviData_Flags.OSDStatusFlags2 = (FC.StatusFlags & ~OSD_FLAG_MASK1) | (FC.StatusFlags2 & ~OSD_FLAG_MASK2);

                                NaviData_Flags.NCFlags = NaviData.NCFlags;

                                NaviData_Flags.Errorcode = ErrorCode;

                                NaviData_Flags.ReserveFlags = 0;

                                NaviData_Flags.SpeakHoTT = FC.FromFC_SpeakHoTT;

                                NaviData_Flags.VarioCharacter = FromFC_VarioCharacter;

                                NaviData_Flags.GPS_ModeCharacter = NC_GPS_ModeCharacter;

                                NaviData_Flags.BL_MinOfMaxPWM = BL_MinOfMaxPWM;

                                crc_flags = CRC16((unsigned char*)(&NaviData_Flags.OSDStatusFlags2), sizeof(NaviData_Flags) - START_PAYLOAD_DATA); // update crc for the license structure

                                if((crc_flags != CRC_Flags) || (--count_flags == 0))

                                {

                                 sent += MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData_Flags, sizeof(NaviData_Flags)) + 1;

                                 CRC_Flags = crc_flags;

                                 count_flags = 11*2;

                                }

                                break;

                 case 1:

                 case 5:

// belegt 43 ASCII-Zeichen

                                NaviData_Target.Index = 12;

                                NaviData_Target.ActualLongitude = NaviData.CurrentPosition.Longitude;

                                NaviData_Target.ActualLatitude = NaviData.CurrentPosition.Latitude;

                                NaviData_Target.Altimeter = NaviData.Altimeter;

                                NaviData_Target.GroundSpeed = NaviData.GroundSpeed / 10;

                                NaviData_Target.OSDStatusFlags = (FC.StatusFlags & OSD_FLAG_MASK1) | (FC.StatusFlags2 & OSD_FLAG_MASK2);

                                NaviData_Target.TargetLongitude = NaviData.TargetPosition.Longitude;

                                NaviData_Target.TargetLatitude = NaviData.TargetPosition.Latitude;

                                NaviData_Target.TargetAltitude = NaviData.TargetPosition.Altitude;

                                NaviData_Target.RC_Quality = NaviData.RC_Quality;

                                crc_target = CRC16((unsigned char*)(&NaviData_Target.TargetLongitude), sizeof(NaviData_Target) - START_PAYLOAD_DATA); // update crc for the license structure

                            if((crc_target != CRC_Target) || (--count_target == 0))

                                {

                                 sent += MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData_Target, sizeof(NaviData_Target)) + 1;

                                 CRC_Target = crc_target;

                                 count_target = 10*2;

                                }

                                break;

                 case 2:

                 case 6:

// belegt 31 ASCII-Zeichen

                                NaviData_WP.Index = 15;

                                NaviData_WP.ActualLongitude = NaviData.CurrentPosition.Longitude;

                                NaviData_WP.ActualLatitude = NaviData.CurrentPosition.Latitude;

                                NaviData_WP.Altimeter = NaviData.Altimeter;

                                NaviData_WP.GroundSpeed = NaviData.GroundSpeed / 10;

                                NaviData_WP.OSDStatusFlags = (FC.StatusFlags & OSD_FLAG_MASK1) | (FC.StatusFlags2 & OSD_FLAG_MASK2);

                                NaviData_WP.WaypointIndex = NaviData.WaypointIndex;

                                NaviData_WP.WaypointNumber = NaviData.WaypointNumber;

                                NaviData_WP.TargetHoldTime = NaviData.TargetHoldTime;

                                // NaviData_WP.WP_Eventchannel = FC_WP_EventChannel; -> happends already in SPI.c

                                crc_wp = CRC16((unsigned char*)(&NaviData_WP.WaypointIndex), sizeof(NaviData_WP) - START_PAYLOAD_DATA); // update crc for the license structure

                                if((crc_wp != CRC_Wp) || (--count_wp == 0))

                                {

                         sent += MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData_WP, sizeof(NaviData_WP)) + 1;

                                 CRC_Wp = crc_wp;

                                 count_wp = 12*2;

                                }

                                break;

                 case 3:

// belegt 43 ASCII-Zeichen

                                NaviData_Home.Index = 13;

                                NaviData_Home.ActualLongitude = NaviData.CurrentPosition.Longitude;

                                NaviData_Home.ActualLatitude = NaviData.CurrentPosition.Latitude;

                                NaviData_Home.Altimeter = NaviData.Altimeter;

                                NaviData_Home.GroundSpeed = NaviData.GroundSpeed / 10;

                                NaviData_Home.OSDStatusFlags = (FC.StatusFlags & OSD_FLAG_MASK1) | (FC.StatusFlags2 & OSD_FLAG_MASK2);

                                NaviData_Home.HomeLongitude = NaviData.HomePosition.Longitude;

                                NaviData_Home.HomeLatitude = NaviData.HomePosition.Latitude;

                                NaviData_Home.HomeAltitude = NaviData.HomePosition.Altitude;

                                crc_home = CRC16((unsigned char*)(&NaviData_Home.HomeLongitude), sizeof(NaviData_Home_t) - START_PAYLOAD_DATA); // update crc for the license structure

                                if((crc_home != CRC_Home) || (--count_home == 0))

                                {

                                 sent += MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData_Home, sizeof(NaviData_Home)) + 1;

                                 CRC_Home = crc_home;

                                 count_home = 25;

                                }

                                break;

                 case 7:

// belegt 39 ASCII-Zeichen

                                NaviData_Deviation.Index = 14;

                                NaviData_Deviation.ActualLongitude = NaviData.CurrentPosition.Longitude;

                                NaviData_Deviation.ActualLatitude = NaviData.CurrentPosition.Latitude;

                                NaviData_Deviation.Altimeter = NaviData.Altimeter;

                                NaviData_Deviation.GroundSpeed = NaviData.GroundSpeed / 10;

                                NaviData_Deviation.OSDStatusFlags = (FC.StatusFlags & OSD_FLAG_MASK1) | (FC.StatusFlags2 & OSD_FLAG_MASK2);

                                NaviData_Deviation.FlyingTime = NaviData.FlyingTime;

                                NaviData_Deviation.DistanceToHome = NaviData.HomePositionDeviation.Distance;

                                NaviData_Deviation.HeadingToHome = NaviData.HomePositionDeviation.Bearing/2;

                                NaviData_Deviation.DistanceToTarget = NaviData.TargetPositionDeviation.Distance;

                                NaviData_Deviation.HeadingToTarget = NaviData.TargetPositionDeviation.Bearing/2;

                                NaviData_Deviation.AngleNick = NaviData.AngleNick;

                                NaviData_Deviation.AngleRoll = NaviData.AngleRoll;

                                NaviData_Deviation.SatsInUse = NaviData.SatsInUse;

                            sent += MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData_Deviation, sizeof(NaviData_Deviation)) + 1;

                                break;

                 case 8:

// belegt 43 ASCII-Zeichen

                                NaviData_Volatile.Index = 16;

                                NaviData_Volatile.ActualLongitude = NaviData.CurrentPosition.Longitude;

                                NaviData_Volatile.ActualLatitude = NaviData.CurrentPosition.Latitude;

                                NaviData_Volatile.Altimeter = NaviData.Altimeter;

                                NaviData_Volatile.GroundSpeed = NaviData.GroundSpeed / 10;

                                NaviData_Volatile.OSDStatusFlags = (FC.StatusFlags & OSD_FLAG_MASK1) | (FC.StatusFlags2 & OSD_FLAG_MASK2);

                                NaviData_Volatile.UBat = FC.BAT_Voltage;

                                NaviData_Volatile.Current = NaviData.Current;

                                NaviData_Volatile.UsedCapacity = NaviData.UsedCapacity;

                                NaviData_Volatile.Variometer = NaviData.Variometer;

                                NaviData_Volatile.Heading = NaviData.Heading / 2;

                                NaviData_Volatile.CompassHeading = NaviData.CompassHeading / 2;

                                NaviData_Volatile.Gas = NaviData.Gas;

                                NaviData_Volatile.Gas = NaviData.SetpointAltitude;

                                sent += MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData_Volatile, sizeof(NaviData_Volatile)) + 1;

                        break;

                 case 9:

// belegt 27 ASCII-Zeichen

                                NaviData_Tiny.Index = 10;

                                NaviData_Tiny.ActualLongitude = NaviData.CurrentPosition.Longitude;

                                NaviData_Tiny.ActualLatitude = NaviData.CurrentPosition.Latitude;

                                NaviData_Tiny.Altimeter = NaviData.Altimeter;

                                NaviData_Tiny.GroundSpeed = NaviData.GroundSpeed / 10;

                                NaviData_Tiny.OSDStatusFlags = (FC.StatusFlags & OSD_FLAG_MASK1) | (FC.StatusFlags2 & OSD_FLAG_MASK2);

                                if(--count_tiny == 0)

                                 {

                                        sent += MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData_Tiny, sizeof(NaviData_Tiny)) + 1;

                                        count_tiny = 60;

                                 }

                        break;

                 default: state = 0;

                                break;

                }

 }

        pause = (sent * 1000) / MaxBytesPerSecond;

        UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval);

        UART1_Request_NaviData = FALSE;

        LastTransmittedFCStatusFlags2 = NaviData.FCStatusFlags2;

return(pause);

// Clear at timeout: 

                UART1_NaviData_Interval = 0;

                UART1_MotorData_Interval = 0;

                UART1_NaviData_Timer = SetDelay(500);

/*

#define CHK_MIN_INTERVAL(a,b) {if(a && a < b) a = b;}

UART1_NaviData_Interval = 500;

CHK_MIN_INTERVAL(UART1_DebugData_Interval,500);

CHK_MIN_INTERVAL(UART1_NaviData_Interval,1000);

CHK_MIN_INTERVAL(UART1_Data3D_Interval,255);

CHK_MIN_INTERVAL(UART1_Display_Interval,1500);

CHK_MIN_INTERVAL(UART1_MotorData_Interval,750);

          u16 time = 0;

      if(UART1_NaviData_MaxBytes == 0) // Transmit the big NC Data frame

           {

                UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval);

           }

           else

           if(CheckDelay(UART1_NaviData_Timer))

           {

// Wert =  50 -> Sekunden laufen in 2 s-Abstand (im Wegpunkteflug mit aktivem Countdown in 3-4s)

// Wert = 100 -> Sekunden laufen in 1-2 s-Abstand (im Wegpunkteflug mit aktivem Countdown in 2s)

// Wert = 150 -> Sekunden laufen flüssig (im Wegpunkteflug mit aktivem Countdown und gleichzeitig Target verschieben manchmal in 2s)

// Wert = 200 -> Sekunden laufen flüssig

// Wert >= 250 -> optimal 

//UART1_NaviData_MaxBytes = 250;

            time = TransmitNavigationData(UART1_NaviData_MaxBytes);

                if(UART1_NaviData_Interval > time) time = UART1_NaviData_Interval;

                UART1_NaviData_Timer = SetDelay(time);

           }

        UART1_Request_NaviData = FALSE;