File content as of revision 15:03ed24574806:

#include "mbed.h"
#include "EthernetInterface.h"
#include <mbed.h>
#include "Thread.h"
/* ---------------------------------------------------------------------------------------- */
 
#define PORT 2342
#define DEBUG 1


#if DEBUG

#endif
/*
void makePacket(char *Payload,char *Packet)
{
    char Header1 = 0xBE;
    char Header2 = 0xEF;
    int checksum = sizeof(*Payload);
    
    *Packet[0] = Header1;
    *Packet[1] = Header2;
    *Packet[2] = sizeof(*Payload);
    
    for (int i = 0; i < *Packet[2]; i++)
    {
        checksum ^= *Payload[i];
        *Packet[i+3] = *Payload[i];
    }
    *Packet[sizeof(*Packet)-1] = checksum;
 }
*/

DigitalIn press(USER_BUTTON);

    char id;              //1
    float AP;             //2
    float PE;             //3
    float SemiMajorAxis;  //4
    float SemiMinorAxis;  //5
    float VVI;            //6
    float e;              //7
    float inc;            //8
    float G;              //9
    int TAp;              //10
    int TPe;              //11
    float TrueAnomaly;    //12
    float Density;        //13
    int period;           //14
    float RAlt;           //15
    float Alt;            //16
    float Vsurf;          //17
    float Lat;            //18
    float Lon;            //19
    float LiquidFuelTot;  //20
    float LiquidFuel;     //21
    float OxidizerTot;    //22
    float Oxidizer;       //23
    float EChargeTot;     //24
    float ECharge;        //25
    float MonoPropTot;    //26
    float MonoProp;       //27
    float IntakeAirTot;   //28
    float IntakeAir;      //29
    float SolidFuelTot;   //30
    float SolidFuel;      //31
    float XenonGasTot;    //32
    float XenonGas;       //33
    float LiquidFuelTotS; //34
    float LiquidFuelS;    //35
    float OxidizerTotS;   //36
    float OxidizerS;      //37
    uint32_t MissionTime;   //38
    float deltaTime;      //39
    float VOrbit;         //40
    uint32_t MNTime;        //41
    float MNDeltaV;       //42
    uint16_t Pitch;          //43
    uint16_t Roll;           //44
    uint16_t Heading;        //45
    uint16_t ActionGroups;  //46  status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10 
    char SOINumber;       //47  SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
    char MaxOverHeat;     //48  Max part overheat (% percent)
    float MachNumber;     //49
    float IAS;            //50  Indicated Air Speed
    char CurrentStage;    //51  Current stage number
    char TotalStage;      //52  TotalNumber of stages
    float TargetDist;     //53  Distance to targeted vessel (m)
    float TargetV;        //54  Target vessel relative velocity (m/s)
    char NavballSASMode;  //55  Combined byte for navball target mode and SAS mode
                             // First four bits indicate AutoPilot mode:
                             // 0 SAS is off  //1 = Regular Stability Assist //2 = Prograde
                             // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
                             // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
                             // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
    uint16_t ProgradePitch;  //56 Pitch   Of the Prograde Vector;  int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees)); 
    uint16_t ProgradeHeading;//57 Heading Of the Prograde Vector;  see above for range   (Prograde vector depends on navball mode, eg Surface/Orbit/Target)
    uint16_t ManeuverPitch;  //58 Pitch   Of the Maneuver Vector;  see above for range;  (0 if no Maneuver node)
    uint16_t ManeuverHeading;//59 Heading Of the Maneuver Vector;  see above for range;  (0 if no Maneuver node)
    uint16_t TargetPitch;    //60 Pitch   Of the Target   Vector;  see above for range;  (0 if no Target)
    uint16_t TargetHeading;  //61 Heading Of the Target   Vector;  see above for range;  (0 if no Target)
    uint16_t NormalHeading;  //62 Heading Of the Prograde Vector;  see above for range;  (Pitch of the Heading Vector is always 0)
    char vesselSync;       //63 Starting with 1, increased on every VesselChange

void float2Bytes(char bytes_temp[4],float float_variable){ 
  union {
    float a;
    unsigned char bytes[4];
  } thing;
  thing.a = float_variable;
  memcpy(bytes_temp, thing.bytes, 4);
}

void unpackVesselData(char *PayloadBuffer, int payloadLength)
{
    char varBuffer[8];
    id = PayloadBuffer[0];              //1
    if(id == 0x01)
    {   
        //float AP - 1    
        for(int i=0; i < 4; i++)
        {
            varBuffer[i]=PayloadBuffer[i+1];
        }
        /**
        varBuffer[1]=PayloadBuffer[2];
        varBuffer[2]=PayloadBuffer[3];
        varBuffer[3]=PayloadBuffer[4];
        **/ 
        AP = *(float *)&varBuffer;
        
        // float PE - 5
        for(int i=0; i < 4; i++)
        {
            varBuffer[i]=PayloadBuffer[i+5];
        }    
        PE = *(float *)&varBuffer;
        
        //float SemiMajorAxis - 9
        for(int i=0; i < 4; i++)
        {
            varBuffer[i]=PayloadBuffer[i+9];
        }  
        SemiMajorAxis = *(float *)&varBuffer;
        
        //float SemiMinorAxis - 13
        for(int i=0; i < 4; i++)
        {
            varBuffer[i]=PayloadBuffer[i+13];
        }  
        SemiMinorAxis = *(float *)&varBuffer;  //5
        
        //float VVI 17
        for(int i=0; i < 4; i++)
        {
            varBuffer[i]=PayloadBuffer[i+17];
        }  
        VVI = *(float *)&varBuffer;            //6
        
        //float e - 21
        for(int i=0; i < 4; i++)
        {
            varBuffer[i]=PayloadBuffer[i+21];
        }  
        e = *(float *)&varBuffer;              //7
        
        //float inc - 25
        for(int i=0; i < 4; i++)
        {
            varBuffer[i]=PayloadBuffer[i+25];
        }  
        inc = *(float *)&varBuffer;            //8
    }
    else
    {
        printf("Bad Packet Id: %d", id);
    }
}




int main() {
   
    EthernetInterface eth;
    eth.init(); //Use DHCP
    eth.connect();
    printf("\nServer IP Address is %s\n\r", eth.getIPAddress());
    
    UDPSocket server;
    server.bind(PORT);
    
    Endpoint KSPServer;
    char buffer[256];
    
        printf("Waiting for UDP packet...\n\r");
        int n = server.receiveFrom(KSPServer, buffer, sizeof(buffer));
        buffer[n] = '\0';
        #if DEBUG
        printf("Received packet from: %s\n\r", KSPServer.get_address());
        //printf("Packet contents : '%s'\n\r",buffer);
        #endif
       // server.sendTo(client, buffer, n)
   
      
    /**
    
    HSPacket.id=0;
    HSPacket.M1=3;
    HSPacket.M2=1;
    HSPacket.status=4;
    **/
    char HSPayload[4]={0x00, 0x03, 0x01, 0x04};
    char HSPacket[sizeof(HSPayload)+4];
    char Header1 = 0xBE;
    char Header2 = 0xEF;
    int checksum = sizeof(HSPayload);
    
    HSPacket[0] = Header1;
    HSPacket[1] = Header2;
    HSPacket[2] = sizeof(HSPayload);
    
    int CurrentPacketLength=0;
    int CurrentBytesRead=0;
    char PayloadBuffer[300];
    char NewPacketBuffer[300];

    for (int i = 0; i < sizeof(HSPayload); i++)
    {
        checksum ^= HSPayload[i];
        HSPacket[i+3] = HSPayload[i];
    }
    HSPacket[sizeof(HSPacket)-1] = checksum;
    
    
    for(int i=0;i<sizeof(HSPacket);i++)
    {
        printf("%c", HSPacket[i]);
    }
    
    //makePacket(HSPayload, HSPacket);
    
    TCPSocketConnection sock;
    
    while (sock.connect(KSPServer.get_address(), PORT) < 0) {
        printf("Unable to connect to KSPServer on Port %d \r\n", PORT);
        wait(1);
    }
    
    printf("Connected to KSP Server \n\r");
    sock.send(HSPacket, sizeof(HSPacket));
    
    while (sock.is_connected()) {
        char buffer[300];
        int ret;
        
        ret = sock.receive(buffer, sizeof(buffer)-1);
        buffer[ret] = '\0';
        if(ret > 0)
        {
            int CurrentState = 0;
            
            for (int x = 0; x < sizeof(buffer)-1; x++)
            {
                switch (CurrentState)
                {
                    case 0: //HEADER 1
                        if (buffer[x] == 0xBE) CurrentState = 1;
                        break;
                    case 1:
                        if (buffer[x] == 0xEF) CurrentState = 2;
                        else CurrentState = 0;
                        break;
                    case 2: //HEADER 2
                        CurrentPacketLength = (int)buffer[x];
                        checksum = CurrentPacketLength;
                        CurrentBytesRead = 0;
                        CurrentState = 3;
                        break;
                    case 3: //PAYLOAD SIZE
                        PayloadBuffer[CurrentBytesRead] = buffer[x];
                        CurrentBytesRead++;
                        if (CurrentBytesRead == CurrentPacketLength)
                        {
                            CurrentState = 4;
                        }
                        break;
                    case 4://CHECKSUM
                        for (int i = 0; i < CurrentPacketLength; i++)
                        {
                            checksum ^= PayloadBuffer[i];
                        }
                        if(buffer[x] == checksum)
                        {
                            #if DEBUG
                            printf("Checksum Success. Payload recieved. \r\n");
                            #endif
                        }
                        else printf("Checksum Failed! Sorry. \r\n");
                        CurrentState = 0;
                        break;
                }
            }
            #if DEBUG
            printf("Payload ID: %d \r\n", PayloadBuffer[0]);
            #endif
            if(PayloadBuffer[0] == 0)
            {
                
            }
            if(PayloadBuffer[0] == 1)
            {
                unpackVesselData(PayloadBuffer, CurrentPacketLength);
                #if OUTPUTENABLE
                printf("AP %f \n\r", AP);
                #endif
                printf("00000%s",PayloadBuffer);
                printf("CCCCC%c",checksum);
                //sock.send(HSPacket, sizeof(HSPacket));
            }
            
            else
            {
                
            }
            
                                         //55  Combined byte for navball target mode and SAS mode
                                         // First four bits indicate AutoPilot mode:
                                         // 0 SAS is off  //1 = Regular Stability Assist //2 = Prograde
                                         // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
                                         // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
                                         // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
            char MainControls = 0b11111111;                  //SAS RCS Lights Gear Brakes Precision Abort Stage 
            char Mode = 0b00000000;                          //0 = stage, 1 = docking, 2 = map (Bit 0-3) 
                                                       //0 = Auto, 1 = Free, 2 = Orbital, 3 = Chase, 4 = Locked (Bit 4-7)
            uint16_t ControlGroup = 0b00000000;                //control groups 1-10 in 2 bytes
             char NavballSASMode = press;                //AutoPilot mode (See above for AutoPilot modes)(Ignored if the equal to zero or out of bounds (>10)) //Navball mode
             short Throttle = 500; 
            if(press)
            {
                NavballSASMode = 0b01000000;
                Throttle = 0;
                printf("PRESS\n\r\n");
            }
            else
            {
                NavballSASMode = 0b00000000;
            }
             
             
             char AdditionalControlByte1 = 0;        //Bit 0: Open Menu, Bit 1: Open Map
             short Pitch = 0;                        //-1000 -> 1000
             short Roll = 0;                         //-1000 -> 1000
             short Yaw = 0;                          //-1000 -> 1000
             short TX = 0;                           //-1000 -> 1000
             short TY = 0;                           //-1000 -> 1000
             short TZ = 0;                           //-1000 -> 1000
             short WheelSteer = 0;                   //-1000 -> 1000
             //short Throttle = 500;                     // 0 -> 1000
             short WheelThrottle = 0;                // 0 -> 1000
             char vesselSync = PayloadBuffer[sizeof(PayloadBuffer)-1];
             char vSync = vesselSync + 30;
            printf("Vsync %c \n\r\n", vSync);
            char CSPayload[26];
            char CSPacket[sizeof(CSPayload)+4];
            Header1 = 0xBE;
            Header2 = 0xEF;
            checksum = sizeof(CSPayload);
            
            CSPacket[0] = Header1;
            CSPacket[1] = Header2;
            CSPacket[2] = sizeof(CSPayload);
            
            CSPayload[0] = 101; //Csid 101
            CSPayload[1] = MainControls;
            CSPayload[2] = Mode;
            CSPayload[3] = ControlGroup & 0xff;
            CSPayload[4] = (ControlGroup >> 8);
            CSPayload[5] = NavballSASMode;
            CSPayload[6] = AdditionalControlByte1;
            CSPayload[7] = Pitch & 0xff;
            CSPayload[8] = (Pitch >> 8);
            CSPayload[9] = Roll & 0xff;
            CSPayload[10] = (Roll >> 8);
            CSPayload[11] = Yaw & 0xff;
            CSPayload[12] = (Yaw >> 8);
            CSPayload[13] = TX & 0xff;
            CSPayload[14] = (TX >> 8);
            CSPayload[15] = TY & 0xff;
            CSPayload[16] = (TY >> 8);
            CSPayload[17] = TZ & 0xff;
            CSPayload[18] = (TZ >> 8);
            CSPayload[19] = WheelSteer & 0xff;
            CSPayload[20] = (WheelSteer >> 8);
            CSPayload[21] = Throttle & 0xff;
            CSPayload[22] = (Throttle >> 8);
            CSPayload[23] = WheelThrottle & 0xff;
            CSPayload[24] = (WheelThrottle >> 8);
            CSPayload[25] = vesselSync;
            
            for (int i = 0; i < sizeof(CSPayload); i++)
            {
                checksum ^= CSPayload[i];
                CSPacket[i+3] = CSPayload[i];
            }
            CSPacket[sizeof(CSPacket)-1] = checksum;     
            sock.send_all(CSPacket, sizeof(CSPacket));
            
            
        }
//        sock.send_all(HSPacket, sizeof(HSPacket));
    }
    
      printf("We got shut down! Socket Closed!");
    sock.close();
    
    eth.disconnect();
    
    while(1) {}
}