Ian Key / RovComms

UDP comms to ROV

RovComms.cpp@1:f51739cada16, 2012-01-16 (annotated)

Committer:
inky
Date:
Mon Jan 16 17:17:58 2012 +0000
Revision:
1:f51739cada16
Parent:
0:4a75d653c18c
Child:
2:48524892982c
UDP

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 1:f51739cada16 35 debug("Setting up Tx");
inky 0:4a75d653c18c 36 if (addr == ROV_ADDR)
inky 1:f51739cada16 37 InitUDP(HANDSET_ADDR);
inky 0:4a75d653c18c 38 else
inky 1:f51739cada16 39 InitUDP(ROV_ADDR);
inky 1:f51739cada16 40
inky 1:f51739cada16 41 debug("ROV Comms done");
inky 0:4a75d653c18c 42 } // RovComms()
inky 0:4a75d653c18c 43
inky 1:f51739cada16 44
inky 1:f51739cada16 45 void RovComms::InitUDP(int addr)
inky 0:4a75d653c18c 46 {
inky 1:f51739cada16 47 UdpSkt = new UDPSocket();
inky 1:f51739cada16 48 server = Host( IpAddr(/*10,0,0,addr*/), RX_TCP_LISTENING_PORT, NULL);
inky 1:f51739cada16 49 UdpSkt->setOnEvent(this, &RovComms::onUDPSocketEvent);
inky 1:f51739cada16 50 UdpSkt->bind(server);
inky 0:4a75d653c18c 51 }
inky 0:4a75d653c18c 52
inky 1:f51739cada16 53
inky 1:f51739cada16 54 void RovComms::CloseSocket(UDPSocket *s, int *state)
inky 0:4a75d653c18c 55 {
inky 0:4a75d653c18c 56 s->close();
inky 0:4a75d653c18c 57 *state = CLOSED;
inky 0:4a75d653c18c 58 }
inky 0:4a75d653c18c 59
inky 0:4a75d653c18c 60
inky 0:4a75d653c18c 61 /* @fn Transmit(char *data)
inky 0:4a75d653c18c 62 * Sends the buffer to the other end
inky 0:4a75d653c18c 63 */
inky 0:4a75d653c18c 64 void RovComms::Transmit(int pktType)
inky 0:4a75d653c18c 65 {
inky 0:4a75d653c18c 66 ClearBuffer(iTxData);
inky 0:4a75d653c18c 67
inky 0:4a75d653c18c 68 iTxData[0] = pktType;
inky 0:4a75d653c18c 69 switch (pktType)
inky 0:4a75d653c18c 70 {
inky 0:4a75d653c18c 71 case CMD_ROV:
inky 0:4a75d653c18c 72 iTxData[ROV_SPEED_POS] = iRovSpeed;
inky 0:4a75d653c18c 73 iTxData[ROV_TURN_POS] = iRovTurn;
inky 0:4a75d653c18c 74 iTxData[ROV_VERT_POS] = iRovVert;
inky 0:4a75d653c18c 75 break;
inky 0:4a75d653c18c 76
inky 0:4a75d653c18c 77 case CMD_AUX:
inky 0:4a75d653c18c 78 iTxData[AUX_CMD_POS] = 0;
inky 0:4a75d653c18c 79 iTxData[AUX_LIGHT_POS] = iLight;
inky 0:4a75d653c18c 80 iTxData[AUX_CAM_POS] = iCamera;
inky 0:4a75d653c18c 81 iTxData[AUX_SOLENOID_POS] = iWaterSol + iAirSol;
inky 0:4a75d653c18c 82 break;
inky 0:4a75d653c18c 83
inky 0:4a75d653c18c 84 case CMD_STATS:
inky 0:4a75d653c18c 85 iTxData[STATS_COMPASS_POS] = 23;
inky 0:4a75d653c18c 86 iTxData[STATS_PRESSURE_POS] = 4;
inky 0:4a75d653c18c 87 iTxData[STATS_X_ACCEL_POS] = 1;
inky 0:4a75d653c18c 88 iTxData[STATS_Y_ACCEL_POS] = 1;
inky 0:4a75d653c18c 89 iTxData[STATS_Z_ACCEL_POS] = 0;
inky 0:4a75d653c18c 90 iTxData[STATS_TEMPERATURE_POS] = 20;
inky 0:4a75d653c18c 91 iTxData[STATS_MISC_POS] = 69;
inky 0:4a75d653c18c 92 break;
inky 0:4a75d653c18c 93
inky 0:4a75d653c18c 94 case CMD_CLAW:
inky 0:4a75d653c18c 95 iTxData[1] = 1;
inky 0:4a75d653c18c 96 iTxData[2] = 2;
inky 0:4a75d653c18c 97 iTxData[3] = 3;
inky 0:4a75d653c18c 98 iTxData[4] = 4;
inky 0:4a75d653c18c 99 break;
inky 0:4a75d653c18c 100
inky 0:4a75d653c18c 101 case CMD_PTZ:
inky 0:4a75d653c18c 102 iTxData[1] = 5;
inky 0:4a75d653c18c 103 iTxData[2] = 6;
inky 0:4a75d653c18c 104 iTxData[3] = 7;
inky 0:4a75d653c18c 105 iTxData[4] = 8;
inky 0:4a75d653c18c 106 break;
inky 0:4a75d653c18c 107
inky 0:4a75d653c18c 108 default:
inky 0:4a75d653c18c 109 return;
inky 0:4a75d653c18c 110 } // switch pktType
inky 0:4a75d653c18c 111
inky 1:f51739cada16 112 int errCode = UdpSkt->sendto(iTxData, MAX_BUFFER_SIZE, NULL);
inky 1:f51739cada16 113 if (errCode < 0) // UDPSocketErr
inky 0:4a75d653c18c 114 {
inky 0:4a75d653c18c 115 }
inky 0:4a75d653c18c 116 Net::poll(); // tells stack to process the info (transfer from here to the stack and tx it)
inky 0:4a75d653c18c 117 }
inky 0:4a75d653c18c 118
inky 0:4a75d653c18c 119 char *RovComms::Receive(void)
inky 0:4a75d653c18c 120 {
inky 0:4a75d653c18c 121 int errCode = 0;
inky 0:4a75d653c18c 122 Net::poll(); // tell the stack to transfer the data from it's stack to the readable one here
inky 1:f51739cada16 123 errCode = UdpSkt->recvfrom(iRxData, MAX_BUFFER_SIZE, &server);
inky 0:4a75d653c18c 124 if (errCode < 0) // TCPSocketErr
inky 0:4a75d653c18c 125 {
inky 0:4a75d653c18c 126 }
inky 0:4a75d653c18c 127 // decode the packet.....
inky 1:f51739cada16 128 debugInt("Pkt type = ", iRxData[0]);
inky 0:4a75d653c18c 129 for (int i=1;i<MAX_BUFFER_SIZE;i++)
inky 1:f51739cada16 130 debugInt("Pkt data = ", iRxData[i]);
inky 1:f51739cada16 131 return iRxData;
inky 0:4a75d653c18c 132 }
inky 0:4a75d653c18c 133
inky 1:f51739cada16 134 void RovComms::onUDPSocketEvent(UDPSocketEvent e)
inky 0:4a75d653c18c 135 {
inky 1:f51739cada16 136 switch (e)
inky 1:f51739cada16 137 {
inky 1:f51739cada16 138 case UDPSOCKET_READABLE:
inky 1:f51739cada16 139 {
inky 1:f51739cada16 140 Host client;
inky 1:f51739cada16 141 int len = UdpSkt->recvfrom(iRxData, MAX_BUFFER_SIZE, &client);
inky 1:f51739cada16 142 //printf("Connected from %d.%d.%d.%d\r\n", (unsigned char)client->getIp()[0], (unsigned char)client->getIp()[1],
inky 1:f51739cada16 143 // (unsigned char)client->getIp()[2], (unsigned char)client->getIp()[3]);
inky 1:f51739cada16 144 break;
inky 1:f51739cada16 145 }
inky 0:4a75d653c18c 146
inky 1:f51739cada16 147 default:
inky 1:f51739cada16 148 break;
inky 1:f51739cada16 149 } // end switch
inky 1:f51739cada16 150 }
inky 0:4a75d653c18c 151
inky 0:4a75d653c18c 152
inky 0:4a75d653c18c 153 void RovComms::ResetValues()
inky 0:4a75d653c18c 154 {
inky 0:4a75d653c18c 155 iRovSpeed = 0;
inky 0:4a75d653c18c 156 iRovTurn = 0;
inky 0:4a75d653c18c 157 iRovVert = 0;
inky 0:4a75d653c18c 158
inky 0:4a75d653c18c 159 iLight = 0;
inky 0:4a75d653c18c 160 iWaterSol = 0;
inky 0:4a75d653c18c 161 iAirSol = 0;
inky 0:4a75d653c18c 162 iCamera = 0;
inky 0:4a75d653c18c 163 iPressure = 0;
inky 0:4a75d653c18c 164 iCompass = 0;
inky 0:4a75d653c18c 165 iAccelX = 0;
inky 0:4a75d653c18c 166 iAccelY = 0;
inky 0:4a75d653c18c 167 iAccelZ = 0;
inky 0:4a75d653c18c 168 }
inky 0:4a75d653c18c 169
inky 0:4a75d653c18c 170 void RovComms::RovPropulsion(int speed, int turn, int vert)
inky 0:4a75d653c18c 171 {
inky 0:4a75d653c18c 172 iRovSpeed = 0;
inky 0:4a75d653c18c 173 iRovTurn = 0;
inky 0:4a75d653c18c 174 iRovVert = 0;
inky 0:4a75d653c18c 175 Transmit(CMD_ROV);
inky 0:4a75d653c18c 176 }
inky 0:4a75d653c18c 177
inky 0:4a75d653c18c 178 void RovComms::FrontLights(int state)
inky 0:4a75d653c18c 179 {
inky 0:4a75d653c18c 180 if (state == 1)
inky 0:4a75d653c18c 181 iLight |= AUX_FN_LIGHT1_ON;
inky 0:4a75d653c18c 182 else
inky 0:4a75d653c18c 183 iLight &= ~AUX_FN_LIGHT1_ON;
inky 0:4a75d653c18c 184 Transmit(CMD_AUX);
inky 0:4a75d653c18c 185 }
inky 0:4a75d653c18c 186
inky 0:4a75d653c18c 187 void RovComms::RearLights(int state)
inky 0:4a75d653c18c 188 {
inky 0:4a75d653c18c 189 if (state == 1)
inky 0:4a75d653c18c 190 iLight |= AUX_FN_LIGHT2_ON;
inky 0:4a75d653c18c 191 else
inky 0:4a75d653c18c 192 iLight &= ~AUX_FN_LIGHT2_ON;
inky 0:4a75d653c18c 193 Transmit(CMD_AUX);
inky 0:4a75d653c18c 194 }
inky 0:4a75d653c18c 195
inky 0:4a75d653c18c 196 void RovComms::Camera(int cam)
inky 0:4a75d653c18c 197 {
inky 0:4a75d653c18c 198 if (cam == 1)
inky 0:4a75d653c18c 199 iCamera = AUX_FN_CAM1_ON;
inky 0:4a75d653c18c 200 else
inky 0:4a75d653c18c 201 iCamera = AUX_FN_CAM2_ON;
inky 0:4a75d653c18c 202 Transmit(CMD_AUX);
inky 0:4a75d653c18c 203 }
inky 0:4a75d653c18c 204
inky 0:4a75d653c18c 205 void RovComms::Debug(const char *format, ...)
inky 0:4a75d653c18c 206 {
inky 0:4a75d653c18c 207 // send data to the debug device
inky 0:4a75d653c18c 208 }
inky 0:4a75d653c18c 209
inky 0:4a75d653c18c 210 // end of file