main.cpp

00001 #include "mbed.h"
00002 #include "EthernetInterface.h"
00003 #include <mbed.h>
00004 #include "Thread.h"
00005 
00006 /* ---------------------------------------------------------------------------------------- */
00007  
00008 #define PORT 2342
00009 #define DEBUG 0
00010 
00011 #define OUTPUTENABLE 1
00012 
00013 #if DEBUG
00014 
00015 #endif
00016 
00017 //pins for LEDs
00018 #define GLED 5
00019 #define YLED 6
00020 #define RLED 7
00021 #define SASLED 11
00022 #define RCSLED 12
00023 #define CG1LED 13
00024 
00025 //pins for input
00026 #define SASPIN 8
00027 #define RCSPIN 9
00028 #define CG1PIN 10
00029 #define THROTTLEPIN 0
00030 
00031 #define THROTTLEDB 4 //Throttle axis deadband
00032 
00033 //Input enums
00034 #define SAS 7
00035 #define RCS 6
00036 #define LIGHTS 5
00037 #define GEAR 4
00038 #define BRAKES 3
00039 #define PRECISION 2
00040 #define ABORT 1
00041 #define STAGE 0
00042 
00043 //Action group statuses
00044 #define AGSAS      0
00045 #define AGRCS      1
00046 #define AGLight    2
00047 #define AGGear     3
00048 #define AGBrakes   4
00049 #define AGAbort    5
00050 #define AGCustom01 6
00051 #define AGCustom02 7
00052 #define AGCustom03 8
00053 #define AGCustom04 9
00054 #define AGCustom05 10
00055 #define AGCustom06 11
00056 #define AGCustom07 12
00057 #define AGCustom08 13
00058 #define AGCustom09 14
00059 #define AGCustom10 15
00060 
00061 //SAS Modes
00062 #define SMOFF           0
00063 #define SMSAS           1
00064 #define SMPrograde      2
00065 #define SMRetroGrade    3
00066 #define SMNormal        4
00067 #define SMAntinormal    5
00068 #define SMRadialIn      6
00069 #define SMRadialOut     7
00070 #define SMTarget        8
00071 #define SMAntiTarget    9
00072 #define SMManeuverNode  10
00073 
00074 //Navball Target Modes
00075 #define NAVBallIGNORE   0
00076 #define NAVBallORBIT    1
00077 #define NAVBallSURFACE  2
00078 #define NAVBallTARGET   3
00079 
00080 //macro
00081 #define details(name) (uint8_t*)&name,sizeof(name)
00082 
00083 //if no message received from KSP for more than 2s, go idle
00084 #define IDLETIMER 8000
00085 #define CONTROLREFRESH 25
00086 
00087 //warnings
00088 #define GWARN 9                  //9G Warning
00089 #define GCAUTION 5               //5G Caution
00090 #define FUELCAUTION 10.0         //10% Fuel Caution
00091 #define FUELWARN 5.0             //5% Fuel warning
00092 
00093 void Indicators();
00094 void initLEDS();
00095 void LEDSAllOff();
00096 void InitTxPackets();
00097 int input();
00098 char ControlStatus(char n);
00099 void output();
00100 void controls();
00101 void controlsInit();
00102 char getSASMode();
00103 char getNavballMode();
00104 void setSASMode(char m);
00105 void setNavballMode(char m);
00106 void MainControls(char n, bool s);
00107 void ControlGroups(char n, bool s);
00108 bool KSPBoardReceiveData();
00109 void KSPBoardSendData(uint8_t * data, uint8_t len);
00110 void Handshake();
00111 
00112 DigitalIn ControlButton(USER_BUTTON);
00113 
00114 int deadtime, deadtimeOld, controlTime, controlTimeOld;
00115 int now;
00116 Timer t;
00117 bool Connected = false;
00118 char VesselID = 0;
00119 char caution = 0, warning = 0, id;
00120 
00121 uint8_t rx_len;
00122 uint16_t * address;
00123 char buffer[256]; //address for temporary storage and parsing buffer
00124 uint8_t structSize;
00125 uint8_t rx_array_inx;  //index for RX parsing buffer
00126 uint8_t calc_CS;       //calculated Chacksum
00127 char payloadBuffer[255];
00128 
00129 #pragma pack (1)
00130 
00131 struct VesselData
00132 {
00133      char id;              //1
00134      float AP;             //2
00135      float PE;             //3
00136      float SemiMajorAxis;  //4
00137      float SemiMinorAxis;  //5
00138      float VVI;            //6
00139      float e;              //7
00140      float inc;            //8
00141      float G;              //9
00142      int TAp;              //10
00143      int TPe;              //11 41
00144      float TrueAnomaly;    //12
00145      float Density;        //13
00146      int period;           //14
00147      float RAlt;           //15
00148      float Alt;            //16 61
00149      float Vsurf;          //17
00150      float Lat;            //18
00151      float Lon;            //19
00152      float LiquidFuelTot;  //20
00153      float LiquidFuel;     //21
00154      float OxidizerTot;    //22
00155      float Oxidizer;       //23
00156      float EChargeTot;     //24
00157      float ECharge;        //25
00158      float MonoPropTot;    //26
00159      float MonoProp;       //27
00160      float IntakeAirTot;   //28
00161      float IntakeAir;      //29
00162      float SolidFuelTot;   //30
00163      float SolidFuel;      //31
00164      float XenonGasTot;    //32
00165      float XenonGas;       //33
00166      float LiquidFuelTotS; //34
00167      float LiquidFuelS;    //35
00168      float OxidizerTotS;   //36
00169      float OxidizerS;      //37
00170      uint32_t MissionTime;   //38
00171      float deltaTime;      //39
00172      float VOrbit;         //40
00173      uint32_t MNTime;        //41
00174      float MNDeltaV;       //42 165
00175      uint16_t Pitch;          //43
00176      uint16_t Roll;           //44
00177      uint16_t Heading;        //45
00178      uint16_t ActionGroups;  //46   173 status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10 
00179      char SOINumber;       //47  SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
00180      char MaxOverHeat;     //48 175 Max part overheat (% percent)
00181      float MachNumber;     //49
00182      float IAS;            //50  Indicated Air Speed
00183      char CurrentStage;    //51  Current stage number
00184      char TotalStage;      //52  185 TotalNumber of stages
00185      float TargetDist;     //53  Distance to targeted vessel (m)
00186      float TargetV;        //54  193 Target vessel relative velocity (m/s)
00187      char NavballSASMode;  //55 194 Combined char for navball target mode and SAS mode
00188                                  // First four bits indicate AutoPilot mode:
00189                                  // 0 SAS is off  //1 = Regular Stability Assist //2 = Prograde
00190                                  // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
00191                                  // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
00192                                  // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
00193      uint16_t ProgradePitch;  //56 Pitch   Of the Prograde Vector;  int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees)); 
00194      uint16_t ProgradeHeading;//57 Heading Of the Prograde Vector;  see above for range   (Prograde vector depends on navball mode, eg Surface/Orbit/Target)
00195      uint16_t ManeuverPitch;  //58 Pitch   Of the Maneuver Vector;  see above for range;  (0 if no Maneuver node)
00196      uint16_t ManeuverHeading;//59 Heading Of the Maneuver Vector;  see above for range;  (0 if no Maneuver node)
00197      uint16_t TargetPitch;    //60 Pitch   Of the Target   Vector;  see above for range;  (0 if no Target)
00198      uint16_t TargetHeading;  //61 Heading Of the Target   Vector;  see above for range;  (0 if no Target)
00199      uint16_t NormalHeading;  //62 Heading Of the Prograde Vector;  see above for range;  (Pitch of the Heading Vector is always 0)
00200      char vesselSync;       //63 208 Starting with 1, increased on every VesselChange
00201 };
00202 
00203 
00204 struct HandShakePacket
00205 {
00206   char id;
00207   char M1;
00208   char M2;
00209   char M3;
00210 };
00211 
00212 struct ControlPacket {
00213   char id;
00214   char MainControls;                  //SAS RCS Lights Gear Brakes Precision Abort Stage
00215   char Mode;                          //0 = stage, 1 = docking, 2 = map
00216   short ControlGroup;          //control groups 1-10 in 2 chars
00217   char NavballSASMode;                //AutoPilot mode
00218   char AdditionalControlByte1; //9
00219   int32_t Pitch;                          //-1000 -> 1000
00220   int32_t Roll;                           //-1000 -> 1000
00221   int32_t Yaw;                            //-1000 -> 1000
00222   int32_t TX;                             //-1000 -> 1000
00223   int32_t TY;                             //-1000 -> 1000
00224   int32_t TZ;                             //-1000 -> 1000
00225   int32_t WheelSteer;                     //-1000 -> 1000
00226   int32_t Throttle;                       //    0 -> 1000
00227   int32_t WheelThrottle;                  //    0 -> 1000
00228   char vesselSync;                    //Starting with 1, increased on every VesselChange
00229 };
00230 
00231 HandShakePacket HPacket;
00232 VesselData VData;
00233 ControlPacket CPacket;
00234 EthernetInterface eth;
00235 TCPSocketConnection sock;
00236 UDPSocket server;
00237 
00238 int main() {
00239     //SETUP
00240     char buffer[256];
00241     int ConnectionState = 0;
00242     
00243     t.start();
00244     printf("\n\r KSP mbedIO V0.1 \n\r");
00245     eth.init(); //Use DHCP
00246     eth.connect();
00247     printf("\nServer IP Address is %s\n\r", eth.getIPAddress());
00248     Endpoint KSPServer;
00249     while(true)
00250     {
00251         switch(ConnectionState)
00252         {
00253             case 0:// Connect to UDP server and get server IP address
00254             {
00255                 server.bind(PORT);
00256                 printf("Waiting for UDP packet...\n\r");
00257                 while(server.receiveFrom(KSPServer, buffer, sizeof(buffer)) < 0)
00258                 {
00259                     //Waiting for UDP to Connect
00260                 }
00261                 #if DEBUG
00262                 printf("Received packet from: %s\n\r", KSPServer.get_address());
00263                 #endif
00264                 ConnectionState++;
00265                 break;
00266             }
00267             case 1: // Connect to TCP Server
00268             {
00269                 sock.set_blocking(false, 1500); // Set socket to non-blocking timeout to 1.5s
00270                 while (sock.connect(KSPServer.get_address(), PORT) < 0) {
00271                     printf("Unable to connect to KSPServer on Port %d \r\n", PORT);
00272                     wait(1);
00273                 }
00274                 printf("Connected to KSPServer on Port %d \r\n", PORT);
00275                 Handshake();
00276                 printf("Handshake sent \r\n");
00277                 ConnectionState++;
00278                 break;
00279             }
00280             case 2: // We're Connected, check connection and keep running
00281             {
00282                 initLEDS();
00283                 InitTxPackets();
00284                 while(sock.is_connected())
00285                 {
00286                     input();
00287                     output();
00288                 }
00289                 sock.close();
00290                 printf("Connection Lost, Reconnecting... \n\r");
00291                 ConnectionState = 0;
00292                 break;
00293             }
00294         }
00295     }
00296 }
00297 
00298 
00299 
00300 //////////////////////////////////////////////////
00301 /////////////////// CONTROLS /////////////////////
00302 //////////////////////////////////////////////////
00303 
00304 void Indicators() {
00305     caution = 0;
00306     warning = 0;
00307     
00308     caution += VData.G > GCAUTION;
00309     warning += VData.G > GWARN;
00310    // caution += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELCAUTION;
00311    // warning += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELWARN;
00312     
00313     if (caution != 0)
00314     {
00315         printf("Yellow! \n\r");
00316     }
00317     else
00318     {
00319         //yel off
00320     }
00321     
00322     if (warning != 0)
00323     {
00324         printf("RED! \n\r");
00325     }
00326     else
00327     {
00328         //red off
00329     }
00330     #if OUTPUTENABLE
00331     printf("\033[2A");
00332     printf("\rAP : %f\n\r", VData.AP);
00333     //printf("TAp : %i \n\r", VData.TAp);
00334     //printf("Sync : %d \n\r", VData.vesselSync);
00335     printf("Alt : %f \n\r", VData.Alt);
00336     //printf("LiqFuel : %f \n\r", VData.LiquidFuel);
00337     float LiqFuelRem = VData.LiquidFuel/VData.LiquidFuelTot*100;
00338     printf("Fuel Left : %f %%", LiqFuelRem);
00339     //printf("Vsurf : %f", VData.Vsurf);
00340     //if(ControlStatus(AGSAS)) printf("AGSAS ON \n\r");
00341     //if(ControlStatus(AGRCS)) printf("AGRCS ON \n\r");
00342     //if(ControlStatus(AGCustom01)) printf("AGCustom1 ON \n\r");
00343     #endif
00344 }
00345 
00346 void initLEDS() {
00347 
00348 }
00349 
00350 void LEDSAllOff() {
00351 
00352 }
00353 
00354 void InitTxPackets() {
00355   HPacket.id = 0;  
00356   CPacket.id = 101;
00357 }
00358 
00359 
00360 //////////////////////////////////////////////////
00361 //////////////////// INPUT   /////////////////////
00362 //////////////////////////////////////////////////
00363 
00364 int input() {
00365   int returnValue = -1;
00366   now = t.read_ms();
00367 
00368   if (KSPBoardReceiveData())
00369   {
00370     deadtimeOld = now;
00371     returnValue = id;
00372     switch(id) 
00373     {
00374         case 0: //Handshake packet
00375             Handshake();
00376             #if DEBUG
00377             printf("HS Packet recieved\n\r");
00378             #endif
00379             break;
00380         case 1:
00381             VesselID = VData.vesselSync;
00382             Indicators();
00383             #if DEBUG
00384             printf("VD Packet recieved\n\r");
00385             #endif
00386             break;
00387     }
00388     Connected = true;
00389   }
00390   
00391   else
00392   { //if no message received for a while, go idle
00393     deadtime = now - deadtimeOld; 
00394     if (deadtime > IDLETIMER)
00395     {
00396       deadtimeOld = now;
00397       //Connected = false;
00398       LEDSAllOff();
00399       printf("Is Sleeping? \r\n");
00400     }    
00401   }
00402 
00403   return returnValue;
00404 }
00405 
00406 char ControlStatus(char n)
00407 {
00408   return ((VData.ActionGroups >> n) & 1) == 1;
00409 }
00410 
00411 //////////////////////////////////////////////////
00412 ///////////////////// OUTPUT /////////////////////
00413 //////////////////////////////////////////////////
00414 
00415 void output() {
00416   now = t.read_ms();
00417   controlTime = now - controlTimeOld;
00418   if (controlTime > CONTROLREFRESH) {
00419     controlTimeOld = now;
00420     controls();
00421   }
00422 }
00423 
00424 void controls() {
00425   if (Connected) {
00426 
00427     if (!ControlButton) { //--------- This is how you do main controls
00428         MainControls(SAS, 1);
00429         setSASMode(SMSAS); //setting SAS mode
00430         #if DEBUG
00431         printf("SAS PRESS \n\r");
00432         #endif
00433       //setNavballMode(NAVBallSURFACE); //setting navball mode
00434     }
00435     else {
00436       //setNavballMode(NAVBallTARGET);
00437       MainControls(SAS, 0);
00438     }
00439 
00440     if (ControlButton)
00441       MainControls(RCS, 1);
00442     else
00443       MainControls(RCS, 0);
00444 
00445     /**
00446     if (digitalRead(CG1PIN))   //--------- This is how you do control groups
00447       ControlGroups(1, 1);
00448     else
00449       ControlGroups(1, 0);
00450       **/
00451     /*
00452        if (getSASMode() == SMPrograde) { //--------- This is how you read SAS modes
00453          //Blink LED, do stuff, etc.
00454        }
00455 
00456        if (getNavballMode() == NAVBallTARGET) { //--------- This is how you read navball modes
00457          //Blink LED, do stuff, etc.
00458        }
00459     */
00460 
00461     //This is an example of reading analog inputs to an axis, with deadband and limits
00462     //CPacket.Throttle = constrain(map(analogRead(THROTTLEPIN), THROTTLEDB, 1024 - THROTTLEDB, 0, 1000), 0, 1000);
00463 
00464     //This is an example of reading analog inputs to an axis, with deadband and limits
00465     //CPacket.Pitch = constrain(map(analogRead(THROTTLEPIN),0,1024,-1000,1000),-1000, 1000);
00466     CPacket.vesselSync = VData.vesselSync;
00467     KSPBoardSendData(details(CPacket));
00468   }
00469 }
00470 
00471 void controlsInit() {
00472   /**
00473   pinMode(SASPIN, INPUT_PULLUP);
00474   pinMode(RCSPIN, INPUT_PULLUP);
00475   pinMode(CG1PIN, INPUT_PULLUP);
00476   **/
00477 }
00478 
00479 char getSASMode() {
00480   return VData.NavballSASMode & 0b00001111; // leaves alone the lower 4 bits of; all higher bits set to 0.
00481 }
00482 
00483 char getNavballMode() {
00484   return VData.NavballSASMode >> 4; // leaves alone the higher 4 bits of; all lower bits set to 0.
00485 }
00486 
00487 void setSASMode(char m) {
00488   CPacket.NavballSASMode &= 0b11110000;
00489   CPacket.NavballSASMode += m;
00490 }
00491 
00492 void setNavballMode(char m) {
00493   CPacket.NavballSASMode &= 0b00001111;
00494   CPacket.NavballSASMode += m << 4;
00495 }
00496 
00497 void MainControls(char n, bool s) {
00498   if (s)
00499     CPacket.MainControls |= (1 << n);       // forces nth bit of x to be 1.  all other bits left alone.
00500   else
00501     CPacket.MainControls &= ~(1 << n);      // forces nth bit of x to be 0.  all other bits left alone.
00502 }
00503 
00504 void ControlGroups(char n, bool s) {
00505   if (s)
00506     CPacket.ControlGroup |= (1 << n);       // forces nth bit of x to be 1.  all other bits left alone.
00507   else
00508     CPacket.ControlGroup &= ~(1 << n);      // forces nth bit of x to be 0.  all other bits left alone.
00509 }
00510 
00511 
00512 
00513 
00514 //////////////////////////////////////////////////
00515 //////////////// COMMUNICATIONS///////////////////
00516 //////////////////////////////////////////////////
00517 
00518 
00519 
00520 bool KSPBoardReceiveData() {
00521     int ret;
00522         
00523     ret = sock.receive(buffer, sizeof(buffer)-1);
00524     
00525     if(ret > 0)
00526     {
00527       int packetCheckState = 0;
00528         #if DEBUG
00529         printf("Packet Rx'd \n\r");
00530         #endif
00531         for(int x = 0; x < 4; x++)
00532         {
00533           switch(packetCheckState){
00534             case 0: // CHECK HEADER 1
00535                 if(buffer[packetCheckState] == 0xBE)
00536                 {
00537                     packetCheckState++;
00538                 }
00539                 break;
00540             case 1: // CHECK HEADER 2
00541                 if(buffer[packetCheckState] == 0xEF)
00542                 {
00543                     packetCheckState++;
00544                 }
00545                 break;
00546             case 2: // GET PAYLOAD SIZE
00547                 rx_len = buffer[packetCheckState];
00548                 packetCheckState++;
00549                 #if DEBUG
00550                 printf("PL Size at Rx: %d \n\r", rx_len);
00551                 #endif
00552                 break;
00553             case 3:
00554                 id = buffer[packetCheckState];
00555                 #if DEBUG
00556                 printf("Packet ID: %d \n\r", id);
00557                 #endif
00558                 if(id == 0)
00559                 {
00560                     structSize = sizeof(HPacket);   
00561                     address = (uint16_t*)&HPacket;
00562                 }
00563                 else if(id == 1)
00564                 {
00565                     structSize = sizeof(VData);   
00566                     address = (uint16_t*)&VData;     
00567                 }
00568                 else if(id == 2)
00569                 {
00570                     //Status Packet
00571                 }
00572                 else
00573                 {
00574                     printf("Bad Packet ID: %d", buffer[packetCheckState]);
00575                     packetCheckState = 0;
00576                 }
00577                 break;
00578             default :
00579                 printf("Something went wrong in Packet Rx Loop \r\n");
00580                 break;
00581             }
00582         }
00583         
00584         if(packetCheckState == 3) //We had a valid packet
00585         {
00586             calc_CS = rx_len;
00587             for(int i = 0; i < rx_len; i++)
00588             {
00589                 payloadBuffer[i] = buffer[i+3];
00590                 calc_CS^=buffer[i+3];
00591             }
00592             
00593             if(calc_CS == buffer[rx_len+3])   //CS Check
00594             {
00595                 char varBuffer[4];
00596                 #if DEBUG
00597                 printf("Checksum Passed! \n\r");
00598                 #endif
00599                 
00600                 VData.id = payloadBuffer[0];              //1
00601                 if(VData.id == 1)
00602                 {
00603                     memcpy(address,payloadBuffer,structSize);
00604                     rx_len = 0;
00605                     return true;
00606                 }
00607             }
00608             else
00609             {
00610                 //failed checksum, need to clear this out anyway
00611                 printf("Failed Checksum! Sorry \n\r");
00612                 rx_len = 0;
00613                 rx_array_inx = 1;
00614                 return false;
00615             }
00616                 
00617 
00618         }
00619     }
00620     
00621     return false;
00622 }
00623 
00624 void KSPBoardSendData(uint8_t * data, uint8_t len){
00625     uint8_t CS = len;
00626     char fullPacket[len+4];
00627     fullPacket[0] = 0xBE;
00628     fullPacket[1] = 0xEF;
00629     fullPacket[2] = len;
00630     
00631     if(*(data) == 101)
00632     {
00633         fullPacket[3] = 101; //Csid 101
00634         fullPacket[4] = CPacket.MainControls;
00635         fullPacket[5] = CPacket.Mode;
00636         fullPacket[6] = CPacket.ControlGroup & 0xff;
00637         fullPacket[7] = (CPacket.ControlGroup >> 8);
00638         fullPacket[8] = CPacket.NavballSASMode;
00639         fullPacket[9] = CPacket.AdditionalControlByte1;
00640         fullPacket[10] = CPacket.Pitch & 0xff;
00641         fullPacket[11] = (CPacket.Pitch >> 8);
00642         fullPacket[12] = CPacket.Roll & 0xff;
00643         fullPacket[13] = (CPacket.Roll >> 8);
00644         fullPacket[14] = CPacket.Yaw & 0xff;
00645         fullPacket[15] = (CPacket.Yaw >> 8);
00646         fullPacket[16] = CPacket.TX & 0xff;
00647         fullPacket[17] = (CPacket.TX >> 8);
00648         fullPacket[18] = CPacket.TY & 0xff;
00649         fullPacket[19] = (CPacket.TY >> 8);
00650         fullPacket[20] = CPacket.TZ & 0xff;
00651         fullPacket[21] = (CPacket.TZ >> 8);
00652         fullPacket[22] = CPacket.WheelSteer & 0xff;
00653         fullPacket[23] = (CPacket.WheelSteer >> 8);
00654         fullPacket[24] = CPacket.Throttle & 0xff;
00655         fullPacket[25] = (CPacket.Throttle >> 8);
00656         fullPacket[26] = CPacket.WheelThrottle & 0xff;
00657         fullPacket[27] = (CPacket.WheelThrottle >> 8);
00658         fullPacket[28] = CPacket.vesselSync;
00659         for(int i = 0; i<len; i++)
00660         {
00661             CS^= fullPacket[i+3];
00662         }
00663         fullPacket[sizeof(fullPacket)-1] = CS;
00664     }
00665     else
00666     {
00667         for(int i = 0; i<len; i++)
00668         {
00669             CS^=*(data+i);
00670             fullPacket[i+3] = *(data+i);
00671         }
00672         fullPacket[sizeof(fullPacket)-1] = CS;
00673     }
00674     #if DEBUG
00675     printf("Sending a Packet \n\r");
00676     #endif
00677     sock.send(fullPacket, sizeof(fullPacket)); // Send the packet 
00678 }
00679 
00680 void Handshake(){
00681 
00682     HPacket.id = 0;
00683     HPacket.M1 = 3;
00684     HPacket.M2 = 1;
00685     HPacket.M3 = 4;
00686     
00687     KSPBoardSendData(details(HPacket));
00688 }