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:
- 17:00d66e5edf58
- Parent:
- 16:0eda62f0d9e8
--- a/main.cpp Sun Mar 15 04:35:51 2020 +0000
+++ b/main.cpp Tue Mar 17 01:25:35 2020 +0000
@@ -8,7 +8,7 @@
#define PORT 2342
#define DEBUG 0
-#define OUTPUTENABLE 0
+#define OUTPUTENABLE 1
#if DEBUG
@@ -126,6 +126,8 @@
uint8_t calc_CS; //calculated Chacksum
char payloadBuffer[255];
+#pragma pack (1)
+
struct VesselData
{
char id; //1
@@ -231,50 +233,65 @@
ControlPacket CPacket;
EthernetInterface eth;
TCPSocketConnection sock;
-
+UDPSocket server;
int main() {
//SETUP
+ char buffer[256];
+ int ConnectionState = 0;
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();
+ switch(ConnectionState)
+ {
+ case 0:// Connect to UDP server and get server IP address
+ {
+ server.bind(PORT);
+ printf("Waiting for UDP packet...\n\r");
+ while(server.receiveFrom(KSPServer, buffer, sizeof(buffer)) < 0)
+ {
+ //Waiting for UDP to Connect
+ }
+ #if DEBUG
+ printf("Received packet from: %s\n\r", KSPServer.get_address());
+ #endif
+ ConnectionState++;
+ break;
+ }
+ case 1: // Connect to TCP Server
+ {
+ sock.set_blocking(false, 1500); // Set socket to non-blocking timeout to 1.5s
+ 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");
+ ConnectionState++;
+ break;
+ }
+ case 2: // We're Connected, check connection and keep running
+ {
+ initLEDS();
+ InitTxPackets();
+ while(sock.is_connected())
+ {
+ input();
+ output();
+ }
+ sock.close();
+ printf("Connection Lost, Reconnecting... \n\r");
+ ConnectionState = 0;
+ break;
+ }
+ }
}
}
@@ -290,8 +307,8 @@
caution += VData.G > GCAUTION;
warning += VData.G > GWARN;
- caution += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELCAUTION;
- warning += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELWARN;
+ // caution += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELCAUTION;
+ // warning += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELWARN;
if (caution != 0)
{
@@ -311,13 +328,18 @@
//red off
}
#if OUTPUTENABLE
- printf("AP : %f \n\r", VData.AP);
- printf("Sync : %d \n\r", VData.vesselSync);
+ printf("\033[2A");
+ printf("\rAP : %f\n\r", VData.AP);
+ //printf("TAp : %i \n\r", VData.TAp);
+ //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");
+ //printf("LiqFuel : %f \n\r", VData.LiquidFuel);
+ float LiqFuelRem = VData.LiquidFuel/VData.LiquidFuelTot*100;
+ printf("Fuel Left : %f %%", LiqFuelRem);
+ //printf("Vsurf : %f", 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
}
@@ -372,9 +394,9 @@
if (deadtime > IDLETIMER)
{
deadtimeOld = now;
- Connected = false;
+ //Connected = false;
LEDSAllOff();
- printf("No packets for a while... going Idle \n\r");
+ printf("Is Sleeping? \r\n");
}
}
@@ -543,6 +565,10 @@
structSize = sizeof(VData);
address = (uint16_t*)&VData;
}
+ else if(id == 2)
+ {
+ //Status Packet
+ }
else
{
printf("Bad Packet ID: %d", buffer[packetCheckState]);
@@ -571,308 +597,13 @@
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++)
+ if(VData.id == 1)
{
- 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];
+ memcpy(address,payloadBuffer,structSize);
+ rx_len = 0;
+ return true;
}
- 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
{