Ian Key
UDP comms to ROV
RovComms.cpp@2:48524892982c, 2012-01-16 (annotated)
- Committer:
- inky
- Date:
- Mon Jan 16 17:32:54 2012 +0000
- Revision:
- 2:48524892982c
- Parent:
- 1:f51739cada16
Who changed what in which revision?
| User | Revision | Line number | New 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 | 2:48524892982c | 140 | Receive(); |
| inky | 2:48524892982c | 141 | //Host client; |
| inky | 2:48524892982c | 142 | //int len = UdpSkt->recvfrom(iRxData, MAX_BUFFER_SIZE, &client); |
| inky | 1:f51739cada16 | 143 | //printf("Connected from %d.%d.%d.%d\r\n", (unsigned char)client->getIp()[0], (unsigned char)client->getIp()[1], |
| inky | 1:f51739cada16 | 144 | // (unsigned char)client->getIp()[2], (unsigned char)client->getIp()[3]); |
| inky | 1:f51739cada16 | 145 | break; |
| inky | 1:f51739cada16 | 146 | } |
| inky | 0:4a75d653c18c | 147 | |
| inky | 1:f51739cada16 | 148 | default: |
| inky | 1:f51739cada16 | 149 | break; |
| inky | 1:f51739cada16 | 150 | } // end switch |
| inky | 1:f51739cada16 | 151 | } |
| inky | 0:4a75d653c18c | 152 | |
| inky | 0:4a75d653c18c | 153 | |
| inky | 0:4a75d653c18c | 154 | void RovComms::ResetValues() |
| inky | 0:4a75d653c18c | 155 | { |
| inky | 0:4a75d653c18c | 156 | iRovSpeed = 0; |
| inky | 0:4a75d653c18c | 157 | iRovTurn = 0; |
| inky | 0:4a75d653c18c | 158 | iRovVert = 0; |
| inky | 0:4a75d653c18c | 159 | |
| inky | 0:4a75d653c18c | 160 | iLight = 0; |
| inky | 0:4a75d653c18c | 161 | iWaterSol = 0; |
| inky | 0:4a75d653c18c | 162 | iAirSol = 0; |
| inky | 0:4a75d653c18c | 163 | iCamera = 0; |
| inky | 0:4a75d653c18c | 164 | iPressure = 0; |
| inky | 0:4a75d653c18c | 165 | iCompass = 0; |
| inky | 0:4a75d653c18c | 166 | iAccelX = 0; |
| inky | 0:4a75d653c18c | 167 | iAccelY = 0; |
| inky | 0:4a75d653c18c | 168 | iAccelZ = 0; |
| inky | 0:4a75d653c18c | 169 | } |
| inky | 0:4a75d653c18c | 170 | |
| inky | 0:4a75d653c18c | 171 | void RovComms::RovPropulsion(int speed, int turn, int vert) |
| inky | 0:4a75d653c18c | 172 | { |
| inky | 0:4a75d653c18c | 173 | iRovSpeed = 0; |
| inky | 0:4a75d653c18c | 174 | iRovTurn = 0; |
| inky | 0:4a75d653c18c | 175 | iRovVert = 0; |
| inky | 0:4a75d653c18c | 176 | Transmit(CMD_ROV); |
| inky | 0:4a75d653c18c | 177 | } |
| inky | 0:4a75d653c18c | 178 | |
| inky | 0:4a75d653c18c | 179 | void RovComms::FrontLights(int state) |
| inky | 0:4a75d653c18c | 180 | { |
| inky | 0:4a75d653c18c | 181 | if (state == 1) |
| inky | 0:4a75d653c18c | 182 | iLight |= AUX_FN_LIGHT1_ON; |
| inky | 0:4a75d653c18c | 183 | else |
| inky | 0:4a75d653c18c | 184 | iLight &= ~AUX_FN_LIGHT1_ON; |
| inky | 0:4a75d653c18c | 185 | Transmit(CMD_AUX); |
| inky | 0:4a75d653c18c | 186 | } |
| inky | 0:4a75d653c18c | 187 | |
| inky | 0:4a75d653c18c | 188 | void RovComms::RearLights(int state) |
| inky | 0:4a75d653c18c | 189 | { |
| inky | 0:4a75d653c18c | 190 | if (state == 1) |
| inky | 0:4a75d653c18c | 191 | iLight |= AUX_FN_LIGHT2_ON; |
| inky | 0:4a75d653c18c | 192 | else |
| inky | 0:4a75d653c18c | 193 | iLight &= ~AUX_FN_LIGHT2_ON; |
| inky | 0:4a75d653c18c | 194 | Transmit(CMD_AUX); |
| inky | 0:4a75d653c18c | 195 | } |
| inky | 0:4a75d653c18c | 196 | |
| inky | 0:4a75d653c18c | 197 | void RovComms::Camera(int cam) |
| inky | 0:4a75d653c18c | 198 | { |
| inky | 0:4a75d653c18c | 199 | if (cam == 1) |
| inky | 0:4a75d653c18c | 200 | iCamera = AUX_FN_CAM1_ON; |
| inky | 0:4a75d653c18c | 201 | else |
| inky | 0:4a75d653c18c | 202 | iCamera = AUX_FN_CAM2_ON; |
| inky | 0:4a75d653c18c | 203 | Transmit(CMD_AUX); |
| inky | 0:4a75d653c18c | 204 | } |
| inky | 0:4a75d653c18c | 205 | |
| inky | 0:4a75d653c18c | 206 | void RovComms::Debug(const char *format, ...) |
| inky | 0:4a75d653c18c | 207 | { |
| inky | 0:4a75d653c18c | 208 | // send data to the debug device |
| inky | 0:4a75d653c18c | 209 | } |
| inky | 0:4a75d653c18c | 210 | |
| inky | 0:4a75d653c18c | 211 | // end of file |