Ian Key
UDP comms to ROV
Diff: RovComms.cpp
- Revision:
- 0:4a75d653c18c
- Child:
- 1:f51739cada16
diff -r 000000000000 -r 4a75d653c18c RovComms.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RovComms.cpp Fri Jan 13 20:24:19 2012 +0000
@@ -0,0 +1,299 @@
+
+#include "RovComms.h"
+
+
+extern void debug(char *format,...);
+extern void debugInt(const char *format, int i);
+
+
+Ticker readNetwork;
+void readNetworkKick()
+{
+ Net::poll();
+} // readNetworkKick()
+
+
+RovComms::RovComms(int addr)
+{
+ ResetValues();
+
+#ifdef STATIC_IP_ADDR
+ rovAddr = IpAddr(10,0,0,addr);
+ eth = new EthernetNetIf(rovAddr, ipMask, ipGateway, ipDns);
+#else
+ eth = new EthernetNetIf(); // uses DHCP
+#endif
+
+ RxNetworkState = TxNetworkState = INIT;
+ readNetwork.attach(&readNetworkKick, 0.1); // every 100ms read
+ EthernetErr ethErr = eth->setup();
+ if(ethErr)
+ {
+ printf("Error %d in setup.\n", ethErr);
+ //return -1;
+ }
+ InitRx(addr);
+ if (addr == ROV_ADDR)
+ InitTx(HANDSET_ADDR);
+ else
+ InitTx(ROV_ADDR);
+} // RovComms()
+
+void RovComms::InitRx(int addr)
+{
+ TCPSocketErr err;
+ RxSock = new TCPSocket();
+ RxSock->setOnEvent(this, &RovComms::onRxTCPSocketEvent);
+ err = RxSock->bind(Host(IpAddr(), RX_TCP_LISTENING_PORT));
+ if(err)
+ {
+ //Deal with that error...
+ } else {
+ // set the socket to listen
+ err = RxSock->listen(); //Starts listening
+ if (err)
+ CloseSocket(RxSock, &RxNetworkState);
+ }
+}
+
+void RovComms::CloseSocket(TCPSocket *s, int *state)
+{
+ s->close();
+ *state = CLOSED;
+}
+
+
+void RovComms::InitTx(int addr)
+{
+ TCPSocketErr bindErr;
+ Host server(IpAddr(10,0,0,addr), RX_TCP_LISTENING_PORT); // ip addr and port nos
+
+ bindErr = TxSock->connect(server);
+ switch(bindErr)
+ {
+ case TCPSOCKET_CONNECTED:
+ TxNetworkState = CONNECTED;
+ debug("RovComms::InitSlave -> connected");
+ break;
+
+ case TCPSOCKET_CONTIMEOUT:
+ case TCPSOCKET_CONRST:
+ case TCPSOCKET_CONABRT:
+ case TCPSOCKET_ERROR:
+ default:
+ CloseSocket(TxSock, &TxNetworkState);
+ break;
+ } // switch
+}
+
+/* @fn Transmit(char *data)
+ * Sends the buffer to the other end
+ */
+void RovComms::Transmit(int pktType)
+{
+ ClearBuffer(iTxData);
+
+ iTxData[0] = pktType;
+ switch (pktType)
+ {
+ case CMD_ROV:
+ iTxData[ROV_SPEED_POS] = iRovSpeed;
+ iTxData[ROV_TURN_POS] = iRovTurn;
+ iTxData[ROV_VERT_POS] = iRovVert;
+ break;
+
+ case CMD_AUX:
+ iTxData[AUX_CMD_POS] = 0;
+ iTxData[AUX_LIGHT_POS] = iLight;
+ iTxData[AUX_CAM_POS] = iCamera;
+ iTxData[AUX_SOLENOID_POS] = iWaterSol + iAirSol;
+ break;
+
+ case CMD_STATS:
+ iTxData[STATS_COMPASS_POS] = 23;
+ iTxData[STATS_PRESSURE_POS] = 4;
+ iTxData[STATS_X_ACCEL_POS] = 1;
+ iTxData[STATS_Y_ACCEL_POS] = 1;
+ iTxData[STATS_Z_ACCEL_POS] = 0;
+ iTxData[STATS_TEMPERATURE_POS] = 20;
+ iTxData[STATS_MISC_POS] = 69;
+ break;
+
+ case CMD_CLAW:
+ iTxData[1] = 1;
+ iTxData[2] = 2;
+ iTxData[3] = 3;
+ iTxData[4] = 4;
+ break;
+
+ case CMD_PTZ:
+ iTxData[1] = 5;
+ iTxData[2] = 6;
+ iTxData[3] = 7;
+ iTxData[4] = 8;
+ break;
+
+ default:
+ return;
+ } // switch pktType
+
+ int errCode = TxSock->send(iTxData, MAX_BUFFER_SIZE);
+ if (errCode < 0) // TCPSocketErr
+ {
+ }
+ Net::poll(); // tells stack to process the info (transfer from here to the stack and tx it)
+}
+
+char *RovComms::Receive(void)
+{
+ char *buf={0};
+ int errCode = 0;
+ Net::poll(); // tell the stack to transfer the data from it's stack to the readable one here
+ errCode = RxSock->recv(buf, MAX_BUFFER_SIZE);
+ if (errCode < 0) // TCPSocketErr
+ {
+ }
+ // decode the packet.....
+ debugInt("Pkt type = ", buf[0]);
+ for (int i=1;i<MAX_BUFFER_SIZE;i++)
+ debugInt("Pkt data = ", buf[i]);
+ return buf;
+}
+
+
+void RovComms::onConnectedTCPSocketEvent(TCPSocketEvent e)
+{
+ switch(e)
+ {
+ case TCPSOCKET_ACCEPT:
+ break;
+
+ case TCPSOCKET_READABLE:
+ Receive();
+ break;
+
+ default:
+ break;
+ }
+}
+
+void RovComms::onRxTCPSocketEvent(TCPSocketEvent e)
+{
+ TCPSocket* pConnectedSock;
+ Host client;
+ TCPSocketErr err;
+ switch (e)
+ {
+ case TCPSOCKET_ACCEPT:
+ err = RxSock->accept(&client, &pConnectedSock);
+ if(!err)
+ pConnectedSock->setOnEvent(this, &RovComms::onConnectedTCPSocketEvent); //Setup the new socket events case TCPSOCKET_READABLE: //The only event for now
+ break;
+
+ case TCPSOCKET_READABLE:
+ Receive();
+ break;
+
+ case TCPSOCKET_WRITEABLE:
+ // continue the write process
+ break;
+
+ case TCPSOCKET_CONNECTED:
+ case TCPSOCKET_CONTIMEOUT:
+ case TCPSOCKET_CONRST:
+ case TCPSOCKET_CONABRT:
+ case TCPSOCKET_ERROR:
+ default:
+ break;
+ } // switch(e)
+} // onRxTCPSocketEvent()
+
+// Slave socket event handler
+void RovComms::onTxTCPSocketEvent(TCPSocketEvent e)
+{
+ TCPSocket* pConnectedSock;
+ Host client;
+ TCPSocketErr err;
+ switch (e)
+ {
+ case TCPSOCKET_ACCEPT:
+ // this event should never happen as a slave!!!
+ break;
+
+ case TCPSOCKET_READABLE:
+ // don't think this event should happen
+ Receive();
+ break;
+
+ case TCPSOCKET_WRITEABLE:
+ // continue the write process
+ break;
+
+ case TCPSOCKET_CONNECTED:
+ case TCPSOCKET_CONTIMEOUT:
+ case TCPSOCKET_CONRST:
+ case TCPSOCKET_CONABRT:
+ case TCPSOCKET_ERROR:
+ default:
+ break;
+ } // switch(e)
+} // onSlaveTCPSocketEvent()
+
+void RovComms::ResetValues()
+{
+ iRovSpeed = 0;
+ iRovTurn = 0;
+ iRovVert = 0;
+
+ iLight = 0;
+ iWaterSol = 0;
+ iAirSol = 0;
+ iCamera = 0;
+ iPressure = 0;
+ iCompass = 0;
+ iAccelX = 0;
+ iAccelY = 0;
+ iAccelZ = 0;
+}
+
+void RovComms::RovPropulsion(int speed, int turn, int vert)
+{
+ iRovSpeed = 0;
+ iRovTurn = 0;
+ iRovVert = 0;
+ Transmit(CMD_ROV);
+}
+
+void RovComms::FrontLights(int state)
+{
+ if (state == 1)
+ iLight |= AUX_FN_LIGHT1_ON;
+ else
+ iLight &= ~AUX_FN_LIGHT1_ON;
+ Transmit(CMD_AUX);
+}
+
+void RovComms::RearLights(int state)
+{
+ if (state == 1)
+ iLight |= AUX_FN_LIGHT2_ON;
+ else
+ iLight &= ~AUX_FN_LIGHT2_ON;
+ Transmit(CMD_AUX);
+}
+
+void RovComms::Camera(int cam)
+{
+ if (cam == 1)
+ iCamera = AUX_FN_CAM1_ON;
+ else
+ iCamera = AUX_FN_CAM2_ON;
+ Transmit(CMD_AUX);
+}
+
+void RovComms::Debug(const char *format, ...)
+{
+ // send data to the debug device
+}
+
+// end of file
\ No newline at end of file