UDP comms to ROV
RovComms.cpp
- Committer:
- inky
- Date:
- 2012-01-16
- Revision:
- 1:f51739cada16
- Parent:
- 0:4a75d653c18c
- Child:
- 2:48524892982c
File content as of revision 1:f51739cada16:
#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; } debug("Setting up Tx"); if (addr == ROV_ADDR) InitUDP(HANDSET_ADDR); else InitUDP(ROV_ADDR); debug("ROV Comms done"); } // RovComms() void RovComms::InitUDP(int addr) { UdpSkt = new UDPSocket(); server = Host( IpAddr(/*10,0,0,addr*/), RX_TCP_LISTENING_PORT, NULL); UdpSkt->setOnEvent(this, &RovComms::onUDPSocketEvent); UdpSkt->bind(server); } void RovComms::CloseSocket(UDPSocket *s, int *state) { s->close(); *state = CLOSED; } /* @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 = UdpSkt->sendto(iTxData, MAX_BUFFER_SIZE, NULL); if (errCode < 0) // UDPSocketErr { } Net::poll(); // tells stack to process the info (transfer from here to the stack and tx it) } char *RovComms::Receive(void) { int errCode = 0; Net::poll(); // tell the stack to transfer the data from it's stack to the readable one here errCode = UdpSkt->recvfrom(iRxData, MAX_BUFFER_SIZE, &server); if (errCode < 0) // TCPSocketErr { } // decode the packet..... debugInt("Pkt type = ", iRxData[0]); for (int i=1;i<MAX_BUFFER_SIZE;i++) debugInt("Pkt data = ", iRxData[i]); return iRxData; } void RovComms::onUDPSocketEvent(UDPSocketEvent e) { switch (e) { case UDPSOCKET_READABLE: { Host client; int len = UdpSkt->recvfrom(iRxData, MAX_BUFFER_SIZE, &client); //printf("Connected from %d.%d.%d.%d\r\n", (unsigned char)client->getIp()[0], (unsigned char)client->getIp()[1], // (unsigned char)client->getIp()[2], (unsigned char)client->getIp()[3]); break; } default: break; } // end switch } 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