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:
- 15:03ed24574806
- Parent:
- 11:59dcefdda506
- Child:
- 16:0eda62f0d9e8
diff -r 72be2b8b7f24 -r 03ed24574806 main.cpp
--- a/main.cpp Wed May 14 15:07:26 2014 +0000
+++ b/main.cpp Sat Mar 14 01:33:34 2020 +0000
@@ -1,31 +1,420 @@
#include "mbed.h"
#include "EthernetInterface.h"
+#include <mbed.h>
+#include "Thread.h"
+/* ---------------------------------------------------------------------------------------- */
+
+#define PORT 2342
+#define DEBUG 1
+
+
+#if DEBUG
+
+#endif
+/*
+void makePacket(char *Payload,char *Packet)
+{
+ char Header1 = 0xBE;
+ char Header2 = 0xEF;
+ int checksum = sizeof(*Payload);
+
+ *Packet[0] = Header1;
+ *Packet[1] = Header2;
+ *Packet[2] = sizeof(*Payload);
+
+ for (int i = 0; i < *Packet[2]; i++)
+ {
+ checksum ^= *Payload[i];
+ *Packet[i+3] = *Payload[i];
+ }
+ *Packet[sizeof(*Packet)-1] = checksum;
+ }
+*/
+
+DigitalIn press(USER_BUTTON);
+
+ char id; //1
+ float AP; //2
+ float PE; //3
+ float SemiMajorAxis; //4
+ float SemiMinorAxis; //5
+ float VVI; //6
+ float e; //7
+ float inc; //8
+ float G; //9
+ int TAp; //10
+ int TPe; //11
+ float TrueAnomaly; //12
+ float Density; //13
+ int period; //14
+ float RAlt; //15
+ float Alt; //16
+ float Vsurf; //17
+ float Lat; //18
+ float Lon; //19
+ float LiquidFuelTot; //20
+ float LiquidFuel; //21
+ float OxidizerTot; //22
+ float Oxidizer; //23
+ float EChargeTot; //24
+ float ECharge; //25
+ float MonoPropTot; //26
+ float MonoProp; //27
+ float IntakeAirTot; //28
+ float IntakeAir; //29
+ float SolidFuelTot; //30
+ float SolidFuel; //31
+ float XenonGasTot; //32
+ float XenonGas; //33
+ float LiquidFuelTotS; //34
+ float LiquidFuelS; //35
+ float OxidizerTotS; //36
+ float OxidizerS; //37
+ uint32_t MissionTime; //38
+ float deltaTime; //39
+ float VOrbit; //40
+ uint32_t MNTime; //41
+ float MNDeltaV; //42
+ uint16_t Pitch; //43
+ uint16_t Roll; //44
+ uint16_t Heading; //45
+ uint16_t ActionGroups; //46 status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10
+ char SOINumber; //47 SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
+ char MaxOverHeat; //48 Max part overheat (% percent)
+ float MachNumber; //49
+ float IAS; //50 Indicated Air Speed
+ char CurrentStage; //51 Current stage number
+ char TotalStage; //52 TotalNumber of stages
+ float TargetDist; //53 Distance to targeted vessel (m)
+ float TargetV; //54 Target vessel relative velocity (m/s)
+ char NavballSASMode; //55 Combined byte for navball target mode and SAS mode
+ // First four bits indicate AutoPilot mode:
+ // 0 SAS is off //1 = Regular Stability Assist //2 = Prograde
+ // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
+ // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
+ // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
+ uint16_t ProgradePitch; //56 Pitch Of the Prograde Vector; int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees));
+ uint16_t ProgradeHeading;//57 Heading Of the Prograde Vector; see above for range (Prograde vector depends on navball mode, eg Surface/Orbit/Target)
+ uint16_t ManeuverPitch; //58 Pitch Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
+ uint16_t ManeuverHeading;//59 Heading Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
+ uint16_t TargetPitch; //60 Pitch Of the Target Vector; see above for range; (0 if no Target)
+ uint16_t TargetHeading; //61 Heading Of the Target Vector; see above for range; (0 if no Target)
+ uint16_t NormalHeading; //62 Heading Of the Prograde Vector; see above for range; (Pitch of the Heading Vector is always 0)
+ char vesselSync; //63 Starting with 1, increased on every VesselChange
+
+void float2Bytes(char bytes_temp[4],float float_variable){
+ union {
+ float a;
+ unsigned char bytes[4];
+ } thing;
+ thing.a = float_variable;
+ memcpy(bytes_temp, thing.bytes, 4);
+}
+
+void unpackVesselData(char *PayloadBuffer, int payloadLength)
+{
+ char varBuffer[8];
+ id = PayloadBuffer[0]; //1
+ if(id == 0x01)
+ {
+ //float AP - 1
+ for(int i=0; i < 4; i++)
+ {
+ varBuffer[i]=PayloadBuffer[i+1];
+ }
+ /**
+ varBuffer[1]=PayloadBuffer[2];
+ varBuffer[2]=PayloadBuffer[3];
+ varBuffer[3]=PayloadBuffer[4];
+ **/
+ AP = *(float *)&varBuffer;
+
+ // float PE - 5
+ for(int i=0; i < 4; i++)
+ {
+ varBuffer[i]=PayloadBuffer[i+5];
+ }
+ PE = *(float *)&varBuffer;
+
+ //float SemiMajorAxis - 9
+ for(int i=0; i < 4; i++)
+ {
+ varBuffer[i]=PayloadBuffer[i+9];
+ }
+ SemiMajorAxis = *(float *)&varBuffer;
+
+ //float SemiMinorAxis - 13
+ for(int i=0; i < 4; i++)
+ {
+ varBuffer[i]=PayloadBuffer[i+13];
+ }
+ SemiMinorAxis = *(float *)&varBuffer; //5
+
+ //float VVI 17
+ for(int i=0; i < 4; i++)
+ {
+ varBuffer[i]=PayloadBuffer[i+17];
+ }
+ VVI = *(float *)&varBuffer; //6
+
+ //float e - 21
+ for(int i=0; i < 4; i++)
+ {
+ varBuffer[i]=PayloadBuffer[i+21];
+ }
+ e = *(float *)&varBuffer; //7
+
+ //float inc - 25
+ for(int i=0; i < 4; i++)
+ {
+ varBuffer[i]=PayloadBuffer[i+25];
+ }
+ inc = *(float *)&varBuffer; //8
+ }
+ else
+ {
+ printf("Bad Packet Id: %d", id);
+ }
+}
+
+
+
int main() {
+
EthernetInterface eth;
eth.init(); //Use DHCP
eth.connect();
- printf("IP Address is %s\n", eth.getIPAddress());
+ printf("\nServer IP Address is %s\n\r", eth.getIPAddress());
+
+ UDPSocket server;
+ server.bind(PORT);
+
+ Endpoint KSPServer;
+ char buffer[256];
+
+ printf("Waiting for UDP packet...\n\r");
+ int n = server.receiveFrom(KSPServer, buffer, sizeof(buffer));
+ buffer[n] = '\0';
+ #if DEBUG
+ printf("Received packet from: %s\n\r", KSPServer.get_address());
+ //printf("Packet contents : '%s'\n\r",buffer);
+ #endif
+ // server.sendTo(client, buffer, n)
+
+
+ /**
+
+ HSPacket.id=0;
+ HSPacket.M1=3;
+ HSPacket.M2=1;
+ HSPacket.status=4;
+ **/
+ char HSPayload[4]={0x00, 0x03, 0x01, 0x04};
+ char HSPacket[sizeof(HSPayload)+4];
+ char Header1 = 0xBE;
+ char Header2 = 0xEF;
+ int checksum = sizeof(HSPayload);
+
+ HSPacket[0] = Header1;
+ HSPacket[1] = Header2;
+ HSPacket[2] = sizeof(HSPayload);
+
+ int CurrentPacketLength=0;
+ int CurrentBytesRead=0;
+ char PayloadBuffer[300];
+ char NewPacketBuffer[300];
+
+ for (int i = 0; i < sizeof(HSPayload); i++)
+ {
+ checksum ^= HSPayload[i];
+ HSPacket[i+3] = HSPayload[i];
+ }
+ HSPacket[sizeof(HSPacket)-1] = checksum;
+
+
+ for(int i=0;i<sizeof(HSPacket);i++)
+ {
+ printf("%c", HSPacket[i]);
+ }
+
+ //makePacket(HSPayload, HSPacket);
TCPSocketConnection sock;
- sock.connect("mbed.org", 80);
- char http_cmd[] = "GET /media/uploads/mbed_official/hello.txt HTTP/1.0\n\n";
- sock.send_all(http_cmd, sizeof(http_cmd)-1);
+ while (sock.connect(KSPServer.get_address(), PORT) < 0) {
+ printf("Unable to connect to KSPServer on Port %d \r\n", PORT);
+ wait(1);
+ }
+
+ printf("Connected to KSP Server \n\r");
+ sock.send(HSPacket, sizeof(HSPacket));
- char buffer[300];
- int ret;
- while (true) {
+ while (sock.is_connected()) {
+ char buffer[300];
+ int ret;
+
ret = sock.receive(buffer, sizeof(buffer)-1);
- if (ret <= 0)
- break;
buffer[ret] = '\0';
- printf("Received %d chars from server:\n%s\n", ret, buffer);
+ if(ret > 0)
+ {
+ int CurrentState = 0;
+
+ for (int x = 0; x < sizeof(buffer)-1; x++)
+ {
+ switch (CurrentState)
+ {
+ case 0: //HEADER 1
+ if (buffer[x] == 0xBE) CurrentState = 1;
+ break;
+ case 1:
+ if (buffer[x] == 0xEF) CurrentState = 2;
+ else CurrentState = 0;
+ break;
+ case 2: //HEADER 2
+ CurrentPacketLength = (int)buffer[x];
+ checksum = CurrentPacketLength;
+ CurrentBytesRead = 0;
+ CurrentState = 3;
+ break;
+ case 3: //PAYLOAD SIZE
+ PayloadBuffer[CurrentBytesRead] = buffer[x];
+ CurrentBytesRead++;
+ if (CurrentBytesRead == CurrentPacketLength)
+ {
+ CurrentState = 4;
+ }
+ break;
+ case 4://CHECKSUM
+ for (int i = 0; i < CurrentPacketLength; i++)
+ {
+ checksum ^= PayloadBuffer[i];
+ }
+ if(buffer[x] == checksum)
+ {
+ #if DEBUG
+ printf("Checksum Success. Payload recieved. \r\n");
+ #endif
+ }
+ else printf("Checksum Failed! Sorry. \r\n");
+ CurrentState = 0;
+ break;
+ }
+ }
+ #if DEBUG
+ printf("Payload ID: %d \r\n", PayloadBuffer[0]);
+ #endif
+ if(PayloadBuffer[0] == 0)
+ {
+
+ }
+ if(PayloadBuffer[0] == 1)
+ {
+ unpackVesselData(PayloadBuffer, CurrentPacketLength);
+ #if OUTPUTENABLE
+ printf("AP %f \n\r", AP);
+ #endif
+ printf("00000%s",PayloadBuffer);
+ printf("CCCCC%c",checksum);
+ //sock.send(HSPacket, sizeof(HSPacket));
+ }
+
+ else
+ {
+
+ }
+
+ //55 Combined byte for navball target mode and SAS mode
+ // First four bits indicate AutoPilot mode:
+ // 0 SAS is off //1 = Regular Stability Assist //2 = Prograde
+ // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
+ // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
+ // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
+ char MainControls = 0b11111111; //SAS RCS Lights Gear Brakes Precision Abort Stage
+ char Mode = 0b00000000; //0 = stage, 1 = docking, 2 = map (Bit 0-3)
+ //0 = Auto, 1 = Free, 2 = Orbital, 3 = Chase, 4 = Locked (Bit 4-7)
+ uint16_t ControlGroup = 0b00000000; //control groups 1-10 in 2 bytes
+ char NavballSASMode = press; //AutoPilot mode (See above for AutoPilot modes)(Ignored if the equal to zero or out of bounds (>10)) //Navball mode
+ short Throttle = 500;
+ if(press)
+ {
+ NavballSASMode = 0b01000000;
+ Throttle = 0;
+ printf("PRESS\n\r\n");
+ }
+ else
+ {
+ NavballSASMode = 0b00000000;
+ }
+
+
+ char AdditionalControlByte1 = 0; //Bit 0: Open Menu, Bit 1: Open Map
+ short Pitch = 0; //-1000 -> 1000
+ short Roll = 0; //-1000 -> 1000
+ short Yaw = 0; //-1000 -> 1000
+ short TX = 0; //-1000 -> 1000
+ short TY = 0; //-1000 -> 1000
+ short TZ = 0; //-1000 -> 1000
+ short WheelSteer = 0; //-1000 -> 1000
+ //short Throttle = 500; // 0 -> 1000
+ short WheelThrottle = 0; // 0 -> 1000
+ char vesselSync = PayloadBuffer[sizeof(PayloadBuffer)-1];
+ char vSync = vesselSync + 30;
+ printf("Vsync %c \n\r\n", vSync);
+ char CSPayload[26];
+ char CSPacket[sizeof(CSPayload)+4];
+ Header1 = 0xBE;
+ Header2 = 0xEF;
+ checksum = sizeof(CSPayload);
+
+ CSPacket[0] = Header1;
+ CSPacket[1] = Header2;
+ CSPacket[2] = sizeof(CSPayload);
+
+ CSPayload[0] = 101; //Csid 101
+ CSPayload[1] = MainControls;
+ CSPayload[2] = Mode;
+ CSPayload[3] = ControlGroup & 0xff;
+ CSPayload[4] = (ControlGroup >> 8);
+ CSPayload[5] = NavballSASMode;
+ CSPayload[6] = AdditionalControlByte1;
+ CSPayload[7] = Pitch & 0xff;
+ CSPayload[8] = (Pitch >> 8);
+ CSPayload[9] = Roll & 0xff;
+ CSPayload[10] = (Roll >> 8);
+ CSPayload[11] = Yaw & 0xff;
+ CSPayload[12] = (Yaw >> 8);
+ CSPayload[13] = TX & 0xff;
+ CSPayload[14] = (TX >> 8);
+ CSPayload[15] = TY & 0xff;
+ CSPayload[16] = (TY >> 8);
+ CSPayload[17] = TZ & 0xff;
+ CSPayload[18] = (TZ >> 8);
+ CSPayload[19] = WheelSteer & 0xff;
+ CSPayload[20] = (WheelSteer >> 8);
+ CSPayload[21] = Throttle & 0xff;
+ CSPayload[22] = (Throttle >> 8);
+ CSPayload[23] = WheelThrottle & 0xff;
+ CSPayload[24] = (WheelThrottle >> 8);
+ CSPayload[25] = vesselSync;
+
+ for (int i = 0; i < sizeof(CSPayload); i++)
+ {
+ checksum ^= CSPayload[i];
+ CSPacket[i+3] = CSPayload[i];
+ }
+ CSPacket[sizeof(CSPacket)-1] = checksum;
+ sock.send_all(CSPacket, sizeof(CSPacket));
+
+
+ }
+// sock.send_all(HSPacket, sizeof(HSPacket));
}
-
+
+ printf("We got shut down! Socket Closed!");
sock.close();
eth.disconnect();
while(1) {}
-}
+}
\ No newline at end of file