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@16:0eda62f0d9e8, 2020-03-15 (annotated)

Committer:
wakestrap
Date:
Sun Mar 15 04:35:51 2020 +0000
Revision:
16:0eda62f0d9e8
Parent:
15:03ed24574806
Child:
17:00d66e5edf58
Working V0.1. All Comms Works. No external controls except button press.

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 16:0eda62f0d9e8 5
wakestrap 15:03ed24574806 6 /* ---------------------------------------------------------------------------------------- */
wakestrap 15:03ed24574806 7
wakestrap 15:03ed24574806 8 #define PORT 2342
wakestrap 16:0eda62f0d9e8 9 #define DEBUG 0
wakestrap 15:03ed24574806 10
wakestrap 16:0eda62f0d9e8 11 #define OUTPUTENABLE 0
wakestrap 15:03ed24574806 12
wakestrap 15:03ed24574806 13 #if DEBUG
wakestrap 15:03ed24574806 14
wakestrap 15:03ed24574806 15 #endif
wakestrap 16:0eda62f0d9e8 16
wakestrap 16:0eda62f0d9e8 17 //pins for LEDs
wakestrap 16:0eda62f0d9e8 18 #define GLED 5
wakestrap 16:0eda62f0d9e8 19 #define YLED 6
wakestrap 16:0eda62f0d9e8 20 #define RLED 7
wakestrap 16:0eda62f0d9e8 21 #define SASLED 11
wakestrap 16:0eda62f0d9e8 22 #define RCSLED 12
wakestrap 16:0eda62f0d9e8 23 #define CG1LED 13
wakestrap 16:0eda62f0d9e8 24
wakestrap 16:0eda62f0d9e8 25 //pins for input
wakestrap 16:0eda62f0d9e8 26 #define SASPIN 8
wakestrap 16:0eda62f0d9e8 27 #define RCSPIN 9
wakestrap 16:0eda62f0d9e8 28 #define CG1PIN 10
wakestrap 16:0eda62f0d9e8 29 #define THROTTLEPIN 0
wakestrap 16:0eda62f0d9e8 30
wakestrap 16:0eda62f0d9e8 31 #define THROTTLEDB 4 //Throttle axis deadband
wakestrap 16:0eda62f0d9e8 32
wakestrap 16:0eda62f0d9e8 33 //Input enums
wakestrap 16:0eda62f0d9e8 34 #define SAS 7
wakestrap 16:0eda62f0d9e8 35 #define RCS 6
wakestrap 16:0eda62f0d9e8 36 #define LIGHTS 5
wakestrap 16:0eda62f0d9e8 37 #define GEAR 4
wakestrap 16:0eda62f0d9e8 38 #define BRAKES 3
wakestrap 16:0eda62f0d9e8 39 #define PRECISION 2
wakestrap 16:0eda62f0d9e8 40 #define ABORT 1
wakestrap 16:0eda62f0d9e8 41 #define STAGE 0
wakestrap 15:03ed24574806 42
wakestrap 16:0eda62f0d9e8 43 //Action group statuses
wakestrap 16:0eda62f0d9e8 44 #define AGSAS 0
wakestrap 16:0eda62f0d9e8 45 #define AGRCS 1
wakestrap 16:0eda62f0d9e8 46 #define AGLight 2
wakestrap 16:0eda62f0d9e8 47 #define AGGear 3
wakestrap 16:0eda62f0d9e8 48 #define AGBrakes 4
wakestrap 16:0eda62f0d9e8 49 #define AGAbort 5
wakestrap 16:0eda62f0d9e8 50 #define AGCustom01 6
wakestrap 16:0eda62f0d9e8 51 #define AGCustom02 7
wakestrap 16:0eda62f0d9e8 52 #define AGCustom03 8
wakestrap 16:0eda62f0d9e8 53 #define AGCustom04 9
wakestrap 16:0eda62f0d9e8 54 #define AGCustom05 10
wakestrap 16:0eda62f0d9e8 55 #define AGCustom06 11
wakestrap 16:0eda62f0d9e8 56 #define AGCustom07 12
wakestrap 16:0eda62f0d9e8 57 #define AGCustom08 13
wakestrap 16:0eda62f0d9e8 58 #define AGCustom09 14
wakestrap 16:0eda62f0d9e8 59 #define AGCustom10 15
wakestrap 16:0eda62f0d9e8 60
wakestrap 16:0eda62f0d9e8 61 //SAS Modes
wakestrap 16:0eda62f0d9e8 62 #define SMOFF 0
wakestrap 16:0eda62f0d9e8 63 #define SMSAS 1
wakestrap 16:0eda62f0d9e8 64 #define SMPrograde 2
wakestrap 16:0eda62f0d9e8 65 #define SMRetroGrade 3
wakestrap 16:0eda62f0d9e8 66 #define SMNormal 4
wakestrap 16:0eda62f0d9e8 67 #define SMAntinormal 5
wakestrap 16:0eda62f0d9e8 68 #define SMRadialIn 6
wakestrap 16:0eda62f0d9e8 69 #define SMRadialOut 7
wakestrap 16:0eda62f0d9e8 70 #define SMTarget 8
wakestrap 16:0eda62f0d9e8 71 #define SMAntiTarget 9
wakestrap 16:0eda62f0d9e8 72 #define SMManeuverNode 10
wakestrap 16:0eda62f0d9e8 73
wakestrap 16:0eda62f0d9e8 74 //Navball Target Modes
wakestrap 16:0eda62f0d9e8 75 #define NAVBallIGNORE 0
wakestrap 16:0eda62f0d9e8 76 #define NAVBallORBIT 1
wakestrap 16:0eda62f0d9e8 77 #define NAVBallSURFACE 2
wakestrap 16:0eda62f0d9e8 78 #define NAVBallTARGET 3
wakestrap 16:0eda62f0d9e8 79
wakestrap 16:0eda62f0d9e8 80 //macro
wakestrap 16:0eda62f0d9e8 81 #define details(name) (uint8_t*)&name,sizeof(name)
wakestrap 16:0eda62f0d9e8 82
wakestrap 16:0eda62f0d9e8 83 //if no message received from KSP for more than 2s, go idle
wakestrap 16:0eda62f0d9e8 84 #define IDLETIMER 8000
wakestrap 16:0eda62f0d9e8 85 #define CONTROLREFRESH 25
wakestrap 16:0eda62f0d9e8 86
wakestrap 16:0eda62f0d9e8 87 //warnings
wakestrap 16:0eda62f0d9e8 88 #define GWARN 9 //9G Warning
wakestrap 16:0eda62f0d9e8 89 #define GCAUTION 5 //5G Caution
wakestrap 16:0eda62f0d9e8 90 #define FUELCAUTION 10.0 //10% Fuel Caution
wakestrap 16:0eda62f0d9e8 91 #define FUELWARN 5.0 //5% Fuel warning
wakestrap 15:03ed24574806 92
wakestrap 16:0eda62f0d9e8 93 void Indicators();
wakestrap 16:0eda62f0d9e8 94 void initLEDS();
wakestrap 16:0eda62f0d9e8 95 void LEDSAllOff();
wakestrap 16:0eda62f0d9e8 96 void InitTxPackets();
wakestrap 16:0eda62f0d9e8 97 int input();
wakestrap 16:0eda62f0d9e8 98 char ControlStatus(char n);
wakestrap 16:0eda62f0d9e8 99 void output();
wakestrap 16:0eda62f0d9e8 100 void controls();
wakestrap 16:0eda62f0d9e8 101 void controlsInit();
wakestrap 16:0eda62f0d9e8 102 char getSASMode();
wakestrap 16:0eda62f0d9e8 103 char getNavballMode();
wakestrap 16:0eda62f0d9e8 104 void setSASMode(char m);
wakestrap 16:0eda62f0d9e8 105 void setNavballMode(char m);
wakestrap 16:0eda62f0d9e8 106 void MainControls(char n, bool s);
wakestrap 16:0eda62f0d9e8 107 void ControlGroups(char n, bool s);
wakestrap 16:0eda62f0d9e8 108 bool KSPBoardReceiveData();
wakestrap 16:0eda62f0d9e8 109 void KSPBoardSendData(uint8_t * data, uint8_t len);
wakestrap 16:0eda62f0d9e8 110 void Handshake();
wakestrap 16:0eda62f0d9e8 111
wakestrap 16:0eda62f0d9e8 112 DigitalIn ControlButton(USER_BUTTON);
wakestrap 16:0eda62f0d9e8 113
wakestrap 16:0eda62f0d9e8 114 int deadtime, deadtimeOld, controlTime, controlTimeOld;
wakestrap 16:0eda62f0d9e8 115 int now;
wakestrap 16:0eda62f0d9e8 116 Timer t;
wakestrap 16:0eda62f0d9e8 117 bool Connected = false;
wakestrap 16:0eda62f0d9e8 118 char VesselID = 0;
wakestrap 16:0eda62f0d9e8 119 char caution = 0, warning = 0, id;
wakestrap 16:0eda62f0d9e8 120
wakestrap 16:0eda62f0d9e8 121 uint8_t rx_len;
wakestrap 16:0eda62f0d9e8 122 uint16_t * address;
wakestrap 16:0eda62f0d9e8 123 char buffer[256]; //address for temporary storage and parsing buffer
wakestrap 16:0eda62f0d9e8 124 uint8_t structSize;
wakestrap 16:0eda62f0d9e8 125 uint8_t rx_array_inx; //index for RX parsing buffer
wakestrap 16:0eda62f0d9e8 126 uint8_t calc_CS; //calculated Chacksum
wakestrap 16:0eda62f0d9e8 127 char payloadBuffer[255];
wakestrap 15:03ed24574806 128
wakestrap 16:0eda62f0d9e8 129 struct VesselData
wakestrap 15:03ed24574806 130 {
wakestrap 16:0eda62f0d9e8 131 char id; //1
wakestrap 16:0eda62f0d9e8 132 float AP; //2
wakestrap 16:0eda62f0d9e8 133 float PE; //3
wakestrap 16:0eda62f0d9e8 134 float SemiMajorAxis; //4
wakestrap 16:0eda62f0d9e8 135 float SemiMinorAxis; //5
wakestrap 16:0eda62f0d9e8 136 float VVI; //6
wakestrap 16:0eda62f0d9e8 137 float e; //7
wakestrap 16:0eda62f0d9e8 138 float inc; //8
wakestrap 16:0eda62f0d9e8 139 float G; //9
wakestrap 16:0eda62f0d9e8 140 int TAp; //10
wakestrap 16:0eda62f0d9e8 141 int TPe; //11 41
wakestrap 16:0eda62f0d9e8 142 float TrueAnomaly; //12
wakestrap 16:0eda62f0d9e8 143 float Density; //13
wakestrap 16:0eda62f0d9e8 144 int period; //14
wakestrap 16:0eda62f0d9e8 145 float RAlt; //15
wakestrap 16:0eda62f0d9e8 146 float Alt; //16 61
wakestrap 16:0eda62f0d9e8 147 float Vsurf; //17
wakestrap 16:0eda62f0d9e8 148 float Lat; //18
wakestrap 16:0eda62f0d9e8 149 float Lon; //19
wakestrap 16:0eda62f0d9e8 150 float LiquidFuelTot; //20
wakestrap 16:0eda62f0d9e8 151 float LiquidFuel; //21
wakestrap 16:0eda62f0d9e8 152 float OxidizerTot; //22
wakestrap 16:0eda62f0d9e8 153 float Oxidizer; //23
wakestrap 16:0eda62f0d9e8 154 float EChargeTot; //24
wakestrap 16:0eda62f0d9e8 155 float ECharge; //25
wakestrap 16:0eda62f0d9e8 156 float MonoPropTot; //26
wakestrap 16:0eda62f0d9e8 157 float MonoProp; //27
wakestrap 16:0eda62f0d9e8 158 float IntakeAirTot; //28
wakestrap 16:0eda62f0d9e8 159 float IntakeAir; //29
wakestrap 16:0eda62f0d9e8 160 float SolidFuelTot; //30
wakestrap 16:0eda62f0d9e8 161 float SolidFuel; //31
wakestrap 16:0eda62f0d9e8 162 float XenonGasTot; //32
wakestrap 16:0eda62f0d9e8 163 float XenonGas; //33
wakestrap 16:0eda62f0d9e8 164 float LiquidFuelTotS; //34
wakestrap 16:0eda62f0d9e8 165 float LiquidFuelS; //35
wakestrap 16:0eda62f0d9e8 166 float OxidizerTotS; //36
wakestrap 16:0eda62f0d9e8 167 float OxidizerS; //37
wakestrap 16:0eda62f0d9e8 168 uint32_t MissionTime; //38
wakestrap 16:0eda62f0d9e8 169 float deltaTime; //39
wakestrap 16:0eda62f0d9e8 170 float VOrbit; //40
wakestrap 16:0eda62f0d9e8 171 uint32_t MNTime; //41
wakestrap 16:0eda62f0d9e8 172 float MNDeltaV; //42 165
wakestrap 16:0eda62f0d9e8 173 uint16_t Pitch; //43
wakestrap 16:0eda62f0d9e8 174 uint16_t Roll; //44
wakestrap 16:0eda62f0d9e8 175 uint16_t Heading; //45
wakestrap 16:0eda62f0d9e8 176 uint16_t ActionGroups; //46 173 status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10
wakestrap 16:0eda62f0d9e8 177 char SOINumber; //47 SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
wakestrap 16:0eda62f0d9e8 178 char MaxOverHeat; //48 175 Max part overheat (% percent)
wakestrap 16:0eda62f0d9e8 179 float MachNumber; //49
wakestrap 16:0eda62f0d9e8 180 float IAS; //50 Indicated Air Speed
wakestrap 16:0eda62f0d9e8 181 char CurrentStage; //51 Current stage number
wakestrap 16:0eda62f0d9e8 182 char TotalStage; //52 185 TotalNumber of stages
wakestrap 16:0eda62f0d9e8 183 float TargetDist; //53 Distance to targeted vessel (m)
wakestrap 16:0eda62f0d9e8 184 float TargetV; //54 193 Target vessel relative velocity (m/s)
wakestrap 16:0eda62f0d9e8 185 char NavballSASMode; //55 194 Combined char for navball target mode and SAS mode
wakestrap 16:0eda62f0d9e8 186 // First four bits indicate AutoPilot mode:
wakestrap 16:0eda62f0d9e8 187 // 0 SAS is off //1 = Regular Stability Assist //2 = Prograde
wakestrap 16:0eda62f0d9e8 188 // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
wakestrap 16:0eda62f0d9e8 189 // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
wakestrap 16:0eda62f0d9e8 190 // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
wakestrap 16:0eda62f0d9e8 191 uint16_t ProgradePitch; //56 Pitch Of the Prograde Vector; int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees));
wakestrap 16:0eda62f0d9e8 192 uint16_t ProgradeHeading;//57 Heading Of the Prograde Vector; see above for range (Prograde vector depends on navball mode, eg Surface/Orbit/Target)
wakestrap 16:0eda62f0d9e8 193 uint16_t ManeuverPitch; //58 Pitch Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 16:0eda62f0d9e8 194 uint16_t ManeuverHeading;//59 Heading Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 16:0eda62f0d9e8 195 uint16_t TargetPitch; //60 Pitch Of the Target Vector; see above for range; (0 if no Target)
wakestrap 16:0eda62f0d9e8 196 uint16_t TargetHeading; //61 Heading Of the Target Vector; see above for range; (0 if no Target)
wakestrap 16:0eda62f0d9e8 197 uint16_t NormalHeading; //62 Heading Of the Prograde Vector; see above for range; (Pitch of the Heading Vector is always 0)
wakestrap 16:0eda62f0d9e8 198 char vesselSync; //63 208 Starting with 1, increased on every VesselChange
wakestrap 16:0eda62f0d9e8 199 };
wakestrap 15:03ed24574806 200
wakestrap 15:03ed24574806 201
wakestrap 16:0eda62f0d9e8 202 struct HandShakePacket
wakestrap 16:0eda62f0d9e8 203 {
wakestrap 16:0eda62f0d9e8 204 char id;
wakestrap 16:0eda62f0d9e8 205 char M1;
wakestrap 16:0eda62f0d9e8 206 char M2;
wakestrap 16:0eda62f0d9e8 207 char M3;
wakestrap 16:0eda62f0d9e8 208 };
wakestrap 15:03ed24574806 209
wakestrap 16:0eda62f0d9e8 210 struct ControlPacket {
wakestrap 16:0eda62f0d9e8 211 char id;
wakestrap 16:0eda62f0d9e8 212 char MainControls; //SAS RCS Lights Gear Brakes Precision Abort Stage
wakestrap 16:0eda62f0d9e8 213 char Mode; //0 = stage, 1 = docking, 2 = map
wakestrap 16:0eda62f0d9e8 214 short ControlGroup; //control groups 1-10 in 2 chars
wakestrap 16:0eda62f0d9e8 215 char NavballSASMode; //AutoPilot mode
wakestrap 16:0eda62f0d9e8 216 char AdditionalControlByte1; //9
wakestrap 16:0eda62f0d9e8 217 int32_t Pitch; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 218 int32_t Roll; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 219 int32_t Yaw; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 220 int32_t TX; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 221 int32_t TY; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 222 int32_t TZ; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 223 int32_t WheelSteer; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 224 int32_t Throttle; // 0 -> 1000
wakestrap 16:0eda62f0d9e8 225 int32_t WheelThrottle; // 0 -> 1000
wakestrap 16:0eda62f0d9e8 226 char vesselSync; //Starting with 1, increased on every VesselChange
wakestrap 16:0eda62f0d9e8 227 };
wakestrap 16:0eda62f0d9e8 228
wakestrap 16:0eda62f0d9e8 229 HandShakePacket HPacket;
wakestrap 16:0eda62f0d9e8 230 VesselData VData;
wakestrap 16:0eda62f0d9e8 231 ControlPacket CPacket;
wakestrap 16:0eda62f0d9e8 232 EthernetInterface eth;
wakestrap 16:0eda62f0d9e8 233 TCPSocketConnection sock;
wakestrap 16:0eda62f0d9e8 234
donatien 0:bb128f0e952f 235
emilmont 7:65188f4a8c25 236 int main() {
wakestrap 16:0eda62f0d9e8 237 //SETUP
wakestrap 16:0eda62f0d9e8 238
wakestrap 16:0eda62f0d9e8 239 t.start();
wakestrap 16:0eda62f0d9e8 240 printf("\n\r KSP mbedIO V0.1 \n\r");
donatien 0:bb128f0e952f 241 eth.init(); //Use DHCP
donatien 0:bb128f0e952f 242 eth.connect();
wakestrap 15:03ed24574806 243 printf("\nServer IP Address is %s\n\r", eth.getIPAddress());
wakestrap 15:03ed24574806 244
wakestrap 15:03ed24574806 245 UDPSocket server;
wakestrap 15:03ed24574806 246 server.bind(PORT);
wakestrap 15:03ed24574806 247
wakestrap 15:03ed24574806 248 Endpoint KSPServer;
wakestrap 15:03ed24574806 249 char buffer[256];
wakestrap 15:03ed24574806 250
wakestrap 16:0eda62f0d9e8 251 printf("Waiting for UDP packet...\n\r");
wakestrap 16:0eda62f0d9e8 252 int n = server.receiveFrom(KSPServer, buffer, sizeof(buffer));
wakestrap 16:0eda62f0d9e8 253 buffer[n] = '\0';
wakestrap 16:0eda62f0d9e8 254 #if DEBUG
wakestrap 16:0eda62f0d9e8 255 printf("Received packet from: %s\n\r", KSPServer.get_address());
wakestrap 16:0eda62f0d9e8 256 #endif
wakestrap 15:03ed24574806 257
wakestrap 16:0eda62f0d9e8 258 sock.set_blocking(false, 500); // Set socket to non-blocking and set the timeout for 500ms
donatien 0:bb128f0e952f 259
wakestrap 15:03ed24574806 260 while (sock.connect(KSPServer.get_address(), PORT) < 0) {
wakestrap 15:03ed24574806 261 printf("Unable to connect to KSPServer on Port %d \r\n", PORT);
wakestrap 15:03ed24574806 262 wait(1);
wakestrap 15:03ed24574806 263 }
wakestrap 16:0eda62f0d9e8 264 printf("Connected to KSPServer on Port %d \r\n", PORT);
wakestrap 16:0eda62f0d9e8 265 Handshake();
wakestrap 16:0eda62f0d9e8 266 printf("Handshake sent \r\n");
wakestrap 15:03ed24574806 267
wakestrap 16:0eda62f0d9e8 268
wakestrap 16:0eda62f0d9e8 269 initLEDS();
wakestrap 16:0eda62f0d9e8 270 InitTxPackets();
wakestrap 16:0eda62f0d9e8 271 // controlsInit();
wakestrap 16:0eda62f0d9e8 272
wakestrap 16:0eda62f0d9e8 273 LEDSAllOff();
wakestrap 16:0eda62f0d9e8 274 while(true)
wakestrap 16:0eda62f0d9e8 275 {
wakestrap 16:0eda62f0d9e8 276 input();
wakestrap 16:0eda62f0d9e8 277 output();
wakestrap 16:0eda62f0d9e8 278 }
wakestrap 16:0eda62f0d9e8 279 }
wakestrap 16:0eda62f0d9e8 280
wakestrap 16:0eda62f0d9e8 281
wakestrap 16:0eda62f0d9e8 282
wakestrap 16:0eda62f0d9e8 283 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 284 /////////////////// CONTROLS /////////////////////
wakestrap 16:0eda62f0d9e8 285 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 286
wakestrap 16:0eda62f0d9e8 287 void Indicators() {
wakestrap 16:0eda62f0d9e8 288 caution = 0;
wakestrap 16:0eda62f0d9e8 289 warning = 0;
wakestrap 16:0eda62f0d9e8 290
wakestrap 16:0eda62f0d9e8 291 caution += VData.G > GCAUTION;
wakestrap 16:0eda62f0d9e8 292 warning += VData.G > GWARN;
wakestrap 16:0eda62f0d9e8 293 caution += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELCAUTION;
wakestrap 16:0eda62f0d9e8 294 warning += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELWARN;
wakestrap 16:0eda62f0d9e8 295
wakestrap 16:0eda62f0d9e8 296 if (caution != 0)
wakestrap 16:0eda62f0d9e8 297 {
wakestrap 16:0eda62f0d9e8 298 printf("Yellow! \n\r");
wakestrap 16:0eda62f0d9e8 299 }
wakestrap 16:0eda62f0d9e8 300 else
wakestrap 16:0eda62f0d9e8 301 {
wakestrap 16:0eda62f0d9e8 302 //yel off
wakestrap 16:0eda62f0d9e8 303 }
emilmont 7:65188f4a8c25 304
wakestrap 16:0eda62f0d9e8 305 if (warning != 0)
wakestrap 16:0eda62f0d9e8 306 {
wakestrap 16:0eda62f0d9e8 307 printf("RED! \n\r");
wakestrap 16:0eda62f0d9e8 308 }
wakestrap 16:0eda62f0d9e8 309 else
wakestrap 16:0eda62f0d9e8 310 {
wakestrap 16:0eda62f0d9e8 311 //red off
wakestrap 16:0eda62f0d9e8 312 }
wakestrap 16:0eda62f0d9e8 313 #if OUTPUTENABLE
wakestrap 16:0eda62f0d9e8 314 printf("AP : %f \n\r", VData.AP);
wakestrap 16:0eda62f0d9e8 315 printf("Sync : %d \n\r", VData.vesselSync);
wakestrap 16:0eda62f0d9e8 316 printf("Alt : %f \n\r", VData.Alt);
wakestrap 16:0eda62f0d9e8 317 printf("Vsurf : %f \n\r", VData.Vsurf);
wakestrap 16:0eda62f0d9e8 318 if(ControlStatus(AGSAS)) printf("AGSAS ON \n\r");
wakestrap 16:0eda62f0d9e8 319 if(ControlStatus(AGRCS)) printf("AGRCS ON \n\r");
wakestrap 16:0eda62f0d9e8 320 if(ControlStatus(AGCustom01)) printf("AGCustom1 ON \n\r");
wakestrap 16:0eda62f0d9e8 321 #endif
wakestrap 16:0eda62f0d9e8 322 }
wakestrap 16:0eda62f0d9e8 323
wakestrap 16:0eda62f0d9e8 324 void initLEDS() {
wakestrap 16:0eda62f0d9e8 325
wakestrap 16:0eda62f0d9e8 326 }
wakestrap 16:0eda62f0d9e8 327
wakestrap 16:0eda62f0d9e8 328 void LEDSAllOff() {
wakestrap 16:0eda62f0d9e8 329
wakestrap 16:0eda62f0d9e8 330 }
wakestrap 16:0eda62f0d9e8 331
wakestrap 16:0eda62f0d9e8 332 void InitTxPackets() {
wakestrap 16:0eda62f0d9e8 333 HPacket.id = 0;
wakestrap 16:0eda62f0d9e8 334 CPacket.id = 101;
wakestrap 16:0eda62f0d9e8 335 }
wakestrap 16:0eda62f0d9e8 336
wakestrap 16:0eda62f0d9e8 337
wakestrap 16:0eda62f0d9e8 338 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 339 //////////////////// INPUT /////////////////////
wakestrap 16:0eda62f0d9e8 340 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 341
wakestrap 16:0eda62f0d9e8 342 int input() {
wakestrap 16:0eda62f0d9e8 343 int returnValue = -1;
wakestrap 16:0eda62f0d9e8 344 now = t.read_ms();
wakestrap 16:0eda62f0d9e8 345
wakestrap 16:0eda62f0d9e8 346 if (KSPBoardReceiveData())
wakestrap 16:0eda62f0d9e8 347 {
wakestrap 16:0eda62f0d9e8 348 deadtimeOld = now;
wakestrap 16:0eda62f0d9e8 349 returnValue = id;
wakestrap 16:0eda62f0d9e8 350 switch(id)
wakestrap 16:0eda62f0d9e8 351 {
wakestrap 16:0eda62f0d9e8 352 case 0: //Handshake packet
wakestrap 16:0eda62f0d9e8 353 Handshake();
wakestrap 16:0eda62f0d9e8 354 #if DEBUG
wakestrap 16:0eda62f0d9e8 355 printf("HS Packet recieved\n\r");
wakestrap 16:0eda62f0d9e8 356 #endif
wakestrap 16:0eda62f0d9e8 357 break;
wakestrap 16:0eda62f0d9e8 358 case 1:
wakestrap 16:0eda62f0d9e8 359 VesselID = VData.vesselSync;
wakestrap 16:0eda62f0d9e8 360 Indicators();
wakestrap 16:0eda62f0d9e8 361 #if DEBUG
wakestrap 16:0eda62f0d9e8 362 printf("VD Packet recieved\n\r");
wakestrap 16:0eda62f0d9e8 363 #endif
wakestrap 16:0eda62f0d9e8 364 break;
wakestrap 16:0eda62f0d9e8 365 }
wakestrap 16:0eda62f0d9e8 366 Connected = true;
wakestrap 16:0eda62f0d9e8 367 }
wakestrap 16:0eda62f0d9e8 368
wakestrap 16:0eda62f0d9e8 369 else
wakestrap 16:0eda62f0d9e8 370 { //if no message received for a while, go idle
wakestrap 16:0eda62f0d9e8 371 deadtime = now - deadtimeOld;
wakestrap 16:0eda62f0d9e8 372 if (deadtime > IDLETIMER)
wakestrap 16:0eda62f0d9e8 373 {
wakestrap 16:0eda62f0d9e8 374 deadtimeOld = now;
wakestrap 16:0eda62f0d9e8 375 Connected = false;
wakestrap 16:0eda62f0d9e8 376 LEDSAllOff();
wakestrap 16:0eda62f0d9e8 377 printf("No packets for a while... going Idle \n\r");
wakestrap 16:0eda62f0d9e8 378 }
wakestrap 16:0eda62f0d9e8 379 }
wakestrap 16:0eda62f0d9e8 380
wakestrap 16:0eda62f0d9e8 381 return returnValue;
wakestrap 16:0eda62f0d9e8 382 }
wakestrap 16:0eda62f0d9e8 383
wakestrap 16:0eda62f0d9e8 384 char ControlStatus(char n)
wakestrap 16:0eda62f0d9e8 385 {
wakestrap 16:0eda62f0d9e8 386 return ((VData.ActionGroups >> n) & 1) == 1;
wakestrap 16:0eda62f0d9e8 387 }
wakestrap 16:0eda62f0d9e8 388
wakestrap 16:0eda62f0d9e8 389 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 390 ///////////////////// OUTPUT /////////////////////
wakestrap 16:0eda62f0d9e8 391 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 392
wakestrap 16:0eda62f0d9e8 393 void output() {
wakestrap 16:0eda62f0d9e8 394 now = t.read_ms();
wakestrap 16:0eda62f0d9e8 395 controlTime = now - controlTimeOld;
wakestrap 16:0eda62f0d9e8 396 if (controlTime > CONTROLREFRESH) {
wakestrap 16:0eda62f0d9e8 397 controlTimeOld = now;
wakestrap 16:0eda62f0d9e8 398 controls();
wakestrap 16:0eda62f0d9e8 399 }
wakestrap 16:0eda62f0d9e8 400 }
wakestrap 16:0eda62f0d9e8 401
wakestrap 16:0eda62f0d9e8 402 void controls() {
wakestrap 16:0eda62f0d9e8 403 if (Connected) {
wakestrap 16:0eda62f0d9e8 404
wakestrap 16:0eda62f0d9e8 405 if (!ControlButton) { //--------- This is how you do main controls
wakestrap 16:0eda62f0d9e8 406 MainControls(SAS, 1);
wakestrap 16:0eda62f0d9e8 407 setSASMode(SMSAS); //setting SAS mode
wakestrap 16:0eda62f0d9e8 408 #if DEBUG
wakestrap 16:0eda62f0d9e8 409 printf("SAS PRESS \n\r");
wakestrap 16:0eda62f0d9e8 410 #endif
wakestrap 16:0eda62f0d9e8 411 //setNavballMode(NAVBallSURFACE); //setting navball mode
wakestrap 16:0eda62f0d9e8 412 }
wakestrap 16:0eda62f0d9e8 413 else {
wakestrap 16:0eda62f0d9e8 414 //setNavballMode(NAVBallTARGET);
wakestrap 16:0eda62f0d9e8 415 MainControls(SAS, 0);
wakestrap 16:0eda62f0d9e8 416 }
wakestrap 16:0eda62f0d9e8 417
wakestrap 16:0eda62f0d9e8 418 if (ControlButton)
wakestrap 16:0eda62f0d9e8 419 MainControls(RCS, 1);
wakestrap 16:0eda62f0d9e8 420 else
wakestrap 16:0eda62f0d9e8 421 MainControls(RCS, 0);
wakestrap 16:0eda62f0d9e8 422
wakestrap 16:0eda62f0d9e8 423 /**
wakestrap 16:0eda62f0d9e8 424 if (digitalRead(CG1PIN)) //--------- This is how you do control groups
wakestrap 16:0eda62f0d9e8 425 ControlGroups(1, 1);
wakestrap 16:0eda62f0d9e8 426 else
wakestrap 16:0eda62f0d9e8 427 ControlGroups(1, 0);
wakestrap 16:0eda62f0d9e8 428 **/
wakestrap 16:0eda62f0d9e8 429 /*
wakestrap 16:0eda62f0d9e8 430 if (getSASMode() == SMPrograde) { //--------- This is how you read SAS modes
wakestrap 16:0eda62f0d9e8 431 //Blink LED, do stuff, etc.
wakestrap 16:0eda62f0d9e8 432 }
wakestrap 16:0eda62f0d9e8 433
wakestrap 16:0eda62f0d9e8 434 if (getNavballMode() == NAVBallTARGET) { //--------- This is how you read navball modes
wakestrap 16:0eda62f0d9e8 435 //Blink LED, do stuff, etc.
wakestrap 16:0eda62f0d9e8 436 }
wakestrap 16:0eda62f0d9e8 437 */
wakestrap 16:0eda62f0d9e8 438
wakestrap 16:0eda62f0d9e8 439 //This is an example of reading analog inputs to an axis, with deadband and limits
wakestrap 16:0eda62f0d9e8 440 //CPacket.Throttle = constrain(map(analogRead(THROTTLEPIN), THROTTLEDB, 1024 - THROTTLEDB, 0, 1000), 0, 1000);
wakestrap 16:0eda62f0d9e8 441
wakestrap 16:0eda62f0d9e8 442 //This is an example of reading analog inputs to an axis, with deadband and limits
wakestrap 16:0eda62f0d9e8 443 //CPacket.Pitch = constrain(map(analogRead(THROTTLEPIN),0,1024,-1000,1000),-1000, 1000);
wakestrap 16:0eda62f0d9e8 444 CPacket.vesselSync = VData.vesselSync;
wakestrap 16:0eda62f0d9e8 445 KSPBoardSendData(details(CPacket));
wakestrap 16:0eda62f0d9e8 446 }
wakestrap 16:0eda62f0d9e8 447 }
wakestrap 16:0eda62f0d9e8 448
wakestrap 16:0eda62f0d9e8 449 void controlsInit() {
wakestrap 16:0eda62f0d9e8 450 /**
wakestrap 16:0eda62f0d9e8 451 pinMode(SASPIN, INPUT_PULLUP);
wakestrap 16:0eda62f0d9e8 452 pinMode(RCSPIN, INPUT_PULLUP);
wakestrap 16:0eda62f0d9e8 453 pinMode(CG1PIN, INPUT_PULLUP);
wakestrap 16:0eda62f0d9e8 454 **/
wakestrap 16:0eda62f0d9e8 455 }
wakestrap 16:0eda62f0d9e8 456
wakestrap 16:0eda62f0d9e8 457 char getSASMode() {
wakestrap 16:0eda62f0d9e8 458 return VData.NavballSASMode & 0b00001111; // leaves alone the lower 4 bits of; all higher bits set to 0.
wakestrap 16:0eda62f0d9e8 459 }
wakestrap 16:0eda62f0d9e8 460
wakestrap 16:0eda62f0d9e8 461 char getNavballMode() {
wakestrap 16:0eda62f0d9e8 462 return VData.NavballSASMode >> 4; // leaves alone the higher 4 bits of; all lower bits set to 0.
wakestrap 16:0eda62f0d9e8 463 }
wakestrap 16:0eda62f0d9e8 464
wakestrap 16:0eda62f0d9e8 465 void setSASMode(char m) {
wakestrap 16:0eda62f0d9e8 466 CPacket.NavballSASMode &= 0b11110000;
wakestrap 16:0eda62f0d9e8 467 CPacket.NavballSASMode += m;
wakestrap 16:0eda62f0d9e8 468 }
wakestrap 16:0eda62f0d9e8 469
wakestrap 16:0eda62f0d9e8 470 void setNavballMode(char m) {
wakestrap 16:0eda62f0d9e8 471 CPacket.NavballSASMode &= 0b00001111;
wakestrap 16:0eda62f0d9e8 472 CPacket.NavballSASMode += m << 4;
wakestrap 16:0eda62f0d9e8 473 }
wakestrap 16:0eda62f0d9e8 474
wakestrap 16:0eda62f0d9e8 475 void MainControls(char n, bool s) {
wakestrap 16:0eda62f0d9e8 476 if (s)
wakestrap 16:0eda62f0d9e8 477 CPacket.MainControls |= (1 << n); // forces nth bit of x to be 1. all other bits left alone.
wakestrap 16:0eda62f0d9e8 478 else
wakestrap 16:0eda62f0d9e8 479 CPacket.MainControls &= ~(1 << n); // forces nth bit of x to be 0. all other bits left alone.
wakestrap 16:0eda62f0d9e8 480 }
wakestrap 16:0eda62f0d9e8 481
wakestrap 16:0eda62f0d9e8 482 void ControlGroups(char n, bool s) {
wakestrap 16:0eda62f0d9e8 483 if (s)
wakestrap 16:0eda62f0d9e8 484 CPacket.ControlGroup |= (1 << n); // forces nth bit of x to be 1. all other bits left alone.
wakestrap 16:0eda62f0d9e8 485 else
wakestrap 16:0eda62f0d9e8 486 CPacket.ControlGroup &= ~(1 << n); // forces nth bit of x to be 0. all other bits left alone.
wakestrap 16:0eda62f0d9e8 487 }
wakestrap 16:0eda62f0d9e8 488
wakestrap 16:0eda62f0d9e8 489
wakestrap 16:0eda62f0d9e8 490
wakestrap 16:0eda62f0d9e8 491
wakestrap 16:0eda62f0d9e8 492 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 493 //////////////// COMMUNICATIONS///////////////////
wakestrap 16:0eda62f0d9e8 494 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 495
wakestrap 16:0eda62f0d9e8 496
wakestrap 16:0eda62f0d9e8 497
wakestrap 16:0eda62f0d9e8 498 bool KSPBoardReceiveData() {
wakestrap 16:0eda62f0d9e8 499 int ret;
wakestrap 15:03ed24574806 500
wakestrap 16:0eda62f0d9e8 501 ret = sock.receive(buffer, sizeof(buffer)-1);
wakestrap 16:0eda62f0d9e8 502
wakestrap 16:0eda62f0d9e8 503 if(ret > 0)
wakestrap 16:0eda62f0d9e8 504 {
wakestrap 16:0eda62f0d9e8 505 int packetCheckState = 0;
wakestrap 16:0eda62f0d9e8 506 #if DEBUG
wakestrap 16:0eda62f0d9e8 507 printf("Packet Rx'd \n\r");
wakestrap 16:0eda62f0d9e8 508 #endif
wakestrap 16:0eda62f0d9e8 509 for(int x = 0; x < 4; x++)
wakestrap 15:03ed24574806 510 {
wakestrap 16:0eda62f0d9e8 511 switch(packetCheckState){
wakestrap 16:0eda62f0d9e8 512 case 0: // CHECK HEADER 1
wakestrap 16:0eda62f0d9e8 513 if(buffer[packetCheckState] == 0xBE)
wakestrap 15:03ed24574806 514 {
wakestrap 16:0eda62f0d9e8 515 packetCheckState++;
wakestrap 16:0eda62f0d9e8 516 }
wakestrap 16:0eda62f0d9e8 517 break;
wakestrap 16:0eda62f0d9e8 518 case 1: // CHECK HEADER 2
wakestrap 16:0eda62f0d9e8 519 if(buffer[packetCheckState] == 0xEF)
wakestrap 16:0eda62f0d9e8 520 {
wakestrap 16:0eda62f0d9e8 521 packetCheckState++;
wakestrap 15:03ed24574806 522 }
wakestrap 16:0eda62f0d9e8 523 break;
wakestrap 16:0eda62f0d9e8 524 case 2: // GET PAYLOAD SIZE
wakestrap 16:0eda62f0d9e8 525 rx_len = buffer[packetCheckState];
wakestrap 16:0eda62f0d9e8 526 packetCheckState++;
wakestrap 16:0eda62f0d9e8 527 #if DEBUG
wakestrap 16:0eda62f0d9e8 528 printf("PL Size at Rx: %d \n\r", rx_len);
wakestrap 16:0eda62f0d9e8 529 #endif
wakestrap 16:0eda62f0d9e8 530 break;
wakestrap 16:0eda62f0d9e8 531 case 3:
wakestrap 16:0eda62f0d9e8 532 id = buffer[packetCheckState];
wakestrap 16:0eda62f0d9e8 533 #if DEBUG
wakestrap 16:0eda62f0d9e8 534 printf("Packet ID: %d \n\r", id);
wakestrap 16:0eda62f0d9e8 535 #endif
wakestrap 16:0eda62f0d9e8 536 if(id == 0)
wakestrap 16:0eda62f0d9e8 537 {
wakestrap 16:0eda62f0d9e8 538 structSize = sizeof(HPacket);
wakestrap 16:0eda62f0d9e8 539 address = (uint16_t*)&HPacket;
wakestrap 16:0eda62f0d9e8 540 }
wakestrap 16:0eda62f0d9e8 541 else if(id == 1)
wakestrap 16:0eda62f0d9e8 542 {
wakestrap 16:0eda62f0d9e8 543 structSize = sizeof(VData);
wakestrap 16:0eda62f0d9e8 544 address = (uint16_t*)&VData;
wakestrap 16:0eda62f0d9e8 545 }
wakestrap 16:0eda62f0d9e8 546 else
wakestrap 16:0eda62f0d9e8 547 {
wakestrap 16:0eda62f0d9e8 548 printf("Bad Packet ID: %d", buffer[packetCheckState]);
wakestrap 16:0eda62f0d9e8 549 packetCheckState = 0;
wakestrap 16:0eda62f0d9e8 550 }
wakestrap 16:0eda62f0d9e8 551 break;
wakestrap 16:0eda62f0d9e8 552 default :
wakestrap 16:0eda62f0d9e8 553 printf("Something went wrong in Packet Rx Loop \r\n");
wakestrap 16:0eda62f0d9e8 554 break;
wakestrap 15:03ed24574806 555 }
wakestrap 16:0eda62f0d9e8 556 }
wakestrap 16:0eda62f0d9e8 557
wakestrap 16:0eda62f0d9e8 558 if(packetCheckState == 3) //We had a valid packet
wakestrap 16:0eda62f0d9e8 559 {
wakestrap 16:0eda62f0d9e8 560 calc_CS = rx_len;
wakestrap 16:0eda62f0d9e8 561 for(int i = 0; i < rx_len; i++)
wakestrap 15:03ed24574806 562 {
wakestrap 16:0eda62f0d9e8 563 payloadBuffer[i] = buffer[i+3];
wakestrap 16:0eda62f0d9e8 564 calc_CS^=buffer[i+3];
wakestrap 15:03ed24574806 565 }
wakestrap 15:03ed24574806 566
wakestrap 16:0eda62f0d9e8 567 if(calc_CS == buffer[rx_len+3]) //CS Check
wakestrap 15:03ed24574806 568 {
wakestrap 16:0eda62f0d9e8 569 char varBuffer[4];
wakestrap 16:0eda62f0d9e8 570 #if DEBUG
wakestrap 16:0eda62f0d9e8 571 printf("Checksum Passed! \n\r");
wakestrap 16:0eda62f0d9e8 572 #endif
wakestrap 16:0eda62f0d9e8 573
wakestrap 16:0eda62f0d9e8 574 // A note to the reader, I hate this as much as you do. I really do.
wakestrap 16:0eda62f0d9e8 575 // But I'm a shitty programmer and this compiler pads structs for alignment and it fucks up the memcpy approach.
wakestrap 16:0eda62f0d9e8 576 // I fought this, I really did. But it's 2AM, and I just want my fucking code to work. So forgive me this sin.
wakestrap 16:0eda62f0d9e8 577
wakestrap 16:0eda62f0d9e8 578 //memcpy(address,payloadBuffer,structSize);
wakestrap 15:03ed24574806 579
wakestrap 16:0eda62f0d9e8 580 VData.id = payloadBuffer[0]; //1
wakestrap 16:0eda62f0d9e8 581 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 582 {
wakestrap 16:0eda62f0d9e8 583 varBuffer[i]=payloadBuffer[i+1];
wakestrap 16:0eda62f0d9e8 584 }
wakestrap 16:0eda62f0d9e8 585 VData.AP = *(float *)&varBuffer;
wakestrap 16:0eda62f0d9e8 586 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 587 {
wakestrap 16:0eda62f0d9e8 588 varBuffer[i]=payloadBuffer[i+5];
wakestrap 16:0eda62f0d9e8 589 }
wakestrap 16:0eda62f0d9e8 590 VData.PE = *(float *)&varBuffer; //3
wakestrap 16:0eda62f0d9e8 591 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 592 {
wakestrap 16:0eda62f0d9e8 593 varBuffer[i]=payloadBuffer[i+9];
wakestrap 16:0eda62f0d9e8 594 }
wakestrap 16:0eda62f0d9e8 595 VData.SemiMajorAxis = *(float *)&varBuffer; //4
wakestrap 16:0eda62f0d9e8 596 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 597 {
wakestrap 16:0eda62f0d9e8 598 varBuffer[i]=payloadBuffer[i+13];
wakestrap 16:0eda62f0d9e8 599 }
wakestrap 16:0eda62f0d9e8 600 VData.SemiMinorAxis = *(float *)&varBuffer; //5
wakestrap 16:0eda62f0d9e8 601 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 602 {
wakestrap 16:0eda62f0d9e8 603 varBuffer[i]=payloadBuffer[i+17];
wakestrap 16:0eda62f0d9e8 604 }
wakestrap 16:0eda62f0d9e8 605 VData.VVI = *(float *)&varBuffer; //6
wakestrap 16:0eda62f0d9e8 606 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 607 {
wakestrap 16:0eda62f0d9e8 608 varBuffer[i]=payloadBuffer[i+21];
wakestrap 16:0eda62f0d9e8 609 }
wakestrap 16:0eda62f0d9e8 610 VData.e = *(float *)&varBuffer; //7
wakestrap 16:0eda62f0d9e8 611 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 612 {
wakestrap 16:0eda62f0d9e8 613 varBuffer[i]=payloadBuffer[i+25];
wakestrap 16:0eda62f0d9e8 614 }
wakestrap 16:0eda62f0d9e8 615 VData.inc = *(float *)&varBuffer; //8
wakestrap 16:0eda62f0d9e8 616 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 617 {
wakestrap 16:0eda62f0d9e8 618 varBuffer[i]=payloadBuffer[i+29];
wakestrap 16:0eda62f0d9e8 619 }
wakestrap 16:0eda62f0d9e8 620 VData.G = *(float *)&varBuffer; //9
wakestrap 16:0eda62f0d9e8 621 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 622 {
wakestrap 16:0eda62f0d9e8 623 varBuffer[i]=payloadBuffer[i+33];
wakestrap 16:0eda62f0d9e8 624 }
wakestrap 16:0eda62f0d9e8 625 VData.TAp = *(int *)&varBuffer; //10
wakestrap 16:0eda62f0d9e8 626 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 627 {
wakestrap 16:0eda62f0d9e8 628 varBuffer[i]=payloadBuffer[i+37];
wakestrap 16:0eda62f0d9e8 629 }
wakestrap 16:0eda62f0d9e8 630 VData.TPe = *(int *)&varBuffer; //11 41
wakestrap 16:0eda62f0d9e8 631 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 632 {
wakestrap 16:0eda62f0d9e8 633 varBuffer[i]=payloadBuffer[i+41];
wakestrap 16:0eda62f0d9e8 634 }
wakestrap 16:0eda62f0d9e8 635 VData.TrueAnomaly = *(float *)&varBuffer; //12
wakestrap 16:0eda62f0d9e8 636 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 637 {
wakestrap 16:0eda62f0d9e8 638 varBuffer[i]=payloadBuffer[i+44];
wakestrap 16:0eda62f0d9e8 639 }
wakestrap 16:0eda62f0d9e8 640 VData.Density = *(float *)&varBuffer; //13
wakestrap 16:0eda62f0d9e8 641 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 642 {
wakestrap 16:0eda62f0d9e8 643 varBuffer[i]=payloadBuffer[i+49];
wakestrap 16:0eda62f0d9e8 644 }
wakestrap 16:0eda62f0d9e8 645 VData.period = *(int *)&varBuffer; //14
wakestrap 16:0eda62f0d9e8 646 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 647 {
wakestrap 16:0eda62f0d9e8 648 varBuffer[i]=payloadBuffer[i+53];
wakestrap 16:0eda62f0d9e8 649 }
wakestrap 16:0eda62f0d9e8 650 VData.RAlt = *(float *)&varBuffer; //15
wakestrap 16:0eda62f0d9e8 651 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 652 {
wakestrap 16:0eda62f0d9e8 653 varBuffer[i]=payloadBuffer[i+57];
wakestrap 16:0eda62f0d9e8 654 }
wakestrap 16:0eda62f0d9e8 655 VData.Alt = *(float *)&varBuffer; //16 61
wakestrap 16:0eda62f0d9e8 656 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 657 {
wakestrap 16:0eda62f0d9e8 658 varBuffer[i]=payloadBuffer[i+61];
wakestrap 16:0eda62f0d9e8 659 }
wakestrap 16:0eda62f0d9e8 660 VData.Vsurf = *(float *)&varBuffer; //17
wakestrap 16:0eda62f0d9e8 661 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 662 {
wakestrap 16:0eda62f0d9e8 663 varBuffer[i]=payloadBuffer[i+65];
wakestrap 16:0eda62f0d9e8 664 }
wakestrap 16:0eda62f0d9e8 665 VData.Lat = *(float *)&varBuffer; //18
wakestrap 16:0eda62f0d9e8 666 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 667 {
wakestrap 16:0eda62f0d9e8 668 varBuffer[i]=payloadBuffer[i+69];
wakestrap 16:0eda62f0d9e8 669 }
wakestrap 16:0eda62f0d9e8 670 VData.Lon = *(float *)&varBuffer; //19
wakestrap 16:0eda62f0d9e8 671 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 672 {
wakestrap 16:0eda62f0d9e8 673 varBuffer[i]=payloadBuffer[i+73];
wakestrap 16:0eda62f0d9e8 674 }
wakestrap 16:0eda62f0d9e8 675 VData.LiquidFuelTot = *(float *)&varBuffer; //20
wakestrap 16:0eda62f0d9e8 676 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 677 {
wakestrap 16:0eda62f0d9e8 678 varBuffer[i]=payloadBuffer[i+77];
wakestrap 16:0eda62f0d9e8 679 }
wakestrap 16:0eda62f0d9e8 680 VData.LiquidFuel = *(float *)&varBuffer; //21
wakestrap 16:0eda62f0d9e8 681 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 682 {
wakestrap 16:0eda62f0d9e8 683 varBuffer[i]=payloadBuffer[i+81];
wakestrap 16:0eda62f0d9e8 684 }
wakestrap 16:0eda62f0d9e8 685 VData.OxidizerTot = *(float *)&varBuffer; //22
wakestrap 16:0eda62f0d9e8 686 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 687 {
wakestrap 16:0eda62f0d9e8 688 varBuffer[i]=payloadBuffer[i+85];
wakestrap 16:0eda62f0d9e8 689 }
wakestrap 16:0eda62f0d9e8 690 VData.Oxidizer = *(float *)&varBuffer; //23
wakestrap 16:0eda62f0d9e8 691 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 692 {
wakestrap 16:0eda62f0d9e8 693 varBuffer[i]=payloadBuffer[i+89];
wakestrap 16:0eda62f0d9e8 694 }
wakestrap 16:0eda62f0d9e8 695 VData.EChargeTot = *(float *)&varBuffer; //24
wakestrap 16:0eda62f0d9e8 696 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 697 {
wakestrap 16:0eda62f0d9e8 698 varBuffer[i]=payloadBuffer[i+93];
wakestrap 16:0eda62f0d9e8 699 }
wakestrap 16:0eda62f0d9e8 700 VData.ECharge = *(float *)&varBuffer; //25
wakestrap 16:0eda62f0d9e8 701 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 702 {
wakestrap 16:0eda62f0d9e8 703 varBuffer[i]=payloadBuffer[i+97];
wakestrap 16:0eda62f0d9e8 704 }
wakestrap 16:0eda62f0d9e8 705 VData.MonoPropTot = *(float *)&varBuffer; //26
wakestrap 16:0eda62f0d9e8 706 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 707 {
wakestrap 16:0eda62f0d9e8 708 varBuffer[i]=payloadBuffer[i+101];
wakestrap 16:0eda62f0d9e8 709 }
wakestrap 16:0eda62f0d9e8 710 VData.MonoProp = *(float *)&varBuffer; //27
wakestrap 16:0eda62f0d9e8 711 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 712 {
wakestrap 16:0eda62f0d9e8 713 varBuffer[i]=payloadBuffer[i+104];
wakestrap 16:0eda62f0d9e8 714 }
wakestrap 16:0eda62f0d9e8 715 VData.IntakeAirTot = *(float *)&varBuffer; //28
wakestrap 16:0eda62f0d9e8 716 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 717 {
wakestrap 16:0eda62f0d9e8 718 varBuffer[i]=payloadBuffer[i+109];
wakestrap 16:0eda62f0d9e8 719 }
wakestrap 16:0eda62f0d9e8 720 VData.IntakeAir = *(float *)&varBuffer; //29
wakestrap 16:0eda62f0d9e8 721 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 722 {
wakestrap 16:0eda62f0d9e8 723 varBuffer[i]=payloadBuffer[i+113];
wakestrap 16:0eda62f0d9e8 724 }
wakestrap 16:0eda62f0d9e8 725 VData.SolidFuelTot = *(float *)&varBuffer; //30
wakestrap 16:0eda62f0d9e8 726 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 727 {
wakestrap 16:0eda62f0d9e8 728 varBuffer[i]=payloadBuffer[i+117];
wakestrap 16:0eda62f0d9e8 729 }
wakestrap 16:0eda62f0d9e8 730 VData.SolidFuel = *(float *)&varBuffer; //31
wakestrap 16:0eda62f0d9e8 731 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 732 {
wakestrap 16:0eda62f0d9e8 733 varBuffer[i]=payloadBuffer[i+121];
wakestrap 16:0eda62f0d9e8 734 }
wakestrap 16:0eda62f0d9e8 735 VData.XenonGasTot = *(float *)&varBuffer; //32
wakestrap 16:0eda62f0d9e8 736 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 737 {
wakestrap 16:0eda62f0d9e8 738 varBuffer[i]=payloadBuffer[i+125];
wakestrap 16:0eda62f0d9e8 739 }
wakestrap 16:0eda62f0d9e8 740 VData.XenonGas = *(float *)&varBuffer; //33
wakestrap 16:0eda62f0d9e8 741 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 742 {
wakestrap 16:0eda62f0d9e8 743 varBuffer[i]=payloadBuffer[i+129];
wakestrap 16:0eda62f0d9e8 744 }
wakestrap 16:0eda62f0d9e8 745 VData.LiquidFuelTotS = *(float *)&varBuffer; //34
wakestrap 16:0eda62f0d9e8 746 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 747 {
wakestrap 16:0eda62f0d9e8 748 varBuffer[i]=payloadBuffer[i+133];
wakestrap 16:0eda62f0d9e8 749 }
wakestrap 16:0eda62f0d9e8 750 VData.LiquidFuelS = *(float *)&varBuffer; //35
wakestrap 16:0eda62f0d9e8 751 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 752 {
wakestrap 16:0eda62f0d9e8 753 varBuffer[i]=payloadBuffer[i+137];
wakestrap 16:0eda62f0d9e8 754 }
wakestrap 16:0eda62f0d9e8 755 VData.OxidizerTotS = *(float *)&varBuffer; //36
wakestrap 16:0eda62f0d9e8 756 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 757 {
wakestrap 16:0eda62f0d9e8 758 varBuffer[i]=payloadBuffer[i+141];
wakestrap 16:0eda62f0d9e8 759 }
wakestrap 16:0eda62f0d9e8 760 VData.OxidizerS = *(float *)&varBuffer; //37
wakestrap 16:0eda62f0d9e8 761 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 762 {
wakestrap 16:0eda62f0d9e8 763 varBuffer[i]=payloadBuffer[i+145];
wakestrap 16:0eda62f0d9e8 764 }
wakestrap 16:0eda62f0d9e8 765 VData.MissionTime = *(uint32_t *)&varBuffer; //38
wakestrap 16:0eda62f0d9e8 766 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 767 {
wakestrap 16:0eda62f0d9e8 768 varBuffer[i]=payloadBuffer[i+149];
wakestrap 16:0eda62f0d9e8 769 }
wakestrap 16:0eda62f0d9e8 770 VData.deltaTime = *(float *)&varBuffer; //39
wakestrap 16:0eda62f0d9e8 771 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 772 {
wakestrap 16:0eda62f0d9e8 773 varBuffer[i]=payloadBuffer[i+153];
wakestrap 16:0eda62f0d9e8 774 }
wakestrap 16:0eda62f0d9e8 775 VData.VOrbit = *(float *)&varBuffer; //40
wakestrap 16:0eda62f0d9e8 776 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 777 {
wakestrap 16:0eda62f0d9e8 778 varBuffer[i]=payloadBuffer[i+157];
wakestrap 16:0eda62f0d9e8 779 }
wakestrap 16:0eda62f0d9e8 780 VData.MNTime = *(uint32_t *)&varBuffer; //41
wakestrap 16:0eda62f0d9e8 781 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 782 {
wakestrap 16:0eda62f0d9e8 783 varBuffer[i]=payloadBuffer[i+161];
wakestrap 16:0eda62f0d9e8 784 }
wakestrap 16:0eda62f0d9e8 785 VData.MNDeltaV = *(float *)&varBuffer; //42 165
wakestrap 16:0eda62f0d9e8 786 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 787 {
wakestrap 16:0eda62f0d9e8 788 varBuffer[i]=payloadBuffer[i+165];
wakestrap 16:0eda62f0d9e8 789 }
wakestrap 16:0eda62f0d9e8 790 VData.Pitch = *(float *)&varBuffer; //43//////////////////////////
wakestrap 16:0eda62f0d9e8 791 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 792 {
wakestrap 16:0eda62f0d9e8 793 varBuffer[i]=payloadBuffer[i+167];
wakestrap 16:0eda62f0d9e8 794 }
wakestrap 16:0eda62f0d9e8 795 VData.Roll = *(float *)&varBuffer; //44
wakestrap 16:0eda62f0d9e8 796 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 797 {
wakestrap 16:0eda62f0d9e8 798 varBuffer[i]=payloadBuffer[i+169];
wakestrap 16:0eda62f0d9e8 799 }
wakestrap 16:0eda62f0d9e8 800 VData.Heading = *(float *)&varBuffer; //45
wakestrap 16:0eda62f0d9e8 801 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 802 {
wakestrap 16:0eda62f0d9e8 803 varBuffer[i]=payloadBuffer[i+173];
wakestrap 16:0eda62f0d9e8 804 }
wakestrap 16:0eda62f0d9e8 805 VData.ActionGroups = *(float *)&varBuffer; //46 173 status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10
wakestrap 16:0eda62f0d9e8 806 VData.SOINumber = payloadBuffer[177]; //47 SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
wakestrap 16:0eda62f0d9e8 807 VData.MaxOverHeat = payloadBuffer[178]; //48 175 Max part overheat (% percent)
wakestrap 16:0eda62f0d9e8 808 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 809 {
wakestrap 16:0eda62f0d9e8 810 varBuffer[i]=payloadBuffer[i+179];
wakestrap 16:0eda62f0d9e8 811 }
wakestrap 16:0eda62f0d9e8 812 VData.MachNumber = *(float *)&varBuffer; //49
wakestrap 16:0eda62f0d9e8 813 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 814 {
wakestrap 16:0eda62f0d9e8 815 varBuffer[i]=payloadBuffer[i+183];
wakestrap 16:0eda62f0d9e8 816 }
wakestrap 16:0eda62f0d9e8 817 VData.IAS = *(float *)&varBuffer; //50 Indicated Air Speed
wakestrap 16:0eda62f0d9e8 818
wakestrap 16:0eda62f0d9e8 819 VData.CurrentStage = payloadBuffer[187]; //51 Current stage number
wakestrap 16:0eda62f0d9e8 820 VData.TotalStage = payloadBuffer[188]; //52 185 TotalNumber of stages
wakestrap 16:0eda62f0d9e8 821 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 822 {
wakestrap 16:0eda62f0d9e8 823 varBuffer[i]=payloadBuffer[i+189];
wakestrap 16:0eda62f0d9e8 824 }
wakestrap 16:0eda62f0d9e8 825 VData.TargetDist = *(float *)&varBuffer; //53 Distance to targeted vessel (m)
wakestrap 16:0eda62f0d9e8 826 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 827 {
wakestrap 16:0eda62f0d9e8 828 varBuffer[i]=payloadBuffer[i+193];
wakestrap 16:0eda62f0d9e8 829 }
wakestrap 16:0eda62f0d9e8 830 VData.TargetV = *(float *)&varBuffer; //54 193 Target vessel relative velocity (m/s)
wakestrap 16:0eda62f0d9e8 831 VData.NavballSASMode = payloadBuffer[197]; //55 194 Combined char for navball target mode and SAS mode
wakestrap 15:03ed24574806 832 // First four bits indicate AutoPilot mode:
wakestrap 15:03ed24574806 833 // 0 SAS is off //1 = Regular Stability Assist //2 = Prograde
wakestrap 15:03ed24574806 834 // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
wakestrap 15:03ed24574806 835 // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
wakestrap 15:03ed24574806 836 // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
wakestrap 16:0eda62f0d9e8 837 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 838 {
wakestrap 16:0eda62f0d9e8 839 varBuffer[i]=payloadBuffer[i+198];
wakestrap 16:0eda62f0d9e8 840 }
wakestrap 16:0eda62f0d9e8 841 VData.ProgradePitch = *(float *)&varBuffer; //56 Pitch Of the Prograde Vector; int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees));
wakestrap 16:0eda62f0d9e8 842 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 843 {
wakestrap 16:0eda62f0d9e8 844 varBuffer[i]=payloadBuffer[i+200];
wakestrap 16:0eda62f0d9e8 845 }
wakestrap 16:0eda62f0d9e8 846 VData.ProgradeHeading = *(float *)&varBuffer;//57 Heading Of the Prograde Vector; see above for range (Prograde vector depends on navball mode, eg Surface/Orbit/Target)
wakestrap 16:0eda62f0d9e8 847 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 848 {
wakestrap 16:0eda62f0d9e8 849 varBuffer[i]=payloadBuffer[i+202];
wakestrap 16:0eda62f0d9e8 850 }
wakestrap 16:0eda62f0d9e8 851 VData.ManeuverPitch = *(float *)&varBuffer; //58 Pitch Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 16:0eda62f0d9e8 852 for(int i=0; i < 4; i++)
wakestrap 16:0eda62f0d9e8 853 {
wakestrap 16:0eda62f0d9e8 854 varBuffer[i]=payloadBuffer[i+204];
wakestrap 16:0eda62f0d9e8 855 }
wakestrap 16:0eda62f0d9e8 856 VData.ManeuverHeading = *(float *)&varBuffer;//59 Heading Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 16:0eda62f0d9e8 857 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 858 {
wakestrap 16:0eda62f0d9e8 859 varBuffer[i]=payloadBuffer[i+205];
wakestrap 16:0eda62f0d9e8 860 }
wakestrap 16:0eda62f0d9e8 861 VData.TargetPitch = *(float *)&varBuffer; //60 Pitch Of the Target Vector; see above for range; (0 if no Target)
wakestrap 16:0eda62f0d9e8 862 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 863 {
wakestrap 16:0eda62f0d9e8 864 varBuffer[i]=payloadBuffer[i+206];
wakestrap 16:0eda62f0d9e8 865 }
wakestrap 16:0eda62f0d9e8 866 VData.TargetHeading = *(float *)&varBuffer; //61 Heading Of the Target Vector; see above for range; (0 if no Target)
wakestrap 16:0eda62f0d9e8 867 for(int i=0; i < 2; i++)
wakestrap 16:0eda62f0d9e8 868 {
wakestrap 16:0eda62f0d9e8 869 varBuffer[i]=payloadBuffer[i+207];
wakestrap 16:0eda62f0d9e8 870 }
wakestrap 16:0eda62f0d9e8 871 VData.NormalHeading = *(float *)&varBuffer; //62 Heading Of the Prograde Vector; see above for range; (Pitch of the Heading Vector is always 0)
wakestrap 16:0eda62f0d9e8 872 VData.vesselSync = payloadBuffer[208];
wakestrap 16:0eda62f0d9e8 873
wakestrap 16:0eda62f0d9e8 874 rx_len = 0;
wakestrap 16:0eda62f0d9e8 875 return true;
wakestrap 15:03ed24574806 876 }
wakestrap 15:03ed24574806 877 else
wakestrap 15:03ed24574806 878 {
wakestrap 16:0eda62f0d9e8 879 //failed checksum, need to clear this out anyway
wakestrap 16:0eda62f0d9e8 880 printf("Failed Checksum! Sorry \n\r");
wakestrap 16:0eda62f0d9e8 881 rx_len = 0;
wakestrap 16:0eda62f0d9e8 882 rx_array_inx = 1;
wakestrap 16:0eda62f0d9e8 883 return false;
wakestrap 15:03ed24574806 884 }
wakestrap 16:0eda62f0d9e8 885
wakestrap 16:0eda62f0d9e8 886
wakestrap 15:03ed24574806 887 }
emilmont 7:65188f4a8c25 888 }
wakestrap 15:03ed24574806 889
wakestrap 16:0eda62f0d9e8 890 return false;
wakestrap 16:0eda62f0d9e8 891 }
wakestrap 16:0eda62f0d9e8 892
wakestrap 16:0eda62f0d9e8 893 void KSPBoardSendData(uint8_t * data, uint8_t len){
wakestrap 16:0eda62f0d9e8 894 uint8_t CS = len;
wakestrap 16:0eda62f0d9e8 895 char fullPacket[len+4];
wakestrap 16:0eda62f0d9e8 896 fullPacket[0] = 0xBE;
wakestrap 16:0eda62f0d9e8 897 fullPacket[1] = 0xEF;
wakestrap 16:0eda62f0d9e8 898 fullPacket[2] = len;
donatien 0:bb128f0e952f 899
wakestrap 16:0eda62f0d9e8 900 if(*(data) == 101)
wakestrap 16:0eda62f0d9e8 901 {
wakestrap 16:0eda62f0d9e8 902 fullPacket[3] = 101; //Csid 101
wakestrap 16:0eda62f0d9e8 903 fullPacket[4] = CPacket.MainControls;
wakestrap 16:0eda62f0d9e8 904 fullPacket[5] = CPacket.Mode;
wakestrap 16:0eda62f0d9e8 905 fullPacket[6] = CPacket.ControlGroup & 0xff;
wakestrap 16:0eda62f0d9e8 906 fullPacket[7] = (CPacket.ControlGroup >> 8);
wakestrap 16:0eda62f0d9e8 907 fullPacket[8] = CPacket.NavballSASMode;
wakestrap 16:0eda62f0d9e8 908 fullPacket[9] = CPacket.AdditionalControlByte1;
wakestrap 16:0eda62f0d9e8 909 fullPacket[10] = CPacket.Pitch & 0xff;
wakestrap 16:0eda62f0d9e8 910 fullPacket[11] = (CPacket.Pitch >> 8);
wakestrap 16:0eda62f0d9e8 911 fullPacket[12] = CPacket.Roll & 0xff;
wakestrap 16:0eda62f0d9e8 912 fullPacket[13] = (CPacket.Roll >> 8);
wakestrap 16:0eda62f0d9e8 913 fullPacket[14] = CPacket.Yaw & 0xff;
wakestrap 16:0eda62f0d9e8 914 fullPacket[15] = (CPacket.Yaw >> 8);
wakestrap 16:0eda62f0d9e8 915 fullPacket[16] = CPacket.TX & 0xff;
wakestrap 16:0eda62f0d9e8 916 fullPacket[17] = (CPacket.TX >> 8);
wakestrap 16:0eda62f0d9e8 917 fullPacket[18] = CPacket.TY & 0xff;
wakestrap 16:0eda62f0d9e8 918 fullPacket[19] = (CPacket.TY >> 8);
wakestrap 16:0eda62f0d9e8 919 fullPacket[20] = CPacket.TZ & 0xff;
wakestrap 16:0eda62f0d9e8 920 fullPacket[21] = (CPacket.TZ >> 8);
wakestrap 16:0eda62f0d9e8 921 fullPacket[22] = CPacket.WheelSteer & 0xff;
wakestrap 16:0eda62f0d9e8 922 fullPacket[23] = (CPacket.WheelSteer >> 8);
wakestrap 16:0eda62f0d9e8 923 fullPacket[24] = CPacket.Throttle & 0xff;
wakestrap 16:0eda62f0d9e8 924 fullPacket[25] = (CPacket.Throttle >> 8);
wakestrap 16:0eda62f0d9e8 925 fullPacket[26] = CPacket.WheelThrottle & 0xff;
wakestrap 16:0eda62f0d9e8 926 fullPacket[27] = (CPacket.WheelThrottle >> 8);
wakestrap 16:0eda62f0d9e8 927 fullPacket[28] = CPacket.vesselSync;
wakestrap 16:0eda62f0d9e8 928 for(int i = 0; i<len; i++)
wakestrap 16:0eda62f0d9e8 929 {
wakestrap 16:0eda62f0d9e8 930 CS^= fullPacket[i+3];
wakestrap 16:0eda62f0d9e8 931 }
wakestrap 16:0eda62f0d9e8 932 fullPacket[sizeof(fullPacket)-1] = CS;
wakestrap 16:0eda62f0d9e8 933 }
wakestrap 16:0eda62f0d9e8 934 else
wakestrap 16:0eda62f0d9e8 935 {
wakestrap 16:0eda62f0d9e8 936 for(int i = 0; i<len; i++)
wakestrap 16:0eda62f0d9e8 937 {
wakestrap 16:0eda62f0d9e8 938 CS^=*(data+i);
wakestrap 16:0eda62f0d9e8 939 fullPacket[i+3] = *(data+i);
wakestrap 16:0eda62f0d9e8 940 }
wakestrap 16:0eda62f0d9e8 941 fullPacket[sizeof(fullPacket)-1] = CS;
wakestrap 16:0eda62f0d9e8 942 }
wakestrap 16:0eda62f0d9e8 943 #if DEBUG
wakestrap 16:0eda62f0d9e8 944 printf("Sending a Packet \n\r");
wakestrap 16:0eda62f0d9e8 945 #endif
wakestrap 16:0eda62f0d9e8 946 sock.send(fullPacket, sizeof(fullPacket)); // Send the packet
wakestrap 16:0eda62f0d9e8 947 }
wakestrap 16:0eda62f0d9e8 948
wakestrap 16:0eda62f0d9e8 949 void Handshake(){
wakestrap 16:0eda62f0d9e8 950
wakestrap 16:0eda62f0d9e8 951 HPacket.id = 0;
wakestrap 16:0eda62f0d9e8 952 HPacket.M1 = 3;
wakestrap 16:0eda62f0d9e8 953 HPacket.M2 = 1;
wakestrap 16:0eda62f0d9e8 954 HPacket.M3 = 4;
donatien 5:01f6c3e112af 955
wakestrap 16:0eda62f0d9e8 956 KSPBoardSendData(details(HPacket));
wakestrap 16:0eda62f0d9e8 957 }