Ian Key / RovComms

UDP comms to ROV

RovComms.cpp@0:4a75d653c18c, 2012-01-13 (annotated)

Committer:
inky
Date:
Fri Jan 13 20:24:19 2012 +0000
Revision:
0:4a75d653c18c
Child:
1:f51739cada16
First stab, some comms

Who changed what in which revision?

UserRevisionLine numberNew contents of line
inky 0:4a75d653c18c 1
inky 0:4a75d653c18c 2 #include "RovComms.h"
inky 0:4a75d653c18c 3
inky 0:4a75d653c18c 4
inky 0:4a75d653c18c 5 extern void debug(char *format,...);
inky 0:4a75d653c18c 6 extern void debugInt(const char *format, int i);
inky 0:4a75d653c18c 7
inky 0:4a75d653c18c 8
inky 0:4a75d653c18c 9 Ticker readNetwork;
inky 0:4a75d653c18c 10 void readNetworkKick()
inky 0:4a75d653c18c 11 {
inky 0:4a75d653c18c 12 Net::poll();
inky 0:4a75d653c18c 13 } // readNetworkKick()
inky 0:4a75d653c18c 14
inky 0:4a75d653c18c 15
inky 0:4a75d653c18c 16 RovComms::RovComms(int addr)
inky 0:4a75d653c18c 17 {
inky 0:4a75d653c18c 18 ResetValues();
inky 0:4a75d653c18c 19
inky 0:4a75d653c18c 20 #ifdef STATIC_IP_ADDR
inky 0:4a75d653c18c 21 rovAddr = IpAddr(10,0,0,addr);
inky 0:4a75d653c18c 22 eth = new EthernetNetIf(rovAddr, ipMask, ipGateway, ipDns);
inky 0:4a75d653c18c 23 #else
inky 0:4a75d653c18c 24 eth = new EthernetNetIf(); // uses DHCP
inky 0:4a75d653c18c 25 #endif
inky 0:4a75d653c18c 26
inky 0:4a75d653c18c 27 RxNetworkState = TxNetworkState = INIT;
inky 0:4a75d653c18c 28 readNetwork.attach(&readNetworkKick, 0.1); // every 100ms read
inky 0:4a75d653c18c 29 EthernetErr ethErr = eth->setup();
inky 0:4a75d653c18c 30 if(ethErr)
inky 0:4a75d653c18c 31 {
inky 0:4a75d653c18c 32 printf("Error %d in setup.\n", ethErr);
inky 0:4a75d653c18c 33 //return -1;
inky 0:4a75d653c18c 34 }
inky 0:4a75d653c18c 35 InitRx(addr);
inky 0:4a75d653c18c 36 if (addr == ROV_ADDR)
inky 0:4a75d653c18c 37 InitTx(HANDSET_ADDR);
inky 0:4a75d653c18c 38 else
inky 0:4a75d653c18c 39 InitTx(ROV_ADDR);
inky 0:4a75d653c18c 40 } // RovComms()
inky 0:4a75d653c18c 41
inky 0:4a75d653c18c 42 void RovComms::InitRx(int addr)
inky 0:4a75d653c18c 43 {
inky 0:4a75d653c18c 44 TCPSocketErr err;
inky 0:4a75d653c18c 45 RxSock = new TCPSocket();
inky 0:4a75d653c18c 46 RxSock->setOnEvent(this, &RovComms::onRxTCPSocketEvent);
inky 0:4a75d653c18c 47 err = RxSock->bind(Host(IpAddr(), RX_TCP_LISTENING_PORT));
inky 0:4a75d653c18c 48 if(err)
inky 0:4a75d653c18c 49 {
inky 0:4a75d653c18c 50 //Deal with that error...
inky 0:4a75d653c18c 51 } else {
inky 0:4a75d653c18c 52 // set the socket to listen
inky 0:4a75d653c18c 53 err = RxSock->listen(); //Starts listening
inky 0:4a75d653c18c 54 if (err)
inky 0:4a75d653c18c 55 CloseSocket(RxSock, &RxNetworkState);
inky 0:4a75d653c18c 56 }
inky 0:4a75d653c18c 57 }
inky 0:4a75d653c18c 58
inky 0:4a75d653c18c 59 void RovComms::CloseSocket(TCPSocket *s, int *state)
inky 0:4a75d653c18c 60 {
inky 0:4a75d653c18c 61 s->close();
inky 0:4a75d653c18c 62 *state = CLOSED;
inky 0:4a75d653c18c 63 }
inky 0:4a75d653c18c 64
inky 0:4a75d653c18c 65
inky 0:4a75d653c18c 66 void RovComms::InitTx(int addr)
inky 0:4a75d653c18c 67 {
inky 0:4a75d653c18c 68 TCPSocketErr bindErr;
inky 0:4a75d653c18c 69 Host server(IpAddr(10,0,0,addr), RX_TCP_LISTENING_PORT); // ip addr and port nos
inky 0:4a75d653c18c 70
inky 0:4a75d653c18c 71 bindErr = TxSock->connect(server);
inky 0:4a75d653c18c 72 switch(bindErr)
inky 0:4a75d653c18c 73 {
inky 0:4a75d653c18c 74 case TCPSOCKET_CONNECTED:
inky 0:4a75d653c18c 75 TxNetworkState = CONNECTED;
inky 0:4a75d653c18c 76 debug("RovComms::InitSlave -> connected");
inky 0:4a75d653c18c 77 break;
inky 0:4a75d653c18c 78
inky 0:4a75d653c18c 79 case TCPSOCKET_CONTIMEOUT:
inky 0:4a75d653c18c 80 case TCPSOCKET_CONRST:
inky 0:4a75d653c18c 81 case TCPSOCKET_CONABRT:
inky 0:4a75d653c18c 82 case TCPSOCKET_ERROR:
inky 0:4a75d653c18c 83 default:
inky 0:4a75d653c18c 84 CloseSocket(TxSock, &TxNetworkState);
inky 0:4a75d653c18c 85 break;
inky 0:4a75d653c18c 86 } // switch
inky 0:4a75d653c18c 87 }
inky 0:4a75d653c18c 88
inky 0:4a75d653c18c 89 /* @fn Transmit(char *data)
inky 0:4a75d653c18c 90 * Sends the buffer to the other end
inky 0:4a75d653c18c 91 */
inky 0:4a75d653c18c 92 void RovComms::Transmit(int pktType)
inky 0:4a75d653c18c 93 {
inky 0:4a75d653c18c 94 ClearBuffer(iTxData);
inky 0:4a75d653c18c 95
inky 0:4a75d653c18c 96 iTxData[0] = pktType;
inky 0:4a75d653c18c 97 switch (pktType)
inky 0:4a75d653c18c 98 {
inky 0:4a75d653c18c 99 case CMD_ROV:
inky 0:4a75d653c18c 100 iTxData[ROV_SPEED_POS] = iRovSpeed;
inky 0:4a75d653c18c 101 iTxData[ROV_TURN_POS] = iRovTurn;
inky 0:4a75d653c18c 102 iTxData[ROV_VERT_POS] = iRovVert;
inky 0:4a75d653c18c 103 break;
inky 0:4a75d653c18c 104
inky 0:4a75d653c18c 105 case CMD_AUX:
inky 0:4a75d653c18c 106 iTxData[AUX_CMD_POS] = 0;
inky 0:4a75d653c18c 107 iTxData[AUX_LIGHT_POS] = iLight;
inky 0:4a75d653c18c 108 iTxData[AUX_CAM_POS] = iCamera;
inky 0:4a75d653c18c 109 iTxData[AUX_SOLENOID_POS] = iWaterSol + iAirSol;
inky 0:4a75d653c18c 110 break;
inky 0:4a75d653c18c 111
inky 0:4a75d653c18c 112 case CMD_STATS:
inky 0:4a75d653c18c 113 iTxData[STATS_COMPASS_POS] = 23;
inky 0:4a75d653c18c 114 iTxData[STATS_PRESSURE_POS] = 4;
inky 0:4a75d653c18c 115 iTxData[STATS_X_ACCEL_POS] = 1;
inky 0:4a75d653c18c 116 iTxData[STATS_Y_ACCEL_POS] = 1;
inky 0:4a75d653c18c 117 iTxData[STATS_Z_ACCEL_POS] = 0;
inky 0:4a75d653c18c 118 iTxData[STATS_TEMPERATURE_POS] = 20;
inky 0:4a75d653c18c 119 iTxData[STATS_MISC_POS] = 69;
inky 0:4a75d653c18c 120 break;
inky 0:4a75d653c18c 121
inky 0:4a75d653c18c 122 case CMD_CLAW:
inky 0:4a75d653c18c 123 iTxData[1] = 1;
inky 0:4a75d653c18c 124 iTxData[2] = 2;
inky 0:4a75d653c18c 125 iTxData[3] = 3;
inky 0:4a75d653c18c 126 iTxData[4] = 4;
inky 0:4a75d653c18c 127 break;
inky 0:4a75d653c18c 128
inky 0:4a75d653c18c 129 case CMD_PTZ:
inky 0:4a75d653c18c 130 iTxData[1] = 5;
inky 0:4a75d653c18c 131 iTxData[2] = 6;
inky 0:4a75d653c18c 132 iTxData[3] = 7;
inky 0:4a75d653c18c 133 iTxData[4] = 8;
inky 0:4a75d653c18c 134 break;
inky 0:4a75d653c18c 135
inky 0:4a75d653c18c 136 default:
inky 0:4a75d653c18c 137 return;
inky 0:4a75d653c18c 138 } // switch pktType
inky 0:4a75d653c18c 139
inky 0:4a75d653c18c 140 int errCode = TxSock->send(iTxData, MAX_BUFFER_SIZE);
inky 0:4a75d653c18c 141 if (errCode < 0) // TCPSocketErr
inky 0:4a75d653c18c 142 {
inky 0:4a75d653c18c 143 }
inky 0:4a75d653c18c 144 Net::poll(); // tells stack to process the info (transfer from here to the stack and tx it)
inky 0:4a75d653c18c 145 }
inky 0:4a75d653c18c 146
inky 0:4a75d653c18c 147 char *RovComms::Receive(void)
inky 0:4a75d653c18c 148 {
inky 0:4a75d653c18c 149 char *buf={0};
inky 0:4a75d653c18c 150 int errCode = 0;
inky 0:4a75d653c18c 151 Net::poll(); // tell the stack to transfer the data from it's stack to the readable one here
inky 0:4a75d653c18c 152 errCode = RxSock->recv(buf, MAX_BUFFER_SIZE);
inky 0:4a75d653c18c 153 if (errCode < 0) // TCPSocketErr
inky 0:4a75d653c18c 154 {
inky 0:4a75d653c18c 155 }
inky 0:4a75d653c18c 156 // decode the packet.....
inky 0:4a75d653c18c 157 debugInt("Pkt type = ", buf[0]);
inky 0:4a75d653c18c 158 for (int i=1;i<MAX_BUFFER_SIZE;i++)
inky 0:4a75d653c18c 159 debugInt("Pkt data = ", buf[i]);
inky 0:4a75d653c18c 160 return buf;
inky 0:4a75d653c18c 161 }
inky 0:4a75d653c18c 162
inky 0:4a75d653c18c 163
inky 0:4a75d653c18c 164 void RovComms::onConnectedTCPSocketEvent(TCPSocketEvent e)
inky 0:4a75d653c18c 165 {
inky 0:4a75d653c18c 166 switch(e)
inky 0:4a75d653c18c 167 {
inky 0:4a75d653c18c 168 case TCPSOCKET_ACCEPT:
inky 0:4a75d653c18c 169 break;
inky 0:4a75d653c18c 170
inky 0:4a75d653c18c 171 case TCPSOCKET_READABLE:
inky 0:4a75d653c18c 172 Receive();
inky 0:4a75d653c18c 173 break;
inky 0:4a75d653c18c 174
inky 0:4a75d653c18c 175 default:
inky 0:4a75d653c18c 176 break;
inky 0:4a75d653c18c 177 }
inky 0:4a75d653c18c 178 }
inky 0:4a75d653c18c 179
inky 0:4a75d653c18c 180 void RovComms::onRxTCPSocketEvent(TCPSocketEvent e)
inky 0:4a75d653c18c 181 {
inky 0:4a75d653c18c 182 TCPSocket* pConnectedSock;
inky 0:4a75d653c18c 183 Host client;
inky 0:4a75d653c18c 184 TCPSocketErr err;
inky 0:4a75d653c18c 185 switch (e)
inky 0:4a75d653c18c 186 {
inky 0:4a75d653c18c 187 case TCPSOCKET_ACCEPT:
inky 0:4a75d653c18c 188 err = RxSock->accept(&client, &pConnectedSock);
inky 0:4a75d653c18c 189 if(!err)
inky 0:4a75d653c18c 190 pConnectedSock->setOnEvent(this, &RovComms::onConnectedTCPSocketEvent); //Setup the new socket events case TCPSOCKET_READABLE: //The only event for now
inky 0:4a75d653c18c 191 break;
inky 0:4a75d653c18c 192
inky 0:4a75d653c18c 193 case TCPSOCKET_READABLE:
inky 0:4a75d653c18c 194 Receive();
inky 0:4a75d653c18c 195 break;
inky 0:4a75d653c18c 196
inky 0:4a75d653c18c 197 case TCPSOCKET_WRITEABLE:
inky 0:4a75d653c18c 198 // continue the write process
inky 0:4a75d653c18c 199 break;
inky 0:4a75d653c18c 200
inky 0:4a75d653c18c 201 case TCPSOCKET_CONNECTED:
inky 0:4a75d653c18c 202 case TCPSOCKET_CONTIMEOUT:
inky 0:4a75d653c18c 203 case TCPSOCKET_CONRST:
inky 0:4a75d653c18c 204 case TCPSOCKET_CONABRT:
inky 0:4a75d653c18c 205 case TCPSOCKET_ERROR:
inky 0:4a75d653c18c 206 default:
inky 0:4a75d653c18c 207 break;
inky 0:4a75d653c18c 208 } // switch(e)
inky 0:4a75d653c18c 209 } // onRxTCPSocketEvent()
inky 0:4a75d653c18c 210
inky 0:4a75d653c18c 211 // Slave socket event handler
inky 0:4a75d653c18c 212 void RovComms::onTxTCPSocketEvent(TCPSocketEvent e)
inky 0:4a75d653c18c 213 {
inky 0:4a75d653c18c 214 TCPSocket* pConnectedSock;
inky 0:4a75d653c18c 215 Host client;
inky 0:4a75d653c18c 216 TCPSocketErr err;
inky 0:4a75d653c18c 217 switch (e)
inky 0:4a75d653c18c 218 {
inky 0:4a75d653c18c 219 case TCPSOCKET_ACCEPT:
inky 0:4a75d653c18c 220 // this event should never happen as a slave!!!
inky 0:4a75d653c18c 221 break;
inky 0:4a75d653c18c 222
inky 0:4a75d653c18c 223 case TCPSOCKET_READABLE:
inky 0:4a75d653c18c 224 // don't think this event should happen
inky 0:4a75d653c18c 225 Receive();
inky 0:4a75d653c18c 226 break;
inky 0:4a75d653c18c 227
inky 0:4a75d653c18c 228 case TCPSOCKET_WRITEABLE:
inky 0:4a75d653c18c 229 // continue the write process
inky 0:4a75d653c18c 230 break;
inky 0:4a75d653c18c 231
inky 0:4a75d653c18c 232 case TCPSOCKET_CONNECTED:
inky 0:4a75d653c18c 233 case TCPSOCKET_CONTIMEOUT:
inky 0:4a75d653c18c 234 case TCPSOCKET_CONRST:
inky 0:4a75d653c18c 235 case TCPSOCKET_CONABRT:
inky 0:4a75d653c18c 236 case TCPSOCKET_ERROR:
inky 0:4a75d653c18c 237 default:
inky 0:4a75d653c18c 238 break;
inky 0:4a75d653c18c 239 } // switch(e)
inky 0:4a75d653c18c 240 } // onSlaveTCPSocketEvent()
inky 0:4a75d653c18c 241
inky 0:4a75d653c18c 242 void RovComms::ResetValues()
inky 0:4a75d653c18c 243 {
inky 0:4a75d653c18c 244 iRovSpeed = 0;
inky 0:4a75d653c18c 245 iRovTurn = 0;
inky 0:4a75d653c18c 246 iRovVert = 0;
inky 0:4a75d653c18c 247
inky 0:4a75d653c18c 248 iLight = 0;
inky 0:4a75d653c18c 249 iWaterSol = 0;
inky 0:4a75d653c18c 250 iAirSol = 0;
inky 0:4a75d653c18c 251 iCamera = 0;
inky 0:4a75d653c18c 252 iPressure = 0;
inky 0:4a75d653c18c 253 iCompass = 0;
inky 0:4a75d653c18c 254 iAccelX = 0;
inky 0:4a75d653c18c 255 iAccelY = 0;
inky 0:4a75d653c18c 256 iAccelZ = 0;
inky 0:4a75d653c18c 257 }
inky 0:4a75d653c18c 258
inky 0:4a75d653c18c 259 void RovComms::RovPropulsion(int speed, int turn, int vert)
inky 0:4a75d653c18c 260 {
inky 0:4a75d653c18c 261 iRovSpeed = 0;
inky 0:4a75d653c18c 262 iRovTurn = 0;
inky 0:4a75d653c18c 263 iRovVert = 0;
inky 0:4a75d653c18c 264 Transmit(CMD_ROV);
inky 0:4a75d653c18c 265 }
inky 0:4a75d653c18c 266
inky 0:4a75d653c18c 267 void RovComms::FrontLights(int state)
inky 0:4a75d653c18c 268 {
inky 0:4a75d653c18c 269 if (state == 1)
inky 0:4a75d653c18c 270 iLight |= AUX_FN_LIGHT1_ON;
inky 0:4a75d653c18c 271 else
inky 0:4a75d653c18c 272 iLight &= ~AUX_FN_LIGHT1_ON;
inky 0:4a75d653c18c 273 Transmit(CMD_AUX);
inky 0:4a75d653c18c 274 }
inky 0:4a75d653c18c 275
inky 0:4a75d653c18c 276 void RovComms::RearLights(int state)
inky 0:4a75d653c18c 277 {
inky 0:4a75d653c18c 278 if (state == 1)
inky 0:4a75d653c18c 279 iLight |= AUX_FN_LIGHT2_ON;
inky 0:4a75d653c18c 280 else
inky 0:4a75d653c18c 281 iLight &= ~AUX_FN_LIGHT2_ON;
inky 0:4a75d653c18c 282 Transmit(CMD_AUX);
inky 0:4a75d653c18c 283 }
inky 0:4a75d653c18c 284
inky 0:4a75d653c18c 285 void RovComms::Camera(int cam)
inky 0:4a75d653c18c 286 {
inky 0:4a75d653c18c 287 if (cam == 1)
inky 0:4a75d653c18c 288 iCamera = AUX_FN_CAM1_ON;
inky 0:4a75d653c18c 289 else
inky 0:4a75d653c18c 290 iCamera = AUX_FN_CAM2_ON;
inky 0:4a75d653c18c 291 Transmit(CMD_AUX);
inky 0:4a75d653c18c 292 }
inky 0:4a75d653c18c 293
inky 0:4a75d653c18c 294 void RovComms::Debug(const char *format, ...)
inky 0:4a75d653c18c 295 {
inky 0:4a75d653c18c 296 // send data to the debug device
inky 0:4a75d653c18c 297 }
inky 0:4a75d653c18c 298
inky 0:4a75d653c18c 299 // end of file