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@17:00d66e5edf58, 2020-03-17 (annotated)

Committer:
wakestrap
Date:
Tue Mar 17 01:25:35 2020 +0000
Revision:
17:00d66e5edf58
Parent:
16:0eda62f0d9e8 
Added a connection loop to allow for automatic reconnect when connection is lost. ; Fixed Struct packing issue with Pragma Pack.; Adjusted console output to prevent buffer backlog.

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 17:00d66e5edf58 11 #define OUTPUTENABLE 1
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 17:00d66e5edf58 129 #pragma pack (1)
wakestrap 17:00d66e5edf58 130
wakestrap 16:0eda62f0d9e8 131 struct VesselData
wakestrap 15:03ed24574806 132 {
wakestrap 16:0eda62f0d9e8 133 char id; //1
wakestrap 16:0eda62f0d9e8 134 float AP; //2
wakestrap 16:0eda62f0d9e8 135 float PE; //3
wakestrap 16:0eda62f0d9e8 136 float SemiMajorAxis; //4
wakestrap 16:0eda62f0d9e8 137 float SemiMinorAxis; //5
wakestrap 16:0eda62f0d9e8 138 float VVI; //6
wakestrap 16:0eda62f0d9e8 139 float e; //7
wakestrap 16:0eda62f0d9e8 140 float inc; //8
wakestrap 16:0eda62f0d9e8 141 float G; //9
wakestrap 16:0eda62f0d9e8 142 int TAp; //10
wakestrap 16:0eda62f0d9e8 143 int TPe; //11 41
wakestrap 16:0eda62f0d9e8 144 float TrueAnomaly; //12
wakestrap 16:0eda62f0d9e8 145 float Density; //13
wakestrap 16:0eda62f0d9e8 146 int period; //14
wakestrap 16:0eda62f0d9e8 147 float RAlt; //15
wakestrap 16:0eda62f0d9e8 148 float Alt; //16 61
wakestrap 16:0eda62f0d9e8 149 float Vsurf; //17
wakestrap 16:0eda62f0d9e8 150 float Lat; //18
wakestrap 16:0eda62f0d9e8 151 float Lon; //19
wakestrap 16:0eda62f0d9e8 152 float LiquidFuelTot; //20
wakestrap 16:0eda62f0d9e8 153 float LiquidFuel; //21
wakestrap 16:0eda62f0d9e8 154 float OxidizerTot; //22
wakestrap 16:0eda62f0d9e8 155 float Oxidizer; //23
wakestrap 16:0eda62f0d9e8 156 float EChargeTot; //24
wakestrap 16:0eda62f0d9e8 157 float ECharge; //25
wakestrap 16:0eda62f0d9e8 158 float MonoPropTot; //26
wakestrap 16:0eda62f0d9e8 159 float MonoProp; //27
wakestrap 16:0eda62f0d9e8 160 float IntakeAirTot; //28
wakestrap 16:0eda62f0d9e8 161 float IntakeAir; //29
wakestrap 16:0eda62f0d9e8 162 float SolidFuelTot; //30
wakestrap 16:0eda62f0d9e8 163 float SolidFuel; //31
wakestrap 16:0eda62f0d9e8 164 float XenonGasTot; //32
wakestrap 16:0eda62f0d9e8 165 float XenonGas; //33
wakestrap 16:0eda62f0d9e8 166 float LiquidFuelTotS; //34
wakestrap 16:0eda62f0d9e8 167 float LiquidFuelS; //35
wakestrap 16:0eda62f0d9e8 168 float OxidizerTotS; //36
wakestrap 16:0eda62f0d9e8 169 float OxidizerS; //37
wakestrap 16:0eda62f0d9e8 170 uint32_t MissionTime; //38
wakestrap 16:0eda62f0d9e8 171 float deltaTime; //39
wakestrap 16:0eda62f0d9e8 172 float VOrbit; //40
wakestrap 16:0eda62f0d9e8 173 uint32_t MNTime; //41
wakestrap 16:0eda62f0d9e8 174 float MNDeltaV; //42 165
wakestrap 16:0eda62f0d9e8 175 uint16_t Pitch; //43
wakestrap 16:0eda62f0d9e8 176 uint16_t Roll; //44
wakestrap 16:0eda62f0d9e8 177 uint16_t Heading; //45
wakestrap 16:0eda62f0d9e8 178 uint16_t ActionGroups; //46 173 status bit order:SAS, RCS, Light, Gear, Brakes, Abort, Custom01 - 10
wakestrap 16:0eda62f0d9e8 179 char SOINumber; //47 SOI Number (decimal format: sun-planet-moon e.g. 130 = kerbin, 131 = mun)
wakestrap 16:0eda62f0d9e8 180 char MaxOverHeat; //48 175 Max part overheat (% percent)
wakestrap 16:0eda62f0d9e8 181 float MachNumber; //49
wakestrap 16:0eda62f0d9e8 182 float IAS; //50 Indicated Air Speed
wakestrap 16:0eda62f0d9e8 183 char CurrentStage; //51 Current stage number
wakestrap 16:0eda62f0d9e8 184 char TotalStage; //52 185 TotalNumber of stages
wakestrap 16:0eda62f0d9e8 185 float TargetDist; //53 Distance to targeted vessel (m)
wakestrap 16:0eda62f0d9e8 186 float TargetV; //54 193 Target vessel relative velocity (m/s)
wakestrap 16:0eda62f0d9e8 187 char NavballSASMode; //55 194 Combined char for navball target mode and SAS mode
wakestrap 16:0eda62f0d9e8 188 // First four bits indicate AutoPilot mode:
wakestrap 16:0eda62f0d9e8 189 // 0 SAS is off //1 = Regular Stability Assist //2 = Prograde
wakestrap 16:0eda62f0d9e8 190 // 3 = RetroGrade //4 = Normal //5 = Antinormal //6 = Radial In
wakestrap 16:0eda62f0d9e8 191 // 7 = Radial Out //8 = Target //9 = Anti-Target //10 = Maneuver node
wakestrap 16:0eda62f0d9e8 192 // Last 4 bits set navball mode. (0=ignore,1=ORBIT,2=SURFACE,3=TARGET)
wakestrap 16:0eda62f0d9e8 193 uint16_t ProgradePitch; //56 Pitch Of the Prograde Vector; int_16 ranging from (-0x8000(-360 degrees) to 0x7FFF(359.99ish degrees));
wakestrap 16:0eda62f0d9e8 194 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 195 uint16_t ManeuverPitch; //58 Pitch Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 16:0eda62f0d9e8 196 uint16_t ManeuverHeading;//59 Heading Of the Maneuver Vector; see above for range; (0 if no Maneuver node)
wakestrap 16:0eda62f0d9e8 197 uint16_t TargetPitch; //60 Pitch Of the Target Vector; see above for range; (0 if no Target)
wakestrap 16:0eda62f0d9e8 198 uint16_t TargetHeading; //61 Heading Of the Target Vector; see above for range; (0 if no Target)
wakestrap 16:0eda62f0d9e8 199 uint16_t NormalHeading; //62 Heading Of the Prograde Vector; see above for range; (Pitch of the Heading Vector is always 0)
wakestrap 16:0eda62f0d9e8 200 char vesselSync; //63 208 Starting with 1, increased on every VesselChange
wakestrap 16:0eda62f0d9e8 201 };
wakestrap 15:03ed24574806 202
wakestrap 15:03ed24574806 203
wakestrap 16:0eda62f0d9e8 204 struct HandShakePacket
wakestrap 16:0eda62f0d9e8 205 {
wakestrap 16:0eda62f0d9e8 206 char id;
wakestrap 16:0eda62f0d9e8 207 char M1;
wakestrap 16:0eda62f0d9e8 208 char M2;
wakestrap 16:0eda62f0d9e8 209 char M3;
wakestrap 16:0eda62f0d9e8 210 };
wakestrap 15:03ed24574806 211
wakestrap 16:0eda62f0d9e8 212 struct ControlPacket {
wakestrap 16:0eda62f0d9e8 213 char id;
wakestrap 16:0eda62f0d9e8 214 char MainControls; //SAS RCS Lights Gear Brakes Precision Abort Stage
wakestrap 16:0eda62f0d9e8 215 char Mode; //0 = stage, 1 = docking, 2 = map
wakestrap 16:0eda62f0d9e8 216 short ControlGroup; //control groups 1-10 in 2 chars
wakestrap 16:0eda62f0d9e8 217 char NavballSASMode; //AutoPilot mode
wakestrap 16:0eda62f0d9e8 218 char AdditionalControlByte1; //9
wakestrap 16:0eda62f0d9e8 219 int32_t Pitch; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 220 int32_t Roll; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 221 int32_t Yaw; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 222 int32_t TX; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 223 int32_t TY; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 224 int32_t TZ; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 225 int32_t WheelSteer; //-1000 -> 1000
wakestrap 16:0eda62f0d9e8 226 int32_t Throttle; // 0 -> 1000
wakestrap 16:0eda62f0d9e8 227 int32_t WheelThrottle; // 0 -> 1000
wakestrap 16:0eda62f0d9e8 228 char vesselSync; //Starting with 1, increased on every VesselChange
wakestrap 16:0eda62f0d9e8 229 };
wakestrap 16:0eda62f0d9e8 230
wakestrap 16:0eda62f0d9e8 231 HandShakePacket HPacket;
wakestrap 16:0eda62f0d9e8 232 VesselData VData;
wakestrap 16:0eda62f0d9e8 233 ControlPacket CPacket;
wakestrap 16:0eda62f0d9e8 234 EthernetInterface eth;
wakestrap 16:0eda62f0d9e8 235 TCPSocketConnection sock;
wakestrap 17:00d66e5edf58 236 UDPSocket server;
donatien 0:bb128f0e952f 237
emilmont 7:65188f4a8c25 238 int main() {
wakestrap 16:0eda62f0d9e8 239 //SETUP
wakestrap 17:00d66e5edf58 240 char buffer[256];
wakestrap 17:00d66e5edf58 241 int ConnectionState = 0;
wakestrap 16:0eda62f0d9e8 242
wakestrap 16:0eda62f0d9e8 243 t.start();
wakestrap 16:0eda62f0d9e8 244 printf("\n\r KSP mbedIO V0.1 \n\r");
donatien 0:bb128f0e952f 245 eth.init(); //Use DHCP
donatien 0:bb128f0e952f 246 eth.connect();
wakestrap 15:03ed24574806 247 printf("\nServer IP Address is %s\n\r", eth.getIPAddress());
wakestrap 15:03ed24574806 248 Endpoint KSPServer;
wakestrap 16:0eda62f0d9e8 249 while(true)
wakestrap 16:0eda62f0d9e8 250 {
wakestrap 17:00d66e5edf58 251 switch(ConnectionState)
wakestrap 17:00d66e5edf58 252 {
wakestrap 17:00d66e5edf58 253 case 0:// Connect to UDP server and get server IP address
wakestrap 17:00d66e5edf58 254 {
wakestrap 17:00d66e5edf58 255 server.bind(PORT);
wakestrap 17:00d66e5edf58 256 printf("Waiting for UDP packet...\n\r");
wakestrap 17:00d66e5edf58 257 while(server.receiveFrom(KSPServer, buffer, sizeof(buffer)) < 0)
wakestrap 17:00d66e5edf58 258 {
wakestrap 17:00d66e5edf58 259 //Waiting for UDP to Connect
wakestrap 17:00d66e5edf58 260 }
wakestrap 17:00d66e5edf58 261 #if DEBUG
wakestrap 17:00d66e5edf58 262 printf("Received packet from: %s\n\r", KSPServer.get_address());
wakestrap 17:00d66e5edf58 263 #endif
wakestrap 17:00d66e5edf58 264 ConnectionState++;
wakestrap 17:00d66e5edf58 265 break;
wakestrap 17:00d66e5edf58 266 }
wakestrap 17:00d66e5edf58 267 case 1: // Connect to TCP Server
wakestrap 17:00d66e5edf58 268 {
wakestrap 17:00d66e5edf58 269 sock.set_blocking(false, 1500); // Set socket to non-blocking timeout to 1.5s
wakestrap 17:00d66e5edf58 270 while (sock.connect(KSPServer.get_address(), PORT) < 0) {
wakestrap 17:00d66e5edf58 271 printf("Unable to connect to KSPServer on Port %d \r\n", PORT);
wakestrap 17:00d66e5edf58 272 wait(1);
wakestrap 17:00d66e5edf58 273 }
wakestrap 17:00d66e5edf58 274 printf("Connected to KSPServer on Port %d \r\n", PORT);
wakestrap 17:00d66e5edf58 275 Handshake();
wakestrap 17:00d66e5edf58 276 printf("Handshake sent \r\n");
wakestrap 17:00d66e5edf58 277 ConnectionState++;
wakestrap 17:00d66e5edf58 278 break;
wakestrap 17:00d66e5edf58 279 }
wakestrap 17:00d66e5edf58 280 case 2: // We're Connected, check connection and keep running
wakestrap 17:00d66e5edf58 281 {
wakestrap 17:00d66e5edf58 282 initLEDS();
wakestrap 17:00d66e5edf58 283 InitTxPackets();
wakestrap 17:00d66e5edf58 284 while(sock.is_connected())
wakestrap 17:00d66e5edf58 285 {
wakestrap 17:00d66e5edf58 286 input();
wakestrap 17:00d66e5edf58 287 output();
wakestrap 17:00d66e5edf58 288 }
wakestrap 17:00d66e5edf58 289 sock.close();
wakestrap 17:00d66e5edf58 290 printf("Connection Lost, Reconnecting... \n\r");
wakestrap 17:00d66e5edf58 291 ConnectionState = 0;
wakestrap 17:00d66e5edf58 292 break;
wakestrap 17:00d66e5edf58 293 }
wakestrap 17:00d66e5edf58 294 }
wakestrap 16:0eda62f0d9e8 295 }
wakestrap 16:0eda62f0d9e8 296 }
wakestrap 16:0eda62f0d9e8 297
wakestrap 16:0eda62f0d9e8 298
wakestrap 16:0eda62f0d9e8 299
wakestrap 16:0eda62f0d9e8 300 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 301 /////////////////// CONTROLS /////////////////////
wakestrap 16:0eda62f0d9e8 302 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 303
wakestrap 16:0eda62f0d9e8 304 void Indicators() {
wakestrap 16:0eda62f0d9e8 305 caution = 0;
wakestrap 16:0eda62f0d9e8 306 warning = 0;
wakestrap 16:0eda62f0d9e8 307
wakestrap 16:0eda62f0d9e8 308 caution += VData.G > GCAUTION;
wakestrap 16:0eda62f0d9e8 309 warning += VData.G > GWARN;
wakestrap 17:00d66e5edf58 310 // caution += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELCAUTION;
wakestrap 17:00d66e5edf58 311 // warning += VData.LiquidFuelS/VData.LiquidFuelTotS*100 < FUELWARN;
wakestrap 16:0eda62f0d9e8 312
wakestrap 16:0eda62f0d9e8 313 if (caution != 0)
wakestrap 16:0eda62f0d9e8 314 {
wakestrap 16:0eda62f0d9e8 315 printf("Yellow! \n\r");
wakestrap 16:0eda62f0d9e8 316 }
wakestrap 16:0eda62f0d9e8 317 else
wakestrap 16:0eda62f0d9e8 318 {
wakestrap 16:0eda62f0d9e8 319 //yel off
wakestrap 16:0eda62f0d9e8 320 }
emilmont 7:65188f4a8c25 321
wakestrap 16:0eda62f0d9e8 322 if (warning != 0)
wakestrap 16:0eda62f0d9e8 323 {
wakestrap 16:0eda62f0d9e8 324 printf("RED! \n\r");
wakestrap 16:0eda62f0d9e8 325 }
wakestrap 16:0eda62f0d9e8 326 else
wakestrap 16:0eda62f0d9e8 327 {
wakestrap 16:0eda62f0d9e8 328 //red off
wakestrap 16:0eda62f0d9e8 329 }
wakestrap 16:0eda62f0d9e8 330 #if OUTPUTENABLE
wakestrap 17:00d66e5edf58 331 printf("\033[2A");
wakestrap 17:00d66e5edf58 332 printf("\rAP : %f\n\r", VData.AP);
wakestrap 17:00d66e5edf58 333 //printf("TAp : %i \n\r", VData.TAp);
wakestrap 17:00d66e5edf58 334 //printf("Sync : %d \n\r", VData.vesselSync);
wakestrap 16:0eda62f0d9e8 335 printf("Alt : %f \n\r", VData.Alt);
wakestrap 17:00d66e5edf58 336 //printf("LiqFuel : %f \n\r", VData.LiquidFuel);
wakestrap 17:00d66e5edf58 337 float LiqFuelRem = VData.LiquidFuel/VData.LiquidFuelTot*100;
wakestrap 17:00d66e5edf58 338 printf("Fuel Left : %f %%", LiqFuelRem);
wakestrap 17:00d66e5edf58 339 //printf("Vsurf : %f", VData.Vsurf);
wakestrap 17:00d66e5edf58 340 //if(ControlStatus(AGSAS)) printf("AGSAS ON \n\r");
wakestrap 17:00d66e5edf58 341 //if(ControlStatus(AGRCS)) printf("AGRCS ON \n\r");
wakestrap 17:00d66e5edf58 342 //if(ControlStatus(AGCustom01)) printf("AGCustom1 ON \n\r");
wakestrap 16:0eda62f0d9e8 343 #endif
wakestrap 16:0eda62f0d9e8 344 }
wakestrap 16:0eda62f0d9e8 345
wakestrap 16:0eda62f0d9e8 346 void initLEDS() {
wakestrap 16:0eda62f0d9e8 347
wakestrap 16:0eda62f0d9e8 348 }
wakestrap 16:0eda62f0d9e8 349
wakestrap 16:0eda62f0d9e8 350 void LEDSAllOff() {
wakestrap 16:0eda62f0d9e8 351
wakestrap 16:0eda62f0d9e8 352 }
wakestrap 16:0eda62f0d9e8 353
wakestrap 16:0eda62f0d9e8 354 void InitTxPackets() {
wakestrap 16:0eda62f0d9e8 355 HPacket.id = 0;
wakestrap 16:0eda62f0d9e8 356 CPacket.id = 101;
wakestrap 16:0eda62f0d9e8 357 }
wakestrap 16:0eda62f0d9e8 358
wakestrap 16:0eda62f0d9e8 359
wakestrap 16:0eda62f0d9e8 360 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 361 //////////////////// INPUT /////////////////////
wakestrap 16:0eda62f0d9e8 362 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 363
wakestrap 16:0eda62f0d9e8 364 int input() {
wakestrap 16:0eda62f0d9e8 365 int returnValue = -1;
wakestrap 16:0eda62f0d9e8 366 now = t.read_ms();
wakestrap 16:0eda62f0d9e8 367
wakestrap 16:0eda62f0d9e8 368 if (KSPBoardReceiveData())
wakestrap 16:0eda62f0d9e8 369 {
wakestrap 16:0eda62f0d9e8 370 deadtimeOld = now;
wakestrap 16:0eda62f0d9e8 371 returnValue = id;
wakestrap 16:0eda62f0d9e8 372 switch(id)
wakestrap 16:0eda62f0d9e8 373 {
wakestrap 16:0eda62f0d9e8 374 case 0: //Handshake packet
wakestrap 16:0eda62f0d9e8 375 Handshake();
wakestrap 16:0eda62f0d9e8 376 #if DEBUG
wakestrap 16:0eda62f0d9e8 377 printf("HS Packet recieved\n\r");
wakestrap 16:0eda62f0d9e8 378 #endif
wakestrap 16:0eda62f0d9e8 379 break;
wakestrap 16:0eda62f0d9e8 380 case 1:
wakestrap 16:0eda62f0d9e8 381 VesselID = VData.vesselSync;
wakestrap 16:0eda62f0d9e8 382 Indicators();
wakestrap 16:0eda62f0d9e8 383 #if DEBUG
wakestrap 16:0eda62f0d9e8 384 printf("VD Packet recieved\n\r");
wakestrap 16:0eda62f0d9e8 385 #endif
wakestrap 16:0eda62f0d9e8 386 break;
wakestrap 16:0eda62f0d9e8 387 }
wakestrap 16:0eda62f0d9e8 388 Connected = true;
wakestrap 16:0eda62f0d9e8 389 }
wakestrap 16:0eda62f0d9e8 390
wakestrap 16:0eda62f0d9e8 391 else
wakestrap 16:0eda62f0d9e8 392 { //if no message received for a while, go idle
wakestrap 16:0eda62f0d9e8 393 deadtime = now - deadtimeOld;
wakestrap 16:0eda62f0d9e8 394 if (deadtime > IDLETIMER)
wakestrap 16:0eda62f0d9e8 395 {
wakestrap 16:0eda62f0d9e8 396 deadtimeOld = now;
wakestrap 17:00d66e5edf58 397 //Connected = false;
wakestrap 16:0eda62f0d9e8 398 LEDSAllOff();
wakestrap 17:00d66e5edf58 399 printf("Is Sleeping? \r\n");
wakestrap 16:0eda62f0d9e8 400 }
wakestrap 16:0eda62f0d9e8 401 }
wakestrap 16:0eda62f0d9e8 402
wakestrap 16:0eda62f0d9e8 403 return returnValue;
wakestrap 16:0eda62f0d9e8 404 }
wakestrap 16:0eda62f0d9e8 405
wakestrap 16:0eda62f0d9e8 406 char ControlStatus(char n)
wakestrap 16:0eda62f0d9e8 407 {
wakestrap 16:0eda62f0d9e8 408 return ((VData.ActionGroups >> n) & 1) == 1;
wakestrap 16:0eda62f0d9e8 409 }
wakestrap 16:0eda62f0d9e8 410
wakestrap 16:0eda62f0d9e8 411 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 412 ///////////////////// OUTPUT /////////////////////
wakestrap 16:0eda62f0d9e8 413 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 414
wakestrap 16:0eda62f0d9e8 415 void output() {
wakestrap 16:0eda62f0d9e8 416 now = t.read_ms();
wakestrap 16:0eda62f0d9e8 417 controlTime = now - controlTimeOld;
wakestrap 16:0eda62f0d9e8 418 if (controlTime > CONTROLREFRESH) {
wakestrap 16:0eda62f0d9e8 419 controlTimeOld = now;
wakestrap 16:0eda62f0d9e8 420 controls();
wakestrap 16:0eda62f0d9e8 421 }
wakestrap 16:0eda62f0d9e8 422 }
wakestrap 16:0eda62f0d9e8 423
wakestrap 16:0eda62f0d9e8 424 void controls() {
wakestrap 16:0eda62f0d9e8 425 if (Connected) {
wakestrap 16:0eda62f0d9e8 426
wakestrap 16:0eda62f0d9e8 427 if (!ControlButton) { //--------- This is how you do main controls
wakestrap 16:0eda62f0d9e8 428 MainControls(SAS, 1);
wakestrap 16:0eda62f0d9e8 429 setSASMode(SMSAS); //setting SAS mode
wakestrap 16:0eda62f0d9e8 430 #if DEBUG
wakestrap 16:0eda62f0d9e8 431 printf("SAS PRESS \n\r");
wakestrap 16:0eda62f0d9e8 432 #endif
wakestrap 16:0eda62f0d9e8 433 //setNavballMode(NAVBallSURFACE); //setting navball mode
wakestrap 16:0eda62f0d9e8 434 }
wakestrap 16:0eda62f0d9e8 435 else {
wakestrap 16:0eda62f0d9e8 436 //setNavballMode(NAVBallTARGET);
wakestrap 16:0eda62f0d9e8 437 MainControls(SAS, 0);
wakestrap 16:0eda62f0d9e8 438 }
wakestrap 16:0eda62f0d9e8 439
wakestrap 16:0eda62f0d9e8 440 if (ControlButton)
wakestrap 16:0eda62f0d9e8 441 MainControls(RCS, 1);
wakestrap 16:0eda62f0d9e8 442 else
wakestrap 16:0eda62f0d9e8 443 MainControls(RCS, 0);
wakestrap 16:0eda62f0d9e8 444
wakestrap 16:0eda62f0d9e8 445 /**
wakestrap 16:0eda62f0d9e8 446 if (digitalRead(CG1PIN)) //--------- This is how you do control groups
wakestrap 16:0eda62f0d9e8 447 ControlGroups(1, 1);
wakestrap 16:0eda62f0d9e8 448 else
wakestrap 16:0eda62f0d9e8 449 ControlGroups(1, 0);
wakestrap 16:0eda62f0d9e8 450 **/
wakestrap 16:0eda62f0d9e8 451 /*
wakestrap 16:0eda62f0d9e8 452 if (getSASMode() == SMPrograde) { //--------- This is how you read SAS modes
wakestrap 16:0eda62f0d9e8 453 //Blink LED, do stuff, etc.
wakestrap 16:0eda62f0d9e8 454 }
wakestrap 16:0eda62f0d9e8 455
wakestrap 16:0eda62f0d9e8 456 if (getNavballMode() == NAVBallTARGET) { //--------- This is how you read navball modes
wakestrap 16:0eda62f0d9e8 457 //Blink LED, do stuff, etc.
wakestrap 16:0eda62f0d9e8 458 }
wakestrap 16:0eda62f0d9e8 459 */
wakestrap 16:0eda62f0d9e8 460
wakestrap 16:0eda62f0d9e8 461 //This is an example of reading analog inputs to an axis, with deadband and limits
wakestrap 16:0eda62f0d9e8 462 //CPacket.Throttle = constrain(map(analogRead(THROTTLEPIN), THROTTLEDB, 1024 - THROTTLEDB, 0, 1000), 0, 1000);
wakestrap 16:0eda62f0d9e8 463
wakestrap 16:0eda62f0d9e8 464 //This is an example of reading analog inputs to an axis, with deadband and limits
wakestrap 16:0eda62f0d9e8 465 //CPacket.Pitch = constrain(map(analogRead(THROTTLEPIN),0,1024,-1000,1000),-1000, 1000);
wakestrap 16:0eda62f0d9e8 466 CPacket.vesselSync = VData.vesselSync;
wakestrap 16:0eda62f0d9e8 467 KSPBoardSendData(details(CPacket));
wakestrap 16:0eda62f0d9e8 468 }
wakestrap 16:0eda62f0d9e8 469 }
wakestrap 16:0eda62f0d9e8 470
wakestrap 16:0eda62f0d9e8 471 void controlsInit() {
wakestrap 16:0eda62f0d9e8 472 /**
wakestrap 16:0eda62f0d9e8 473 pinMode(SASPIN, INPUT_PULLUP);
wakestrap 16:0eda62f0d9e8 474 pinMode(RCSPIN, INPUT_PULLUP);
wakestrap 16:0eda62f0d9e8 475 pinMode(CG1PIN, INPUT_PULLUP);
wakestrap 16:0eda62f0d9e8 476 **/
wakestrap 16:0eda62f0d9e8 477 }
wakestrap 16:0eda62f0d9e8 478
wakestrap 16:0eda62f0d9e8 479 char getSASMode() {
wakestrap 16:0eda62f0d9e8 480 return VData.NavballSASMode & 0b00001111; // leaves alone the lower 4 bits of; all higher bits set to 0.
wakestrap 16:0eda62f0d9e8 481 }
wakestrap 16:0eda62f0d9e8 482
wakestrap 16:0eda62f0d9e8 483 char getNavballMode() {
wakestrap 16:0eda62f0d9e8 484 return VData.NavballSASMode >> 4; // leaves alone the higher 4 bits of; all lower bits set to 0.
wakestrap 16:0eda62f0d9e8 485 }
wakestrap 16:0eda62f0d9e8 486
wakestrap 16:0eda62f0d9e8 487 void setSASMode(char m) {
wakestrap 16:0eda62f0d9e8 488 CPacket.NavballSASMode &= 0b11110000;
wakestrap 16:0eda62f0d9e8 489 CPacket.NavballSASMode += m;
wakestrap 16:0eda62f0d9e8 490 }
wakestrap 16:0eda62f0d9e8 491
wakestrap 16:0eda62f0d9e8 492 void setNavballMode(char m) {
wakestrap 16:0eda62f0d9e8 493 CPacket.NavballSASMode &= 0b00001111;
wakestrap 16:0eda62f0d9e8 494 CPacket.NavballSASMode += m << 4;
wakestrap 16:0eda62f0d9e8 495 }
wakestrap 16:0eda62f0d9e8 496
wakestrap 16:0eda62f0d9e8 497 void MainControls(char n, bool s) {
wakestrap 16:0eda62f0d9e8 498 if (s)
wakestrap 16:0eda62f0d9e8 499 CPacket.MainControls |= (1 << n); // forces nth bit of x to be 1. all other bits left alone.
wakestrap 16:0eda62f0d9e8 500 else
wakestrap 16:0eda62f0d9e8 501 CPacket.MainControls &= ~(1 << n); // forces nth bit of x to be 0. all other bits left alone.
wakestrap 16:0eda62f0d9e8 502 }
wakestrap 16:0eda62f0d9e8 503
wakestrap 16:0eda62f0d9e8 504 void ControlGroups(char n, bool s) {
wakestrap 16:0eda62f0d9e8 505 if (s)
wakestrap 16:0eda62f0d9e8 506 CPacket.ControlGroup |= (1 << n); // forces nth bit of x to be 1. all other bits left alone.
wakestrap 16:0eda62f0d9e8 507 else
wakestrap 16:0eda62f0d9e8 508 CPacket.ControlGroup &= ~(1 << n); // forces nth bit of x to be 0. all other bits left alone.
wakestrap 16:0eda62f0d9e8 509 }
wakestrap 16:0eda62f0d9e8 510
wakestrap 16:0eda62f0d9e8 511
wakestrap 16:0eda62f0d9e8 512
wakestrap 16:0eda62f0d9e8 513
wakestrap 16:0eda62f0d9e8 514 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 515 //////////////// COMMUNICATIONS///////////////////
wakestrap 16:0eda62f0d9e8 516 //////////////////////////////////////////////////
wakestrap 16:0eda62f0d9e8 517
wakestrap 16:0eda62f0d9e8 518
wakestrap 16:0eda62f0d9e8 519
wakestrap 16:0eda62f0d9e8 520 bool KSPBoardReceiveData() {
wakestrap 16:0eda62f0d9e8 521 int ret;
wakestrap 15:03ed24574806 522
wakestrap 16:0eda62f0d9e8 523 ret = sock.receive(buffer, sizeof(buffer)-1);
wakestrap 16:0eda62f0d9e8 524
wakestrap 16:0eda62f0d9e8 525 if(ret > 0)
wakestrap 16:0eda62f0d9e8 526 {
wakestrap 16:0eda62f0d9e8 527 int packetCheckState = 0;
wakestrap 16:0eda62f0d9e8 528 #if DEBUG
wakestrap 16:0eda62f0d9e8 529 printf("Packet Rx'd \n\r");
wakestrap 16:0eda62f0d9e8 530 #endif
wakestrap 16:0eda62f0d9e8 531 for(int x = 0; x < 4; x++)
wakestrap 15:03ed24574806 532 {
wakestrap 16:0eda62f0d9e8 533 switch(packetCheckState){
wakestrap 16:0eda62f0d9e8 534 case 0: // CHECK HEADER 1
wakestrap 16:0eda62f0d9e8 535 if(buffer[packetCheckState] == 0xBE)
wakestrap 15:03ed24574806 536 {
wakestrap 16:0eda62f0d9e8 537 packetCheckState++;
wakestrap 16:0eda62f0d9e8 538 }
wakestrap 16:0eda62f0d9e8 539 break;
wakestrap 16:0eda62f0d9e8 540 case 1: // CHECK HEADER 2
wakestrap 16:0eda62f0d9e8 541 if(buffer[packetCheckState] == 0xEF)
wakestrap 16:0eda62f0d9e8 542 {
wakestrap 16:0eda62f0d9e8 543 packetCheckState++;
wakestrap 15:03ed24574806 544 }
wakestrap 16:0eda62f0d9e8 545 break;
wakestrap 16:0eda62f0d9e8 546 case 2: // GET PAYLOAD SIZE
wakestrap 16:0eda62f0d9e8 547 rx_len = buffer[packetCheckState];
wakestrap 16:0eda62f0d9e8 548 packetCheckState++;
wakestrap 16:0eda62f0d9e8 549 #if DEBUG
wakestrap 16:0eda62f0d9e8 550 printf("PL Size at Rx: %d \n\r", rx_len);
wakestrap 16:0eda62f0d9e8 551 #endif
wakestrap 16:0eda62f0d9e8 552 break;
wakestrap 16:0eda62f0d9e8 553 case 3:
wakestrap 16:0eda62f0d9e8 554 id = buffer[packetCheckState];
wakestrap 16:0eda62f0d9e8 555 #if DEBUG
wakestrap 16:0eda62f0d9e8 556 printf("Packet ID: %d \n\r", id);
wakestrap 16:0eda62f0d9e8 557 #endif
wakestrap 16:0eda62f0d9e8 558 if(id == 0)
wakestrap 16:0eda62f0d9e8 559 {
wakestrap 16:0eda62f0d9e8 560 structSize = sizeof(HPacket);
wakestrap 16:0eda62f0d9e8 561 address = (uint16_t*)&HPacket;
wakestrap 16:0eda62f0d9e8 562 }
wakestrap 16:0eda62f0d9e8 563 else if(id == 1)
wakestrap 16:0eda62f0d9e8 564 {
wakestrap 16:0eda62f0d9e8 565 structSize = sizeof(VData);
wakestrap 16:0eda62f0d9e8 566 address = (uint16_t*)&VData;
wakestrap 16:0eda62f0d9e8 567 }
wakestrap 17:00d66e5edf58 568 else if(id == 2)
wakestrap 17:00d66e5edf58 569 {
wakestrap 17:00d66e5edf58 570 //Status Packet
wakestrap 17:00d66e5edf58 571 }
wakestrap 16:0eda62f0d9e8 572 else
wakestrap 16:0eda62f0d9e8 573 {
wakestrap 16:0eda62f0d9e8 574 printf("Bad Packet ID: %d", buffer[packetCheckState]);
wakestrap 16:0eda62f0d9e8 575 packetCheckState = 0;
wakestrap 16:0eda62f0d9e8 576 }
wakestrap 16:0eda62f0d9e8 577 break;
wakestrap 16:0eda62f0d9e8 578 default :
wakestrap 16:0eda62f0d9e8 579 printf("Something went wrong in Packet Rx Loop \r\n");
wakestrap 16:0eda62f0d9e8 580 break;
wakestrap 15:03ed24574806 581 }
wakestrap 16:0eda62f0d9e8 582 }
wakestrap 16:0eda62f0d9e8 583
wakestrap 16:0eda62f0d9e8 584 if(packetCheckState == 3) //We had a valid packet
wakestrap 16:0eda62f0d9e8 585 {
wakestrap 16:0eda62f0d9e8 586 calc_CS = rx_len;
wakestrap 16:0eda62f0d9e8 587 for(int i = 0; i < rx_len; i++)
wakestrap 15:03ed24574806 588 {
wakestrap 16:0eda62f0d9e8 589 payloadBuffer[i] = buffer[i+3];
wakestrap 16:0eda62f0d9e8 590 calc_CS^=buffer[i+3];
wakestrap 15:03ed24574806 591 }
wakestrap 15:03ed24574806 592
wakestrap 16:0eda62f0d9e8 593 if(calc_CS == buffer[rx_len+3]) //CS Check
wakestrap 15:03ed24574806 594 {
wakestrap 16:0eda62f0d9e8 595 char varBuffer[4];
wakestrap 16:0eda62f0d9e8 596 #if DEBUG
wakestrap 16:0eda62f0d9e8 597 printf("Checksum Passed! \n\r");
wakestrap 16:0eda62f0d9e8 598 #endif
wakestrap 16:0eda62f0d9e8 599
wakestrap 16:0eda62f0d9e8 600 VData.id = payloadBuffer[0]; //1
wakestrap 17:00d66e5edf58 601 if(VData.id == 1)
wakestrap 16:0eda62f0d9e8 602 {
wakestrap 17:00d66e5edf58 603 memcpy(address,payloadBuffer,structSize);
wakestrap 17:00d66e5edf58 604 rx_len = 0;
wakestrap 17:00d66e5edf58 605 return true;
wakestrap 16:0eda62f0d9e8 606 }
wakestrap 15:03ed24574806 607 }
wakestrap 15:03ed24574806 608 else
wakestrap 15:03ed24574806 609 {
wakestrap 16:0eda62f0d9e8 610 //failed checksum, need to clear this out anyway
wakestrap 16:0eda62f0d9e8 611 printf("Failed Checksum! Sorry \n\r");
wakestrap 16:0eda62f0d9e8 612 rx_len = 0;
wakestrap 16:0eda62f0d9e8 613 rx_array_inx = 1;
wakestrap 16:0eda62f0d9e8 614 return false;
wakestrap 15:03ed24574806 615 }
wakestrap 16:0eda62f0d9e8 616
wakestrap 16:0eda62f0d9e8 617
wakestrap 15:03ed24574806 618 }
emilmont 7:65188f4a8c25 619 }
wakestrap 15:03ed24574806 620
wakestrap 16:0eda62f0d9e8 621 return false;
wakestrap 16:0eda62f0d9e8 622 }
wakestrap 16:0eda62f0d9e8 623
wakestrap 16:0eda62f0d9e8 624 void KSPBoardSendData(uint8_t * data, uint8_t len){
wakestrap 16:0eda62f0d9e8 625 uint8_t CS = len;
wakestrap 16:0eda62f0d9e8 626 char fullPacket[len+4];
wakestrap 16:0eda62f0d9e8 627 fullPacket[0] = 0xBE;
wakestrap 16:0eda62f0d9e8 628 fullPacket[1] = 0xEF;
wakestrap 16:0eda62f0d9e8 629 fullPacket[2] = len;
donatien 0:bb128f0e952f 630
wakestrap 16:0eda62f0d9e8 631 if(*(data) == 101)
wakestrap 16:0eda62f0d9e8 632 {
wakestrap 16:0eda62f0d9e8 633 fullPacket[3] = 101; //Csid 101
wakestrap 16:0eda62f0d9e8 634 fullPacket[4] = CPacket.MainControls;
wakestrap 16:0eda62f0d9e8 635 fullPacket[5] = CPacket.Mode;
wakestrap 16:0eda62f0d9e8 636 fullPacket[6] = CPacket.ControlGroup & 0xff;
wakestrap 16:0eda62f0d9e8 637 fullPacket[7] = (CPacket.ControlGroup >> 8);
wakestrap 16:0eda62f0d9e8 638 fullPacket[8] = CPacket.NavballSASMode;
wakestrap 16:0eda62f0d9e8 639 fullPacket[9] = CPacket.AdditionalControlByte1;
wakestrap 16:0eda62f0d9e8 640 fullPacket[10] = CPacket.Pitch & 0xff;
wakestrap 16:0eda62f0d9e8 641 fullPacket[11] = (CPacket.Pitch >> 8);
wakestrap 16:0eda62f0d9e8 642 fullPacket[12] = CPacket.Roll & 0xff;
wakestrap 16:0eda62f0d9e8 643 fullPacket[13] = (CPacket.Roll >> 8);
wakestrap 16:0eda62f0d9e8 644 fullPacket[14] = CPacket.Yaw & 0xff;
wakestrap 16:0eda62f0d9e8 645 fullPacket[15] = (CPacket.Yaw >> 8);
wakestrap 16:0eda62f0d9e8 646 fullPacket[16] = CPacket.TX & 0xff;
wakestrap 16:0eda62f0d9e8 647 fullPacket[17] = (CPacket.TX >> 8);
wakestrap 16:0eda62f0d9e8 648 fullPacket[18] = CPacket.TY & 0xff;
wakestrap 16:0eda62f0d9e8 649 fullPacket[19] = (CPacket.TY >> 8);
wakestrap 16:0eda62f0d9e8 650 fullPacket[20] = CPacket.TZ & 0xff;
wakestrap 16:0eda62f0d9e8 651 fullPacket[21] = (CPacket.TZ >> 8);
wakestrap 16:0eda62f0d9e8 652 fullPacket[22] = CPacket.WheelSteer & 0xff;
wakestrap 16:0eda62f0d9e8 653 fullPacket[23] = (CPacket.WheelSteer >> 8);
wakestrap 16:0eda62f0d9e8 654 fullPacket[24] = CPacket.Throttle & 0xff;
wakestrap 16:0eda62f0d9e8 655 fullPacket[25] = (CPacket.Throttle >> 8);
wakestrap 16:0eda62f0d9e8 656 fullPacket[26] = CPacket.WheelThrottle & 0xff;
wakestrap 16:0eda62f0d9e8 657 fullPacket[27] = (CPacket.WheelThrottle >> 8);
wakestrap 16:0eda62f0d9e8 658 fullPacket[28] = CPacket.vesselSync;
wakestrap 16:0eda62f0d9e8 659 for(int i = 0; i<len; i++)
wakestrap 16:0eda62f0d9e8 660 {
wakestrap 16:0eda62f0d9e8 661 CS^= fullPacket[i+3];
wakestrap 16:0eda62f0d9e8 662 }
wakestrap 16:0eda62f0d9e8 663 fullPacket[sizeof(fullPacket)-1] = CS;
wakestrap 16:0eda62f0d9e8 664 }
wakestrap 16:0eda62f0d9e8 665 else
wakestrap 16:0eda62f0d9e8 666 {
wakestrap 16:0eda62f0d9e8 667 for(int i = 0; i<len; i++)
wakestrap 16:0eda62f0d9e8 668 {
wakestrap 16:0eda62f0d9e8 669 CS^=*(data+i);
wakestrap 16:0eda62f0d9e8 670 fullPacket[i+3] = *(data+i);
wakestrap 16:0eda62f0d9e8 671 }
wakestrap 16:0eda62f0d9e8 672 fullPacket[sizeof(fullPacket)-1] = CS;
wakestrap 16:0eda62f0d9e8 673 }
wakestrap 16:0eda62f0d9e8 674 #if DEBUG
wakestrap 16:0eda62f0d9e8 675 printf("Sending a Packet \n\r");
wakestrap 16:0eda62f0d9e8 676 #endif
wakestrap 16:0eda62f0d9e8 677 sock.send(fullPacket, sizeof(fullPacket)); // Send the packet
wakestrap 16:0eda62f0d9e8 678 }
wakestrap 16:0eda62f0d9e8 679
wakestrap 16:0eda62f0d9e8 680 void Handshake(){
wakestrap 16:0eda62f0d9e8 681
wakestrap 16:0eda62f0d9e8 682 HPacket.id = 0;
wakestrap 16:0eda62f0d9e8 683 HPacket.M1 = 3;
wakestrap 16:0eda62f0d9e8 684 HPacket.M2 = 1;
wakestrap 16:0eda62f0d9e8 685 HPacket.M3 = 4;
donatien 5:01f6c3e112af 686
wakestrap 16:0eda62f0d9e8 687 KSPBoardSendData(details(HPacket));
wakestrap 16:0eda62f0d9e8 688 }