File content as of revision 16:0eda62f0d9e8:

#include "mbed.h"
#include "EthernetInterface.h"
#include <mbed.h>
#include "Thread.h"

/* ---------------------------------------------------------------------------------------- */
 
#define PORT 2342
#define DEBUG 0

#define OUTPUTENABLE 0

#if DEBUG

#endif

//pins for LEDs
#define GLED 5
#define YLED 6
#define RLED 7
#define SASLED 11
#define RCSLED 12
#define CG1LED 13

//pins for input
#define SASPIN 8
#define RCSPIN 9
#define CG1PIN 10
#define THROTTLEPIN 0

#define THROTTLEDB 4 //Throttle axis deadband

//Input enums
#define SAS 7
#define RCS 6
#define LIGHTS 5
#define GEAR 4
#define BRAKES 3
#define PRECISION 2
#define ABORT 1
#define STAGE 0

//Action group statuses
#define AGSAS      0
#define AGRCS      1
#define AGLight    2
#define AGGear     3
#define AGBrakes   4
#define AGAbort    5
#define AGCustom01 6
#define AGCustom02 7
#define AGCustom03 8
#define AGCustom04 9
#define AGCustom05 10
#define AGCustom06 11
#define AGCustom07 12
#define AGCustom08 13
#define AGCustom09 14
#define AGCustom10 15

//SAS Modes
#define SMOFF           0
#define SMSAS           1
#define SMPrograde      2
#define SMRetroGrade    3
#define SMNormal        4
#define SMAntinormal    5
#define SMRadialIn      6
#define SMRadialOut     7
#define SMTarget        8
#define SMAntiTarget    9
#define SMManeuverNode  10

//Navball Target Modes
#define NAVBallIGNORE   0
#define NAVBallORBIT    1
#define NAVBallSURFACE  2
#define NAVBallTARGET   3

//macro
#define details(name) (uint8_t*)&name,sizeof(name)

//if no message received from KSP for more than 2s, go idle
#define IDLETIMER 8000
#define CONTROLREFRESH 25

//warnings
#define GWARN 9                  //9G Warning
#define GCAUTION 5               //5G Caution
#define FUELCAUTION 10.0         //10% Fuel Caution
#define FUELWARN 5.0             //5% Fuel warning

void Indicators();
void initLEDS();
void LEDSAllOff();
void InitTxPackets();
int input();
char ControlStatus(char n);
void output();
void controls();
void controlsInit();
char getSASMode();
char getNavballMode();
void setSASMode(char m);
void setNavballMode(char m);
void MainControls(char n, bool s);
void ControlGroups(char n, bool s);
bool KSPBoardReceiveData();
void KSPBoardSendData(uint8_t * data, uint8_t len);
void Handshake();

DigitalIn ControlButton(USER_BUTTON);

int deadtime, deadtimeOld, controlTime, controlTimeOld;
int now;
Timer t;
bool Connected = false;
char VesselID = 0;
char caution = 0, warning = 0, id;

uint8_t rx_len;
uint16_t * address;
char buffer[256]; //address for temporary storage and parsing buffer
uint8_t structSize;
uint8_t rx_array_inx;  //index for RX parsing buffer
uint8_t calc_CS;       //calculated Chacksum
char payloadBuffer[255];

struct VesselData
{
     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 41
     float TrueAnomaly;    //12
     float Density;        //13
     int period;           //14
     float RAlt;           //15
     float Alt;            //16 61
     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 165
     uint16_t Pitch;          //43
     uint16_t Roll;           //44
     uint16_t Heading;        //45
     uint16_t ActionGroups;  //46   173 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 175 Max part overheat (% percent)
     float MachNumber;     //49
     float IAS;            //50  Indicated Air Speed
     char CurrentStage;    //51  Current stage number
     char TotalStage;      //52  185 TotalNumber of stages
     float TargetDist;     //53  Distance to targeted vessel (m)
     float TargetV;        //54  193 Target vessel relative velocity (m/s)
     char NavballSASMode;  //55 194 Combined char 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 208 Starting with 1, increased on every VesselChange
};


struct HandShakePacket
{
  char id;
  char M1;
  char M2;
  char M3;
};

struct ControlPacket {
  char id;
  char MainControls;                  //SAS RCS Lights Gear Brakes Precision Abort Stage
  char Mode;                          //0 = stage, 1 = docking, 2 = map
  short ControlGroup;          //control groups 1-10 in 2 chars
  char NavballSASMode;                //AutoPilot mode
  char AdditionalControlByte1; //9
  int32_t Pitch;                          //-1000 -> 1000
  int32_t Roll;                           //-1000 -> 1000
  int32_t Yaw;                            //-1000 -> 1000
  int32_t TX;                             //-1000 -> 1000
  int32_t TY;                             //-1000 -> 1000
  int32_t TZ;                             //-1000 -> 1000
  int32_t WheelSteer;                     //-1000 -> 1000
  int32_t Throttle;                       //    0 -> 1000
  int32_t WheelThrottle;                  //    0 -> 1000
  char vesselSync;                    //Starting with 1, increased on every VesselChange
};

HandShakePacket HPacket;
VesselData VData;
ControlPacket CPacket;
EthernetInterface eth;
TCPSocketConnection sock;
 

int main() {
    //SETUP
    
    t.start();
    printf("\n\r KSP mbedIO V0.1 \n\r");
    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());
    #endif

    sock.set_blocking(false, 500); // Set socket to non-blocking and set the timeout for 500ms
    
    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 KSPServer on Port %d \r\n", PORT);
    Handshake();
    printf("Handshake sent \r\n");
    
    
    initLEDS();
    InitTxPackets();
   // controlsInit();

    LEDSAllOff();
    while(true)
    {
        input();
        output();
    }
}



//////////////////////////////////////////////////
/////////////////// CONTROLS /////////////////////
//////////////////////////////////////////////////

void Indicators() {
    caution = 0;
    warning = 0;
    
    caution += VData.G > GCAUTION;
    warning += VData.G > GWARN;
    caution += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELCAUTION;
    warning += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELWARN;
    
    if (caution != 0)
    {
        printf("Yellow! \n\r");
    }
    else
    {
        //yel off
    }
    
    if (warning != 0)
    {
        printf("RED! \n\r");
    }
    else
    {
        //red off
    }
    #if OUTPUTENABLE
    printf("AP : %f \n\r", VData.AP);
    printf("Sync : %d \n\r", VData.vesselSync);
    printf("Alt : %f \n\r", VData.Alt);
    printf("Vsurf : %f \n\r", VData.Vsurf);
    if(ControlStatus(AGSAS)) printf("AGSAS ON \n\r");
    if(ControlStatus(AGRCS)) printf("AGRCS ON \n\r");
    if(ControlStatus(AGCustom01)) printf("AGCustom1 ON \n\r");
    #endif
}

void initLEDS() {

}

void LEDSAllOff() {

}

void InitTxPackets() {
  HPacket.id = 0;  
  CPacket.id = 101;
}


//////////////////////////////////////////////////
//////////////////// INPUT   /////////////////////
//////////////////////////////////////////////////

int input() {
  int returnValue = -1;
  now = t.read_ms();

  if (KSPBoardReceiveData())
  {
    deadtimeOld = now;
    returnValue = id;
    switch(id) 
    {
        case 0: //Handshake packet
            Handshake();
            #if DEBUG
            printf("HS Packet recieved\n\r");
            #endif
            break;
        case 1:
            VesselID = VData.vesselSync;
            Indicators();
            #if DEBUG
            printf("VD Packet recieved\n\r");
            #endif
            break;
    }
    Connected = true;
  }
  
  else
  { //if no message received for a while, go idle
    deadtime = now - deadtimeOld; 
    if (deadtime > IDLETIMER)
    {
      deadtimeOld = now;
      Connected = false;
      LEDSAllOff();
      printf("No packets for a while... going Idle \n\r");
    }    
  }

  return returnValue;
}

char ControlStatus(char n)
{
  return ((VData.ActionGroups >> n) & 1) == 1;
}

//////////////////////////////////////////////////
///////////////////// OUTPUT /////////////////////
//////////////////////////////////////////////////

void output() {
  now = t.read_ms();
  controlTime = now - controlTimeOld;
  if (controlTime > CONTROLREFRESH) {
    controlTimeOld = now;
    controls();
  }
}

void controls() {
  if (Connected) {

    if (!ControlButton) { //--------- This is how you do main controls
        MainControls(SAS, 1);
        setSASMode(SMSAS); //setting SAS mode
        #if DEBUG
        printf("SAS PRESS \n\r");
        #endif
      //setNavballMode(NAVBallSURFACE); //setting navball mode
    }
    else {
      //setNavballMode(NAVBallTARGET);
      MainControls(SAS, 0);
    }

    if (ControlButton)
      MainControls(RCS, 1);
    else
      MainControls(RCS, 0);

    /**
    if (digitalRead(CG1PIN))   //--------- This is how you do control groups
      ControlGroups(1, 1);
    else
      ControlGroups(1, 0);
      **/
    /*
       if (getSASMode() == SMPrograde) { //--------- This is how you read SAS modes
         //Blink LED, do stuff, etc.
       }

       if (getNavballMode() == NAVBallTARGET) { //--------- This is how you read navball modes
         //Blink LED, do stuff, etc.
       }
    */

    //This is an example of reading analog inputs to an axis, with deadband and limits
    //CPacket.Throttle = constrain(map(analogRead(THROTTLEPIN), THROTTLEDB, 1024 - THROTTLEDB, 0, 1000), 0, 1000);

    //This is an example of reading analog inputs to an axis, with deadband and limits
    //CPacket.Pitch = constrain(map(analogRead(THROTTLEPIN),0,1024,-1000,1000),-1000, 1000);
    CPacket.vesselSync = VData.vesselSync;
    KSPBoardSendData(details(CPacket));
  }
}

void controlsInit() {
  /**
  pinMode(SASPIN, INPUT_PULLUP);
  pinMode(RCSPIN, INPUT_PULLUP);
  pinMode(CG1PIN, INPUT_PULLUP);
  **/
}

char getSASMode() {
  return VData.NavballSASMode & 0b00001111; // leaves alone the lower 4 bits of; all higher bits set to 0.
}

char getNavballMode() {
  return VData.NavballSASMode >> 4; // leaves alone the higher 4 bits of; all lower bits set to 0.
}

void setSASMode(char m) {
  CPacket.NavballSASMode &= 0b11110000;
  CPacket.NavballSASMode += m;
}

void setNavballMode(char m) {
  CPacket.NavballSASMode &= 0b00001111;
  CPacket.NavballSASMode += m << 4;
}

void MainControls(char n, bool s) {
  if (s)
    CPacket.MainControls |= (1 << n);       // forces nth bit of x to be 1.  all other bits left alone.
  else
    CPacket.MainControls &= ~(1 << n);      // forces nth bit of x to be 0.  all other bits left alone.
}

void ControlGroups(char n, bool s) {
  if (s)
    CPacket.ControlGroup |= (1 << n);       // forces nth bit of x to be 1.  all other bits left alone.
  else
    CPacket.ControlGroup &= ~(1 << n);      // forces nth bit of x to be 0.  all other bits left alone.
}




//////////////////////////////////////////////////
//////////////// COMMUNICATIONS///////////////////
//////////////////////////////////////////////////



bool KSPBoardReceiveData() {
    int ret;
        
    ret = sock.receive(buffer, sizeof(buffer)-1);
    
    if(ret > 0)
    {
      int packetCheckState = 0;
        #if DEBUG
        printf("Packet Rx'd \n\r");
        #endif
        for(int x = 0; x < 4; x++)
        {
          switch(packetCheckState){
            case 0: // CHECK HEADER 1
                if(buffer[packetCheckState] == 0xBE)
                {
                    packetCheckState++;
                }
                break;
            case 1: // CHECK HEADER 2
                if(buffer[packetCheckState] == 0xEF)
                {
                    packetCheckState++;
                }
                break;
            case 2: // GET PAYLOAD SIZE
                rx_len = buffer[packetCheckState];
                packetCheckState++;
                #if DEBUG
                printf("PL Size at Rx: %d \n\r", rx_len);
                #endif
                break;
            case 3:
                id = buffer[packetCheckState];
                #if DEBUG
                printf("Packet ID: %d \n\r", id);
                #endif
                if(id == 0)
                {
                    structSize = sizeof(HPacket);   
                    address = (uint16_t*)&HPacket;
                }
                else if(id == 1)
                {
                    structSize = sizeof(VData);   
                    address = (uint16_t*)&VData;     
                }
                else
                {
                    printf("Bad Packet ID: %d", buffer[packetCheckState]);
                    packetCheckState = 0;
                }
                break;
            default :
                printf("Something went wrong in Packet Rx Loop \r\n");
                break;
            }
        }
        
        if(packetCheckState == 3) //We had a valid packet
        {
            calc_CS = rx_len;
            for(int i = 0; i < rx_len; i++)
            {
                payloadBuffer[i] = buffer[i+3];
                calc_CS^=buffer[i+3];
            }
            
            if(calc_CS == buffer[rx_len+3])   //CS Check
            {
                char varBuffer[4];
                #if DEBUG
                printf("Checksum Passed! \n\r");
                #endif
                
                // A note to the reader, I hate this as much as you do. I really do. 
                // But I'm a shitty programmer and this compiler pads structs for alignment and it fucks up the memcpy approach. 
                // I fought this, I really did. But it's 2AM, and I just want my fucking code to work. So forgive me this sin.
                
                //memcpy(address,payloadBuffer,structSize);
                
                VData.id = payloadBuffer[0];              //1
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+1];
                }    
                VData.AP = *(float *)&varBuffer;
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+5];
                }
                VData.PE = *(float *)&varBuffer;             //3
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+9];
                }
                VData.SemiMajorAxis = *(float *)&varBuffer;  //4
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+13];
                }
                VData.SemiMinorAxis = *(float *)&varBuffer;  //5
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+17];
                }
                VData.VVI = *(float *)&varBuffer;            //6
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+21];
                }
                VData.e = *(float *)&varBuffer;              //7
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+25];
                }
                VData.inc = *(float *)&varBuffer;            //8
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+29];
                }
                VData.G = *(float *)&varBuffer;              //9
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+33];
                }
                VData.TAp = *(int *)&varBuffer;              //10
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+37];
                }
                VData.TPe = *(int *)&varBuffer;              //11 41
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+41];
                }
                VData.TrueAnomaly = *(float *)&varBuffer;    //12
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+44];
                }
                VData.Density = *(float *)&varBuffer;        //13
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+49];
                }
                VData.period = *(int *)&varBuffer;           //14
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+53];
                }
                VData.RAlt = *(float *)&varBuffer;           //15
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+57];
                }
                VData.Alt = *(float *)&varBuffer;            //16 61
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+61];
                }
                VData.Vsurf = *(float *)&varBuffer;          //17
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+65];
                }
                VData.Lat = *(float *)&varBuffer;            //18
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+69];
                }
                VData.Lon = *(float *)&varBuffer;            //19
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+73];
                }
                VData.LiquidFuelTot = *(float *)&varBuffer;  //20
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+77];
                }
                VData.LiquidFuel = *(float *)&varBuffer;     //21
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+81];
                }
                VData.OxidizerTot = *(float *)&varBuffer;    //22
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+85];
                }
                VData.Oxidizer = *(float *)&varBuffer;       //23
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+89];
                }
                VData.EChargeTot = *(float *)&varBuffer;     //24
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+93];
                }
                VData.ECharge = *(float *)&varBuffer;        //25
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+97];
                }
                VData.MonoPropTot = *(float *)&varBuffer;    //26
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+101];
                }
                VData.MonoProp = *(float *)&varBuffer;       //27
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+104];
                }
                VData.IntakeAirTot = *(float *)&varBuffer;   //28
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+109];
                }
                VData.IntakeAir = *(float *)&varBuffer;      //29
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+113];
                }
                VData.SolidFuelTot = *(float *)&varBuffer;   //30
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+117];
                }
                VData.SolidFuel = *(float *)&varBuffer;      //31
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+121];
                }
                VData.XenonGasTot = *(float *)&varBuffer;    //32
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+125];
                }
                VData.XenonGas = *(float *)&varBuffer;       //33
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+129];
                }
                VData.LiquidFuelTotS = *(float *)&varBuffer; //34
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+133];
                }
                VData.LiquidFuelS = *(float *)&varBuffer;    //35
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+137];
                }
                VData.OxidizerTotS = *(float *)&varBuffer;   //36
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+141];
                }
                VData.OxidizerS = *(float *)&varBuffer;      //37
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+145];
                }
                VData.MissionTime = *(uint32_t *)&varBuffer;   //38
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+149];
                }
                VData.deltaTime = *(float *)&varBuffer;      //39
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+153];
                }
                VData.VOrbit = *(float *)&varBuffer;         //40
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+157];
                }
                VData.MNTime = *(uint32_t *)&varBuffer;        //41
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+161];
                }
                VData.MNDeltaV = *(float *)&varBuffer;       //42 165
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+165];
                }
                VData.Pitch = *(float *)&varBuffer;          //43//////////////////////////
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+167];
                }
                VData.Roll = *(float *)&varBuffer;           //44
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+169];
                }
                VData.Heading = *(float *)&varBuffer;        //45
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+173];
                }
                VData.ActionGroups = *(float *)&varBuffer;  //46   173 status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10 
                VData.SOINumber = payloadBuffer[177];       //47  SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
                VData.MaxOverHeat = payloadBuffer[178];    //48 175 Max part overheat (% percent)
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+179];
                }
                VData.MachNumber = *(float *)&varBuffer;     //49
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+183];
                }
                VData.IAS = *(float *)&varBuffer;            //50  Indicated Air Speed
                
                VData.CurrentStage = payloadBuffer[187];    //51  Current stage number
                VData.TotalStage = payloadBuffer[188];      //52  185 TotalNumber of stages
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+189];
                }
                VData.TargetDist = *(float *)&varBuffer;     //53  Distance to targeted vessel (m)
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+193];
                }
                VData.TargetV = *(float *)&varBuffer;        //54  193 Target vessel relative velocity (m/s)
                VData.NavballSASMode = payloadBuffer[197];  //55 194 Combined char 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)
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+198];
                }
                VData.ProgradePitch = *(float *)&varBuffer;  //56 Pitch   Of the Prograde Vector;  int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees)); 
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+200];
                }
                VData.ProgradeHeading = *(float *)&varBuffer;//57 Heading Of the Prograde Vector;  see above for range   (Prograde vector depends on navball mode, eg Surface/Orbit/Target)
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+202];
                }
                VData.ManeuverPitch = *(float *)&varBuffer;  //58 Pitch   Of the Maneuver Vector;  see above for range;  (0 if no Maneuver node)
                for(int i=0; i < 4; i++)
                {
                    varBuffer[i]=payloadBuffer[i+204];
                }
                VData.ManeuverHeading = *(float *)&varBuffer;//59 Heading Of the Maneuver Vector;  see above for range;  (0 if no Maneuver node)
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+205];
                }
                VData.TargetPitch = *(float *)&varBuffer;    //60 Pitch   Of the Target   Vector;  see above for range;  (0 if no Target)
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+206];
                }
                VData.TargetHeading = *(float *)&varBuffer;  //61 Heading Of the Target   Vector;  see above for range;  (0 if no Target)
                for(int i=0; i < 2; i++)
                {
                    varBuffer[i]=payloadBuffer[i+207];
                }
                VData.NormalHeading = *(float *)&varBuffer;  //62 Heading Of the Prograde Vector;  see above for range;  (Pitch of the Heading Vector is always 0)
                VData.vesselSync = payloadBuffer[208];    

                rx_len = 0;
                return true;
            }
            else
            {
                //failed checksum, need to clear this out anyway
                printf("Failed Checksum! Sorry \n\r");
                rx_len = 0;
                rx_array_inx = 1;
                return false;
            }
                

        }
    }
    
    return false;
}

void KSPBoardSendData(uint8_t * data, uint8_t len){
    uint8_t CS = len;
    char fullPacket[len+4];
    fullPacket[0] = 0xBE;
    fullPacket[1] = 0xEF;
    fullPacket[2] = len;
    
    if(*(data) == 101)
    {
        fullPacket[3] = 101; //Csid 101
        fullPacket[4] = CPacket.MainControls;
        fullPacket[5] = CPacket.Mode;
        fullPacket[6] = CPacket.ControlGroup & 0xff;
        fullPacket[7] = (CPacket.ControlGroup >> 8);
        fullPacket[8] = CPacket.NavballSASMode;
        fullPacket[9] = CPacket.AdditionalControlByte1;
        fullPacket[10] = CPacket.Pitch & 0xff;
        fullPacket[11] = (CPacket.Pitch >> 8);
        fullPacket[12] = CPacket.Roll & 0xff;
        fullPacket[13] = (CPacket.Roll >> 8);
        fullPacket[14] = CPacket.Yaw & 0xff;
        fullPacket[15] = (CPacket.Yaw >> 8);
        fullPacket[16] = CPacket.TX & 0xff;
        fullPacket[17] = (CPacket.TX >> 8);
        fullPacket[18] = CPacket.TY & 0xff;
        fullPacket[19] = (CPacket.TY >> 8);
        fullPacket[20] = CPacket.TZ & 0xff;
        fullPacket[21] = (CPacket.TZ >> 8);
        fullPacket[22] = CPacket.WheelSteer & 0xff;
        fullPacket[23] = (CPacket.WheelSteer >> 8);
        fullPacket[24] = CPacket.Throttle & 0xff;
        fullPacket[25] = (CPacket.Throttle >> 8);
        fullPacket[26] = CPacket.WheelThrottle & 0xff;
        fullPacket[27] = (CPacket.WheelThrottle >> 8);
        fullPacket[28] = CPacket.vesselSync;
        for(int i = 0; i<len; i++)
        {
            CS^= fullPacket[i+3];
        }
        fullPacket[sizeof(fullPacket)-1] = CS;
    }
    else
    {
        for(int i = 0; i<len; i++)
        {
            CS^=*(data+i);
            fullPacket[i+3] = *(data+i);
        }
        fullPacket[sizeof(fullPacket)-1] = CS;
    }
    #if DEBUG
    printf("Sending a Packet \n\r");
    #endif
    sock.send(fullPacket, sizeof(fullPacket)); // Send the packet 
}

void Handshake(){

    HPacket.id = 0;
    HPacket.M1 = 3;
    HPacket.M2 = 1;
    HPacket.M3 = 4;
    
    KSPBoardSendData(details(HPacket));
}