Andrew Wakestrap
A port of KSP SerialIO and KSP Ethernet IO designed for a Nucleo F746ZG. This version doesn't read from or write to any external controls or displays but the user can press the USER_BUTTON to change the status of SAS and RCS. A final version of this project with full documentation and external hardware support is coming. This is an Alpha release.
Dependencies: F7_Ethernet mbed mbed-rtos
Diff: main.cpp
- Revision:
- 16:0eda62f0d9e8
- Parent:
- 15:03ed24574806
- Child:
- 17:00d66e5edf58
diff -r 03ed24574806 -r 0eda62f0d9e8 main.cpp
--- a/main.cpp Sat Mar 14 01:33:34 2020 +0000
+++ b/main.cpp Sun Mar 15 04:35:51 2020 +0000
@@ -2,187 +2,242 @@
#include "EthernetInterface.h"
#include <mbed.h>
#include "Thread.h"
+
/* ---------------------------------------------------------------------------------------- */
#define PORT 2342
-#define DEBUG 1
+#define DEBUG 0
+#define OUTPUTENABLE 0
#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;
- }
-*/
+
+//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
-DigitalIn press(USER_BUTTON);
+//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
- 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 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];
-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)
+struct VesselData
{
- 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);
- }
-}
+ 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() {
-
- EthernetInterface eth;
+ //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());
@@ -193,228 +248,710 @@
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];
+ 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
- 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;
+ 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");
- printf("Connected to KSP Server \n\r");
- sock.send(HSPacket, sizeof(HSPacket));
+
+ 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
+ }
- while (sock.is_connected()) {
- char buffer[300];
- int ret;
+ 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);
- buffer[ret] = '\0';
- if(ret > 0)
+ 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++)
{
- int CurrentState = 0;
-
- for (int x = 0; x < sizeof(buffer)-1; x++)
- {
- switch (CurrentState)
+ switch(packetCheckState){
+ case 0: // CHECK HEADER 1
+ if(buffer[packetCheckState] == 0xBE)
{
- 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;
+ packetCheckState++;
+ }
+ break;
+ case 1: // CHECK HEADER 2
+ if(buffer[packetCheckState] == 0xEF)
+ {
+ packetCheckState++;
}
- }
- #if DEBUG
- printf("Payload ID: %d \r\n", PayloadBuffer[0]);
- #endif
- if(PayloadBuffer[0] == 0)
- {
-
+ 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(PayloadBuffer[0] == 1)
+ }
+
+ if(packetCheckState == 3) //We had a valid packet
+ {
+ calc_CS = rx_len;
+ for(int i = 0; i < rx_len; i++)
{
- 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));
+ payloadBuffer[i] = buffer[i+3];
+ calc_CS^=buffer[i+3];
}
- else
+ 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);
- }
-
- //55 Combined byte for navball target mode and SAS mode
+ 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)
- 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");
+ 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
{
- NavballSASMode = 0b00000000;
+ //failed checksum, need to clear this out anyway
+ printf("Failed Checksum! Sorry \n\r");
+ rx_len = 0;
+ rx_array_inx = 1;
+ return false;
}
-
-
- 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();
+ 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;
- eth.disconnect();
+ 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;
- while(1) {}
-}
\ No newline at end of file
+ KSPBoardSendData(details(HPacket));
+}