Andrew Wakestrap / Mbed 2 deprecated KSPmbedIO

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

main.cpp@15:03ed24574806, 2020-03-14 (annotated)

Committer:
wakestrap
Date:
Sat Mar 14 01:33:34 2020 +0000
Revision:
15:03ed24574806
Parent:
11:59dcefdda506
Child:
16:0eda62f0d9e8
WORKING VERSION ISH

Who changed what in which revision?

UserRevisionLine numberNew contents of line
donatien 0:bb128f0e952f 1 #include "mbed.h"
donatien 0:bb128f0e952f 2 #include "EthernetInterface.h"
wakestrap 15:03ed24574806 3 #include <mbed.h>
wakestrap 15:03ed24574806 4 #include "Thread.h"
wakestrap 15:03ed24574806 5 /* ---------------------------------------------------------------------------------------- */
wakestrap 15:03ed24574806 6
wakestrap 15:03ed24574806 7 #define PORT 2342
wakestrap 15:03ed24574806 8 #define DEBUG 1
wakestrap 15:03ed24574806 9
wakestrap 15:03ed24574806 10
wakestrap 15:03ed24574806 11 #if DEBUG
wakestrap 15:03ed24574806 12
wakestrap 15:03ed24574806 13 #endif
wakestrap 15:03ed24574806 14 /*
wakestrap 15:03ed24574806 15 void makePacket(char *Payload,char *Packet)
wakestrap 15:03ed24574806 16 {
wakestrap 15:03ed24574806 17 char Header1 = 0xBE;
wakestrap 15:03ed24574806 18 char Header2 = 0xEF;
wakestrap 15:03ed24574806 19 int checksum = sizeof(*Payload);
wakestrap 15:03ed24574806 20
wakestrap 15:03ed24574806 21 *Packet[0] = Header1;
wakestrap 15:03ed24574806 22 *Packet[1] = Header2;
wakestrap 15:03ed24574806 23 *Packet[2] = sizeof(*Payload);
wakestrap 15:03ed24574806 24
wakestrap 15:03ed24574806 25 for (int i = 0; i < *Packet[2]; i++)
wakestrap 15:03ed24574806 26 {
wakestrap 15:03ed24574806 27 checksum ^= *Payload[i];
wakestrap 15:03ed24574806 28 *Packet[i+3] = *Payload[i];
wakestrap 15:03ed24574806 29 }
wakestrap 15:03ed24574806 30 *Packet[sizeof(*Packet)-1] = checksum;
wakestrap 15:03ed24574806 31 }
wakestrap 15:03ed24574806 32 */
wakestrap 15:03ed24574806 33
wakestrap 15:03ed24574806 34 DigitalIn press(USER_BUTTON);
wakestrap 15:03ed24574806 35
wakestrap 15:03ed24574806 36 char id; //1
wakestrap 15:03ed24574806 37 float AP; //2
wakestrap 15:03ed24574806 38 float PE; //3
wakestrap 15:03ed24574806 39 float SemiMajorAxis; //4
wakestrap 15:03ed24574806 40 float SemiMinorAxis; //5
wakestrap 15:03ed24574806 41 float VVI; //6
wakestrap 15:03ed24574806 42 float e; //7
wakestrap 15:03ed24574806 43 float inc; //8
wakestrap 15:03ed24574806 44 float G; //9
wakestrap 15:03ed24574806 45 int TAp; //10
wakestrap 15:03ed24574806 46 int TPe; //11
wakestrap 15:03ed24574806 47 float TrueAnomaly; //12
wakestrap 15:03ed24574806 48 float Density; //13
wakestrap 15:03ed24574806 49 int period; //14
wakestrap 15:03ed24574806 50 float RAlt; //15
wakestrap 15:03ed24574806 51 float Alt; //16
wakestrap 15:03ed24574806 52 float Vsurf; //17
wakestrap 15:03ed24574806 53 float Lat; //18
wakestrap 15:03ed24574806 54 float Lon; //19
wakestrap 15:03ed24574806 55 float LiquidFuelTot; //20
wakestrap 15:03ed24574806 56 float LiquidFuel; //21
wakestrap 15:03ed24574806 57 float OxidizerTot; //22
wakestrap 15:03ed24574806 58 float Oxidizer; //23
wakestrap 15:03ed24574806 59 float EChargeTot; //24
wakestrap 15:03ed24574806 60 float ECharge; //25
wakestrap 15:03ed24574806 61 float MonoPropTot; //26
wakestrap 15:03ed24574806 62 float MonoProp; //27
wakestrap 15:03ed24574806 63 float IntakeAirTot; //28
wakestrap 15:03ed24574806 64 float IntakeAir; //29
wakestrap 15:03ed24574806 65 float SolidFuelTot; //30
wakestrap 15:03ed24574806 66 float SolidFuel; //31
wakestrap 15:03ed24574806 67 float XenonGasTot; //32
wakestrap 15:03ed24574806 68 float XenonGas; //33
wakestrap 15:03ed24574806 69 float LiquidFuelTotS; //34
wakestrap 15:03ed24574806 70 float LiquidFuelS; //35
wakestrap 15:03ed24574806 71 float OxidizerTotS; //36
wakestrap 15:03ed24574806 72 float OxidizerS; //37
wakestrap 15:03ed24574806 73 uint32_t MissionTime; //38
wakestrap 15:03ed24574806 74 float deltaTime; //39
wakestrap 15:03ed24574806 75 float VOrbit; //40
wakestrap 15:03ed24574806 76 uint32_t MNTime; //41
wakestrap 15:03ed24574806 77 float MNDeltaV; //42
wakestrap 15:03ed24574806 78 uint16_t Pitch; //43
wakestrap 15:03ed24574806 79 uint16_t Roll; //44
wakestrap 15:03ed24574806 80 uint16_t Heading; //45
wakestrap 15:03ed24574806 81 uint16_t ActionGroups; //46 status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10
wakestrap 15:03ed24574806 82 char SOINumber; //47 SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
wakestrap 15:03ed24574806 83 char MaxOverHeat; //48 Max part overheat (% percent)
wakestrap 15:03ed24574806 84 float MachNumber; //49
wakestrap 15:03ed24574806 85 float IAS; //50 Indicated Air Speed
wakestrap 15:03ed24574806 86 char CurrentStage; //51 Current stage number
wakestrap 15:03ed24574806 87 char TotalStage; //52 TotalNumber of stages
wakestrap 15:03ed24574806 88 float TargetDist; //53 Distance to targeted vessel (m)
wakestrap 15:03ed24574806 89 float TargetV; //54 Target vessel relative velocity (m/s)
wakestrap 15:03ed24574806 90 char NavballSASMode; //55 Combined byte for navball target mode and SAS mode
wakestrap 15:03ed24574806 91 // First four bits indicate AutoPilot mode:
wakestrap 15:03ed24574806 92 // 0 SAS is off //1 = Regular Stability Assist //2 = Prograde
wakestrap 15:03ed24574806 93 // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
wakestrap 15:03ed24574806 94 // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
wakestrap 15:03ed24574806 95 // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
wakestrap 15:03ed24574806 96 uint16_t ProgradePitch; //56 Pitch Of the Prograde Vector; int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees));
wakestrap 15:03ed24574806 97 uint16_t ProgradeHeading;//57 Heading Of the Prograde Vector; see above for range (Prograde vector depends on navball mode, eg Surface/Orbit/Target)
wakestrap 15:03ed24574806 98 uint16_t ManeuverPitch; //58 Pitch Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 15:03ed24574806 99 uint16_t ManeuverHeading;//59 Heading Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 15:03ed24574806 100 uint16_t TargetPitch; //60 Pitch Of the Target Vector; see above for range; (0 if no Target)
wakestrap 15:03ed24574806 101 uint16_t TargetHeading; //61 Heading Of the Target Vector; see above for range; (0 if no Target)
wakestrap 15:03ed24574806 102 uint16_t NormalHeading; //62 Heading Of the Prograde Vector; see above for range; (Pitch of the Heading Vector is always 0)
wakestrap 15:03ed24574806 103 char vesselSync; //63 Starting with 1, increased on every VesselChange
wakestrap 15:03ed24574806 104
wakestrap 15:03ed24574806 105 void float2Bytes(char bytes_temp[4],float float_variable){
wakestrap 15:03ed24574806 106 union {
wakestrap 15:03ed24574806 107 float a;
wakestrap 15:03ed24574806 108 unsigned char bytes[4];
wakestrap 15:03ed24574806 109 } thing;
wakestrap 15:03ed24574806 110 thing.a = float_variable;
wakestrap 15:03ed24574806 111 memcpy(bytes_temp, thing.bytes, 4);
wakestrap 15:03ed24574806 112 }
wakestrap 15:03ed24574806 113
wakestrap 15:03ed24574806 114 void unpackVesselData(char *PayloadBuffer, int payloadLength)
wakestrap 15:03ed24574806 115 {
wakestrap 15:03ed24574806 116 char varBuffer[8];
wakestrap 15:03ed24574806 117 id = PayloadBuffer[0]; //1
wakestrap 15:03ed24574806 118 if(id == 0x01)
wakestrap 15:03ed24574806 119 {
wakestrap 15:03ed24574806 120 //float AP - 1
wakestrap 15:03ed24574806 121 for(int i=0; i < 4; i++)
wakestrap 15:03ed24574806 122 {
wakestrap 15:03ed24574806 123 varBuffer[i]=PayloadBuffer[i+1];
wakestrap 15:03ed24574806 124 }
wakestrap 15:03ed24574806 125 /**
wakestrap 15:03ed24574806 126 varBuffer[1]=PayloadBuffer[2];
wakestrap 15:03ed24574806 127 varBuffer[2]=PayloadBuffer[3];
wakestrap 15:03ed24574806 128 varBuffer[3]=PayloadBuffer[4];
wakestrap 15:03ed24574806 129 **/
wakestrap 15:03ed24574806 130 AP = *(float *)&varBuffer;
wakestrap 15:03ed24574806 131
wakestrap 15:03ed24574806 132 // float PE - 5
wakestrap 15:03ed24574806 133 for(int i=0; i < 4; i++)
wakestrap 15:03ed24574806 134 {
wakestrap 15:03ed24574806 135 varBuffer[i]=PayloadBuffer[i+5];
wakestrap 15:03ed24574806 136 }
wakestrap 15:03ed24574806 137 PE = *(float *)&varBuffer;
wakestrap 15:03ed24574806 138
wakestrap 15:03ed24574806 139 //float SemiMajorAxis - 9
wakestrap 15:03ed24574806 140 for(int i=0; i < 4; i++)
wakestrap 15:03ed24574806 141 {
wakestrap 15:03ed24574806 142 varBuffer[i]=PayloadBuffer[i+9];
wakestrap 15:03ed24574806 143 }
wakestrap 15:03ed24574806 144 SemiMajorAxis = *(float *)&varBuffer;
wakestrap 15:03ed24574806 145
wakestrap 15:03ed24574806 146 //float SemiMinorAxis - 13
wakestrap 15:03ed24574806 147 for(int i=0; i < 4; i++)
wakestrap 15:03ed24574806 148 {
wakestrap 15:03ed24574806 149 varBuffer[i]=PayloadBuffer[i+13];
wakestrap 15:03ed24574806 150 }
wakestrap 15:03ed24574806 151 SemiMinorAxis = *(float *)&varBuffer; //5
wakestrap 15:03ed24574806 152
wakestrap 15:03ed24574806 153 //float VVI 17
wakestrap 15:03ed24574806 154 for(int i=0; i < 4; i++)
wakestrap 15:03ed24574806 155 {
wakestrap 15:03ed24574806 156 varBuffer[i]=PayloadBuffer[i+17];
wakestrap 15:03ed24574806 157 }
wakestrap 15:03ed24574806 158 VVI = *(float *)&varBuffer; //6
wakestrap 15:03ed24574806 159
wakestrap 15:03ed24574806 160 //float e - 21
wakestrap 15:03ed24574806 161 for(int i=0; i < 4; i++)
wakestrap 15:03ed24574806 162 {
wakestrap 15:03ed24574806 163 varBuffer[i]=PayloadBuffer[i+21];
wakestrap 15:03ed24574806 164 }
wakestrap 15:03ed24574806 165 e = *(float *)&varBuffer; //7
wakestrap 15:03ed24574806 166
wakestrap 15:03ed24574806 167 //float inc - 25
wakestrap 15:03ed24574806 168 for(int i=0; i < 4; i++)
wakestrap 15:03ed24574806 169 {
wakestrap 15:03ed24574806 170 varBuffer[i]=PayloadBuffer[i+25];
wakestrap 15:03ed24574806 171 }
wakestrap 15:03ed24574806 172 inc = *(float *)&varBuffer; //8
wakestrap 15:03ed24574806 173 }
wakestrap 15:03ed24574806 174 else
wakestrap 15:03ed24574806 175 {
wakestrap 15:03ed24574806 176 printf("Bad Packet Id: %d", id);
wakestrap 15:03ed24574806 177 }
wakestrap 15:03ed24574806 178 }
wakestrap 15:03ed24574806 179
wakestrap 15:03ed24574806 180
wakestrap 15:03ed24574806 181
donatien 0:bb128f0e952f 182
emilmont 7:65188f4a8c25 183 int main() {
wakestrap 15:03ed24574806 184
donatien 0:bb128f0e952f 185 EthernetInterface eth;
donatien 0:bb128f0e952f 186 eth.init(); //Use DHCP
donatien 0:bb128f0e952f 187 eth.connect();
wakestrap 15:03ed24574806 188 printf("\nServer IP Address is %s\n\r", eth.getIPAddress());
wakestrap 15:03ed24574806 189
wakestrap 15:03ed24574806 190 UDPSocket server;
wakestrap 15:03ed24574806 191 server.bind(PORT);
wakestrap 15:03ed24574806 192
wakestrap 15:03ed24574806 193 Endpoint KSPServer;
wakestrap 15:03ed24574806 194 char buffer[256];
wakestrap 15:03ed24574806 195
wakestrap 15:03ed24574806 196 printf("Waiting for UDP packet...\n\r");
wakestrap 15:03ed24574806 197 int n = server.receiveFrom(KSPServer, buffer, sizeof(buffer));
wakestrap 15:03ed24574806 198 buffer[n] = '\0';
wakestrap 15:03ed24574806 199 #if DEBUG
wakestrap 15:03ed24574806 200 printf("Received packet from: %s\n\r", KSPServer.get_address());
wakestrap 15:03ed24574806 201 //printf("Packet contents : '%s'\n\r",buffer);
wakestrap 15:03ed24574806 202 #endif
wakestrap 15:03ed24574806 203 // server.sendTo(client, buffer, n)
wakestrap 15:03ed24574806 204
wakestrap 15:03ed24574806 205
wakestrap 15:03ed24574806 206 /**
wakestrap 15:03ed24574806 207
wakestrap 15:03ed24574806 208 HSPacket.id=0;
wakestrap 15:03ed24574806 209 HSPacket.M1=3;
wakestrap 15:03ed24574806 210 HSPacket.M2=1;
wakestrap 15:03ed24574806 211 HSPacket.status=4;
wakestrap 15:03ed24574806 212 **/
wakestrap 15:03ed24574806 213 char HSPayload[4]={0x00, 0x03, 0x01, 0x04};
wakestrap 15:03ed24574806 214 char HSPacket[sizeof(HSPayload)+4];
wakestrap 15:03ed24574806 215 char Header1 = 0xBE;
wakestrap 15:03ed24574806 216 char Header2 = 0xEF;
wakestrap 15:03ed24574806 217 int checksum = sizeof(HSPayload);
wakestrap 15:03ed24574806 218
wakestrap 15:03ed24574806 219 HSPacket[0] = Header1;
wakestrap 15:03ed24574806 220 HSPacket[1] = Header2;
wakestrap 15:03ed24574806 221 HSPacket[2] = sizeof(HSPayload);
wakestrap 15:03ed24574806 222
wakestrap 15:03ed24574806 223 int CurrentPacketLength=0;
wakestrap 15:03ed24574806 224 int CurrentBytesRead=0;
wakestrap 15:03ed24574806 225 char PayloadBuffer[300];
wakestrap 15:03ed24574806 226 char NewPacketBuffer[300];
wakestrap 15:03ed24574806 227
wakestrap 15:03ed24574806 228 for (int i = 0; i < sizeof(HSPayload); i++)
wakestrap 15:03ed24574806 229 {
wakestrap 15:03ed24574806 230 checksum ^= HSPayload[i];
wakestrap 15:03ed24574806 231 HSPacket[i+3] = HSPayload[i];
wakestrap 15:03ed24574806 232 }
wakestrap 15:03ed24574806 233 HSPacket[sizeof(HSPacket)-1] = checksum;
wakestrap 15:03ed24574806 234
wakestrap 15:03ed24574806 235
wakestrap 15:03ed24574806 236 for(int i=0;i<sizeof(HSPacket);i++)
wakestrap 15:03ed24574806 237 {
wakestrap 15:03ed24574806 238 printf("%c", HSPacket[i]);
wakestrap 15:03ed24574806 239 }
wakestrap 15:03ed24574806 240
wakestrap 15:03ed24574806 241 //makePacket(HSPayload, HSPacket);
donatien 0:bb128f0e952f 242
emilmont 7:65188f4a8c25 243 TCPSocketConnection sock;
donatien 0:bb128f0e952f 244
wakestrap 15:03ed24574806 245 while (sock.connect(KSPServer.get_address(), PORT) < 0) {
wakestrap 15:03ed24574806 246 printf("Unable to connect to KSPServer on Port %d \r\n", PORT);
wakestrap 15:03ed24574806 247 wait(1);
wakestrap 15:03ed24574806 248 }
wakestrap 15:03ed24574806 249
wakestrap 15:03ed24574806 250 printf("Connected to KSP Server \n\r");
wakestrap 15:03ed24574806 251 sock.send(HSPacket, sizeof(HSPacket));
emilmont 7:65188f4a8c25 252
wakestrap 15:03ed24574806 253 while (sock.is_connected()) {
wakestrap 15:03ed24574806 254 char buffer[300];
wakestrap 15:03ed24574806 255 int ret;
wakestrap 15:03ed24574806 256
emilmont 9:4757a976148d 257 ret = sock.receive(buffer, sizeof(buffer)-1);
emilmont 9:4757a976148d 258 buffer[ret] = '\0';
wakestrap 15:03ed24574806 259 if(ret > 0)
wakestrap 15:03ed24574806 260 {
wakestrap 15:03ed24574806 261 int CurrentState = 0;
wakestrap 15:03ed24574806 262
wakestrap 15:03ed24574806 263 for (int x = 0; x < sizeof(buffer)-1; x++)
wakestrap 15:03ed24574806 264 {
wakestrap 15:03ed24574806 265 switch (CurrentState)
wakestrap 15:03ed24574806 266 {
wakestrap 15:03ed24574806 267 case 0: //HEADER 1
wakestrap 15:03ed24574806 268 if (buffer[x] == 0xBE) CurrentState = 1;
wakestrap 15:03ed24574806 269 break;
wakestrap 15:03ed24574806 270 case 1:
wakestrap 15:03ed24574806 271 if (buffer[x] == 0xEF) CurrentState = 2;
wakestrap 15:03ed24574806 272 else CurrentState = 0;
wakestrap 15:03ed24574806 273 break;
wakestrap 15:03ed24574806 274 case 2: //HEADER 2
wakestrap 15:03ed24574806 275 CurrentPacketLength = (int)buffer[x];
wakestrap 15:03ed24574806 276 checksum = CurrentPacketLength;
wakestrap 15:03ed24574806 277 CurrentBytesRead = 0;
wakestrap 15:03ed24574806 278 CurrentState = 3;
wakestrap 15:03ed24574806 279 break;
wakestrap 15:03ed24574806 280 case 3: //PAYLOAD SIZE
wakestrap 15:03ed24574806 281 PayloadBuffer[CurrentBytesRead] = buffer[x];
wakestrap 15:03ed24574806 282 CurrentBytesRead++;
wakestrap 15:03ed24574806 283 if (CurrentBytesRead == CurrentPacketLength)
wakestrap 15:03ed24574806 284 {
wakestrap 15:03ed24574806 285 CurrentState = 4;
wakestrap 15:03ed24574806 286 }
wakestrap 15:03ed24574806 287 break;
wakestrap 15:03ed24574806 288 case 4://CHECKSUM
wakestrap 15:03ed24574806 289 for (int i = 0; i < CurrentPacketLength; i++)
wakestrap 15:03ed24574806 290 {
wakestrap 15:03ed24574806 291 checksum ^= PayloadBuffer[i];
wakestrap 15:03ed24574806 292 }
wakestrap 15:03ed24574806 293 if(buffer[x] == checksum)
wakestrap 15:03ed24574806 294 {
wakestrap 15:03ed24574806 295 #if DEBUG
wakestrap 15:03ed24574806 296 printf("Checksum Success. Payload recieved. \r\n");
wakestrap 15:03ed24574806 297 #endif
wakestrap 15:03ed24574806 298 }
wakestrap 15:03ed24574806 299 else printf("Checksum Failed! Sorry. \r\n");
wakestrap 15:03ed24574806 300 CurrentState = 0;
wakestrap 15:03ed24574806 301 break;
wakestrap 15:03ed24574806 302 }
wakestrap 15:03ed24574806 303 }
wakestrap 15:03ed24574806 304 #if DEBUG
wakestrap 15:03ed24574806 305 printf("Payload ID: %d \r\n", PayloadBuffer[0]);
wakestrap 15:03ed24574806 306 #endif
wakestrap 15:03ed24574806 307 if(PayloadBuffer[0] == 0)
wakestrap 15:03ed24574806 308 {
wakestrap 15:03ed24574806 309
wakestrap 15:03ed24574806 310 }
wakestrap 15:03ed24574806 311 if(PayloadBuffer[0] == 1)
wakestrap 15:03ed24574806 312 {
wakestrap 15:03ed24574806 313 unpackVesselData(PayloadBuffer, CurrentPacketLength);
wakestrap 15:03ed24574806 314 #if OUTPUTENABLE
wakestrap 15:03ed24574806 315 printf("AP %f \n\r", AP);
wakestrap 15:03ed24574806 316 #endif
wakestrap 15:03ed24574806 317 printf("00000%s",PayloadBuffer);
wakestrap 15:03ed24574806 318 printf("CCCCC%c",checksum);
wakestrap 15:03ed24574806 319 //sock.send(HSPacket, sizeof(HSPacket));
wakestrap 15:03ed24574806 320 }
wakestrap 15:03ed24574806 321
wakestrap 15:03ed24574806 322 else
wakestrap 15:03ed24574806 323 {
wakestrap 15:03ed24574806 324
wakestrap 15:03ed24574806 325 }
wakestrap 15:03ed24574806 326
wakestrap 15:03ed24574806 327 //55 Combined byte for navball target mode and SAS mode
wakestrap 15:03ed24574806 328 // First four bits indicate AutoPilot mode:
wakestrap 15:03ed24574806 329 // 0 SAS is off //1 = Regular Stability Assist //2 = Prograde
wakestrap 15:03ed24574806 330 // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
wakestrap 15:03ed24574806 331 // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
wakestrap 15:03ed24574806 332 // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
wakestrap 15:03ed24574806 333 char MainControls = 0b11111111; //SAS RCS Lights Gear Brakes Precision Abort Stage
wakestrap 15:03ed24574806 334 char Mode = 0b00000000; //0 = stage, 1 = docking, 2 = map (Bit 0-3)
wakestrap 15:03ed24574806 335 //0 = Auto, 1 = Free, 2 = Orbital, 3 = Chase, 4 = Locked (Bit 4-7)
wakestrap 15:03ed24574806 336 uint16_t ControlGroup = 0b00000000; //control groups 1-10 in 2 bytes
wakestrap 15:03ed24574806 337 char NavballSASMode = press; //AutoPilot mode (See above for AutoPilot modes)(Ignored if the equal to zero or out of bounds (>10)) //Navball mode
wakestrap 15:03ed24574806 338 short Throttle = 500;
wakestrap 15:03ed24574806 339 if(press)
wakestrap 15:03ed24574806 340 {
wakestrap 15:03ed24574806 341 NavballSASMode = 0b01000000;
wakestrap 15:03ed24574806 342 Throttle = 0;
wakestrap 15:03ed24574806 343 printf("PRESS\n\r\n");
wakestrap 15:03ed24574806 344 }
wakestrap 15:03ed24574806 345 else
wakestrap 15:03ed24574806 346 {
wakestrap 15:03ed24574806 347 NavballSASMode = 0b00000000;
wakestrap 15:03ed24574806 348 }
wakestrap 15:03ed24574806 349
wakestrap 15:03ed24574806 350
wakestrap 15:03ed24574806 351 char AdditionalControlByte1 = 0; //Bit 0: Open Menu, Bit 1: Open Map
wakestrap 15:03ed24574806 352 short Pitch = 0; //-1000 -> 1000
wakestrap 15:03ed24574806 353 short Roll = 0; //-1000 -> 1000
wakestrap 15:03ed24574806 354 short Yaw = 0; //-1000 -> 1000
wakestrap 15:03ed24574806 355 short TX = 0; //-1000 -> 1000
wakestrap 15:03ed24574806 356 short TY = 0; //-1000 -> 1000
wakestrap 15:03ed24574806 357 short TZ = 0; //-1000 -> 1000
wakestrap 15:03ed24574806 358 short WheelSteer = 0; //-1000 -> 1000
wakestrap 15:03ed24574806 359 //short Throttle = 500; // 0 -> 1000
wakestrap 15:03ed24574806 360 short WheelThrottle = 0; // 0 -> 1000
wakestrap 15:03ed24574806 361 char vesselSync = PayloadBuffer[sizeof(PayloadBuffer)-1];
wakestrap 15:03ed24574806 362 char vSync = vesselSync + 30;
wakestrap 15:03ed24574806 363 printf("Vsync %c \n\r\n", vSync);
wakestrap 15:03ed24574806 364 char CSPayload[26];
wakestrap 15:03ed24574806 365 char CSPacket[sizeof(CSPayload)+4];
wakestrap 15:03ed24574806 366 Header1 = 0xBE;
wakestrap 15:03ed24574806 367 Header2 = 0xEF;
wakestrap 15:03ed24574806 368 checksum = sizeof(CSPayload);
wakestrap 15:03ed24574806 369
wakestrap 15:03ed24574806 370 CSPacket[0] = Header1;
wakestrap 15:03ed24574806 371 CSPacket[1] = Header2;
wakestrap 15:03ed24574806 372 CSPacket[2] = sizeof(CSPayload);
wakestrap 15:03ed24574806 373
wakestrap 15:03ed24574806 374 CSPayload[0] = 101; //Csid 101
wakestrap 15:03ed24574806 375 CSPayload[1] = MainControls;
wakestrap 15:03ed24574806 376 CSPayload[2] = Mode;
wakestrap 15:03ed24574806 377 CSPayload[3] = ControlGroup & 0xff;
wakestrap 15:03ed24574806 378 CSPayload[4] = (ControlGroup >> 8);
wakestrap 15:03ed24574806 379 CSPayload[5] = NavballSASMode;
wakestrap 15:03ed24574806 380 CSPayload[6] = AdditionalControlByte1;
wakestrap 15:03ed24574806 381 CSPayload[7] = Pitch & 0xff;
wakestrap 15:03ed24574806 382 CSPayload[8] = (Pitch >> 8);
wakestrap 15:03ed24574806 383 CSPayload[9] = Roll & 0xff;
wakestrap 15:03ed24574806 384 CSPayload[10] = (Roll >> 8);
wakestrap 15:03ed24574806 385 CSPayload[11] = Yaw & 0xff;
wakestrap 15:03ed24574806 386 CSPayload[12] = (Yaw >> 8);
wakestrap 15:03ed24574806 387 CSPayload[13] = TX & 0xff;
wakestrap 15:03ed24574806 388 CSPayload[14] = (TX >> 8);
wakestrap 15:03ed24574806 389 CSPayload[15] = TY & 0xff;
wakestrap 15:03ed24574806 390 CSPayload[16] = (TY >> 8);
wakestrap 15:03ed24574806 391 CSPayload[17] = TZ & 0xff;
wakestrap 15:03ed24574806 392 CSPayload[18] = (TZ >> 8);
wakestrap 15:03ed24574806 393 CSPayload[19] = WheelSteer & 0xff;
wakestrap 15:03ed24574806 394 CSPayload[20] = (WheelSteer >> 8);
wakestrap 15:03ed24574806 395 CSPayload[21] = Throttle & 0xff;
wakestrap 15:03ed24574806 396 CSPayload[22] = (Throttle >> 8);
wakestrap 15:03ed24574806 397 CSPayload[23] = WheelThrottle & 0xff;
wakestrap 15:03ed24574806 398 CSPayload[24] = (WheelThrottle >> 8);
wakestrap 15:03ed24574806 399 CSPayload[25] = vesselSync;
wakestrap 15:03ed24574806 400
wakestrap 15:03ed24574806 401 for (int i = 0; i < sizeof(CSPayload); i++)
wakestrap 15:03ed24574806 402 {
wakestrap 15:03ed24574806 403 checksum ^= CSPayload[i];
wakestrap 15:03ed24574806 404 CSPacket[i+3] = CSPayload[i];
wakestrap 15:03ed24574806 405 }
wakestrap 15:03ed24574806 406 CSPacket[sizeof(CSPacket)-1] = checksum;
wakestrap 15:03ed24574806 407 sock.send_all(CSPacket, sizeof(CSPacket));
wakestrap 15:03ed24574806 408
wakestrap 15:03ed24574806 409
wakestrap 15:03ed24574806 410 }
wakestrap 15:03ed24574806 411 // sock.send_all(HSPacket, sizeof(HSPacket));
emilmont 7:65188f4a8c25 412 }
wakestrap 15:03ed24574806 413
wakestrap 15:03ed24574806 414 printf("We got shut down! Socket Closed!");
emilmont 7:65188f4a8c25 415 sock.close();
donatien 0:bb128f0e952f 416
emilmont 7:65188f4a8c25 417 eth.disconnect();
donatien 5:01f6c3e112af 418
emilmont 9:4757a976148d 419 while(1) {}
wakestrap 15:03ed24574806 420 }