First commit
Dependencies: EthernetInterface FXAS21002 FXOS8700 NetworkAPI mbed-rtos mbed
main.cpp@14:f73e284ebe86, 2016-09-14 (annotated)
- Committer:
- AlexNaylor
- Date:
- Wed Sep 14 23:50:29 2016 +0000
- Revision:
- 14:f73e284ebe86
- Parent:
- 12:4257f5720975
First commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
donatien | 0:bb128f0e952f | 1 | #include "mbed.h" |
donatien | 0:bb128f0e952f | 2 | #include "EthernetInterface.h" |
AlexNaylor | 12:4257f5720975 | 3 | |
AlexNaylor | 12:4257f5720975 | 4 | #include "FXAS21002.h" |
AlexNaylor | 12:4257f5720975 | 5 | #include "FXOS8700.h" |
NegativeBlack | 11:90554d22ade5 | 6 | |
NegativeBlack | 9:a4c85bea2d77 | 7 | #include "NetworkAPI/buffer.hpp" |
NegativeBlack | 11:90554d22ade5 | 8 | #include "NetworkAPI/select.hpp" |
NegativeBlack | 11:90554d22ade5 | 9 | #include "NetworkAPI/ip/address.hpp" |
NegativeBlack | 9:a4c85bea2d77 | 10 | #include "NetworkAPI/tcp/socket.hpp" |
NegativeBlack | 9:a4c85bea2d77 | 11 | using namespace network; |
NegativeBlack | 11:90554d22ade5 | 12 | |
NegativeBlack | 11:90554d22ade5 | 13 | #define MAX_CLIENTS 5 |
AlexNaylor | 12:4257f5720975 | 14 | #define TCP_PORT 55 |
AlexNaylor | 12:4257f5720975 | 15 | |
AlexNaylor | 12:4257f5720975 | 16 | // Initialize RGB LEDs |
AlexNaylor | 12:4257f5720975 | 17 | DigitalOut RGBLED1(LED1); |
AlexNaylor | 12:4257f5720975 | 18 | DigitalOut RGBLED2(LED2); |
AlexNaylor | 12:4257f5720975 | 19 | DigitalOut RGBLED3(LED3); |
AlexNaylor | 12:4257f5720975 | 20 | |
AlexNaylor | 12:4257f5720975 | 21 | int initPulses = 15; |
AlexNaylor | 12:4257f5720975 | 22 | |
AlexNaylor | 12:4257f5720975 | 23 | // Initialize Serial port |
AlexNaylor | 12:4257f5720975 | 24 | Serial pc(USBTX, USBRX); |
AlexNaylor | 12:4257f5720975 | 25 | |
AlexNaylor | 12:4257f5720975 | 26 | // Initialize pins for I2C communication for sensors. Set jumpers J6,J7 in FRDM-STBC-AGM01 board accordingly. |
AlexNaylor | 12:4257f5720975 | 27 | FXOS8700 accel(D14,D15); |
AlexNaylor | 12:4257f5720975 | 28 | FXOS8700 mag(D14,D15); |
AlexNaylor | 12:4257f5720975 | 29 | FXAS21002 gyro(D14,D15); |
AlexNaylor | 12:4257f5720975 | 30 | |
AlexNaylor | 12:4257f5720975 | 31 | float accel_data[3]; |
AlexNaylor | 12:4257f5720975 | 32 | float mag_data[3]; |
AlexNaylor | 12:4257f5720975 | 33 | float gyro_data[3]; |
NegativeBlack | 11:90554d22ade5 | 34 | |
AlexNaylor | 12:4257f5720975 | 35 | // Ethernet initialization and connection return values |
AlexNaylor | 12:4257f5720975 | 36 | int retInit; |
AlexNaylor | 12:4257f5720975 | 37 | int retConn; |
AlexNaylor | 12:4257f5720975 | 38 | |
AlexNaylor | 12:4257f5720975 | 39 | int main() { |
AlexNaylor | 12:4257f5720975 | 40 | // Start with the LEDs off |
AlexNaylor | 12:4257f5720975 | 41 | RGBLED1 = 1; |
AlexNaylor | 12:4257f5720975 | 42 | RGBLED2 = 1; |
AlexNaylor | 12:4257f5720975 | 43 | RGBLED3 = 1; |
AlexNaylor | 12:4257f5720975 | 44 | |
AlexNaylor | 12:4257f5720975 | 45 | // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 & Gyroscope FXAS21002 |
AlexNaylor | 12:4257f5720975 | 46 | accel.accel_config(); |
AlexNaylor | 12:4257f5720975 | 47 | mag.mag_config(); |
AlexNaylor | 12:4257f5720975 | 48 | gyro.gyro_config(); |
AlexNaylor | 12:4257f5720975 | 49 | |
AlexNaylor | 12:4257f5720975 | 50 | // Set baud rate |
AlexNaylor | 12:4257f5720975 | 51 | pc.baud(115200); |
AlexNaylor | 12:4257f5720975 | 52 | pc.printf("Attempting to connect to the network\r\n"); |
AlexNaylor | 12:4257f5720975 | 53 | |
AlexNaylor | 12:4257f5720975 | 54 | // Intialize Ethernet connection |
AlexNaylor | 12:4257f5720975 | 55 | EthernetInterface eth; |
AlexNaylor | 12:4257f5720975 | 56 | |
AlexNaylor | 12:4257f5720975 | 57 | pc.printf("Initializing connection..."); |
AlexNaylor | 12:4257f5720975 | 58 | retInit = eth.init("10.12.1.55","255.255.0.0","10.12.0.1"); //Use a static IP address |
AlexNaylor | 12:4257f5720975 | 59 | |
AlexNaylor | 12:4257f5720975 | 60 | if (retInit == 0) |
AlexNaylor | 12:4257f5720975 | 61 | { |
AlexNaylor | 12:4257f5720975 | 62 | pc.printf("OK (IP = %s)\r\n", eth.getIPAddress()); |
AlexNaylor | 12:4257f5720975 | 63 | |
AlexNaylor | 12:4257f5720975 | 64 | for (int a = 0; a < initPulses; a = a+1) |
AlexNaylor | 12:4257f5720975 | 65 | { |
AlexNaylor | 12:4257f5720975 | 66 | RGBLED1 = 1; |
AlexNaylor | 12:4257f5720975 | 67 | RGBLED2 = 1; |
AlexNaylor | 12:4257f5720975 | 68 | wait(0.025); |
AlexNaylor | 12:4257f5720975 | 69 | RGBLED1 = 0; |
AlexNaylor | 12:4257f5720975 | 70 | RGBLED2 = 0; |
AlexNaylor | 12:4257f5720975 | 71 | wait(0.025); |
AlexNaylor | 12:4257f5720975 | 72 | } |
AlexNaylor | 12:4257f5720975 | 73 | |
AlexNaylor | 12:4257f5720975 | 74 | pc.printf("Connecting..."); |
AlexNaylor | 12:4257f5720975 | 75 | retConn = eth.connect(); |
AlexNaylor | 12:4257f5720975 | 76 | |
AlexNaylor | 12:4257f5720975 | 77 | if (retConn == -1) |
AlexNaylor | 12:4257f5720975 | 78 | //if (retConn == 0) |
AlexNaylor | 12:4257f5720975 | 79 | { |
AlexNaylor | 12:4257f5720975 | 80 | pc.printf("OK\r\n"); |
AlexNaylor | 12:4257f5720975 | 81 | pc.printf("Success! Device with IP Address %s is connected to network!\r\n", eth.getIPAddress()); |
AlexNaylor | 12:4257f5720975 | 82 | |
AlexNaylor | 12:4257f5720975 | 83 | RGBLED1 = 1; |
AlexNaylor | 12:4257f5720975 | 84 | |
AlexNaylor | 12:4257f5720975 | 85 | for (int a = 0; a < initPulses; a = a+1) |
AlexNaylor | 12:4257f5720975 | 86 | { |
AlexNaylor | 12:4257f5720975 | 87 | RGBLED2 = 1; |
AlexNaylor | 12:4257f5720975 | 88 | wait(0.025); |
AlexNaylor | 12:4257f5720975 | 89 | RGBLED2 = 0; |
AlexNaylor | 12:4257f5720975 | 90 | wait(0.025); |
AlexNaylor | 12:4257f5720975 | 91 | } |
AlexNaylor | 12:4257f5720975 | 92 | } |
AlexNaylor | 12:4257f5720975 | 93 | else |
AlexNaylor | 12:4257f5720975 | 94 | { |
AlexNaylor | 12:4257f5720975 | 95 | pc.printf("FAILED\r\n"); |
AlexNaylor | 12:4257f5720975 | 96 | pc.printf("Connection failure! Device not connected to network!\r\n", eth.getIPAddress()); |
AlexNaylor | 12:4257f5720975 | 97 | |
AlexNaylor | 12:4257f5720975 | 98 | RGBLED1 = 0; |
AlexNaylor | 12:4257f5720975 | 99 | } |
AlexNaylor | 12:4257f5720975 | 100 | } |
AlexNaylor | 12:4257f5720975 | 101 | else |
AlexNaylor | 12:4257f5720975 | 102 | { |
AlexNaylor | 12:4257f5720975 | 103 | pc.printf("FAILED\r\n"); |
AlexNaylor | 12:4257f5720975 | 104 | pc.printf("Initialization failure! Device not connected to network!\r\n", eth.getIPAddress()); |
AlexNaylor | 12:4257f5720975 | 105 | |
AlexNaylor | 12:4257f5720975 | 106 | RGBLED1 = 0; |
AlexNaylor | 12:4257f5720975 | 107 | } |
NegativeBlack | 11:90554d22ade5 | 108 | |
NegativeBlack | 11:90554d22ade5 | 109 | Select select; |
NegativeBlack | 9:a4c85bea2d77 | 110 | tcp::Socket server; |
NegativeBlack | 11:90554d22ade5 | 111 | tcp::Socket client[MAX_CLIENTS]; |
NegativeBlack | 11:90554d22ade5 | 112 | tcp::Socket *socket = NULL; |
NegativeBlack | 11:90554d22ade5 | 113 | |
NegativeBlack | 11:90554d22ade5 | 114 | int result = 0; |
NegativeBlack | 11:90554d22ade5 | 115 | int index = 0; |
AlexNaylor | 12:4257f5720975 | 116 | |
AlexNaylor | 12:4257f5720975 | 117 | char imuValStr [1024]; |
AlexNaylor | 12:4257f5720975 | 118 | char welcomeMsgStr [128] = "Connected to IMU"; |
NegativeBlack | 11:90554d22ade5 | 119 | |
AlexNaylor | 12:4257f5720975 | 120 | network::Buffer inbuffer(256); |
AlexNaylor | 12:4257f5720975 | 121 | network::Buffer outbuffer(256); |
NegativeBlack | 11:90554d22ade5 | 122 | |
NegativeBlack | 11:90554d22ade5 | 123 | // Configure the server socket (assume everty thing works) |
NegativeBlack | 11:90554d22ade5 | 124 | server.open(); |
AlexNaylor | 12:4257f5720975 | 125 | server.bind(TCP_PORT); |
AlexNaylor | 12:4257f5720975 | 126 | pc.printf("TCP Port: %d\r\n\n", TCP_PORT); |
NegativeBlack | 11:90554d22ade5 | 127 | server.listen(MAX_CLIENTS); |
NegativeBlack | 11:90554d22ade5 | 128 | |
NegativeBlack | 11:90554d22ade5 | 129 | // Add sockets to the select api |
NegativeBlack | 11:90554d22ade5 | 130 | select.set(&server, Select::Read); |
NegativeBlack | 11:90554d22ade5 | 131 | for (index = 0; index < MAX_CLIENTS; index++) { |
NegativeBlack | 11:90554d22ade5 | 132 | select.set(&client[index], Select::Read); |
NegativeBlack | 10:9e8d5928537a | 133 | } |
NegativeBlack | 11:90554d22ade5 | 134 | |
NegativeBlack | 11:90554d22ade5 | 135 | do { |
NegativeBlack | 11:90554d22ade5 | 136 | // Wait for activity |
NegativeBlack | 11:90554d22ade5 | 137 | result = select.wait(); |
NegativeBlack | 11:90554d22ade5 | 138 | if (result < -1) { |
AlexNaylor | 12:4257f5720975 | 139 | pc.printf("Failed to select\n\r"); |
NegativeBlack | 11:90554d22ade5 | 140 | break; |
NegativeBlack | 9:a4c85bea2d77 | 141 | } |
NegativeBlack | 11:90554d22ade5 | 142 | |
NegativeBlack | 11:90554d22ade5 | 143 | // Get the first socket |
NegativeBlack | 11:90554d22ade5 | 144 | socket = (tcp::Socket *)select.getReadable(); |
NegativeBlack | 11:90554d22ade5 | 145 | |
NegativeBlack | 11:90554d22ade5 | 146 | for (; socket != NULL; socket = (tcp::Socket *)select.getReadable()) { |
NegativeBlack | 11:90554d22ade5 | 147 | // Check if there was a connection request. |
NegativeBlack | 11:90554d22ade5 | 148 | if (socket->getHandle() == server.getHandle()) { |
NegativeBlack | 11:90554d22ade5 | 149 | // Find an unused client |
NegativeBlack | 11:90554d22ade5 | 150 | for (index = 0; index < MAX_CLIENTS; index++) { |
NegativeBlack | 11:90554d22ade5 | 151 | if (client[index].getStatus() == network::Socket::Closed) { |
NegativeBlack | 11:90554d22ade5 | 152 | break; |
NegativeBlack | 11:90554d22ade5 | 153 | } |
NegativeBlack | 11:90554d22ade5 | 154 | } |
NegativeBlack | 11:90554d22ade5 | 155 | |
NegativeBlack | 11:90554d22ade5 | 156 | // Maximum connections reached |
NegativeBlack | 11:90554d22ade5 | 157 | if (index == MAX_CLIENTS) { |
AlexNaylor | 12:4257f5720975 | 158 | pc.printf("Maximum connections reached\n\r"); |
NegativeBlack | 11:90554d22ade5 | 159 | continue; |
NegativeBlack | 11:90554d22ade5 | 160 | } |
NegativeBlack | 11:90554d22ade5 | 161 | |
NegativeBlack | 11:90554d22ade5 | 162 | // Accept the client |
NegativeBlack | 11:90554d22ade5 | 163 | socket->accept(client[index]); |
AlexNaylor | 12:4257f5720975 | 164 | pc.printf("Client connected %s:%d\n\r", |
NegativeBlack | 11:90554d22ade5 | 165 | client[index].getRemoteEndpoint().getAddress().toString().c_str(), |
NegativeBlack | 11:90554d22ade5 | 166 | client[index].getRemoteEndpoint().getPort()); |
AlexNaylor | 12:4257f5720975 | 167 | |
AlexNaylor | 12:4257f5720975 | 168 | // Send data to the client |
AlexNaylor | 12:4257f5720975 | 169 | client[index].write(welcomeMsgStr,strlen(welcomeMsgStr)); |
NegativeBlack | 11:90554d22ade5 | 170 | continue; |
NegativeBlack | 11:90554d22ade5 | 171 | } |
NegativeBlack | 11:90554d22ade5 | 172 | |
NegativeBlack | 11:90554d22ade5 | 173 | // It was not the server socket, so it must be a client talking to us. |
AlexNaylor | 12:4257f5720975 | 174 | switch (socket->read(inbuffer)) { |
NegativeBlack | 10:9e8d5928537a | 175 | case 0: |
NegativeBlack | 11:90554d22ade5 | 176 | // Remote end disconnected |
AlexNaylor | 12:4257f5720975 | 177 | pc.printf("Client disconnected %s:%d\n\r", |
NegativeBlack | 11:90554d22ade5 | 178 | socket->getRemoteEndpoint().getAddress().toString().c_str(), |
NegativeBlack | 11:90554d22ade5 | 179 | socket->getRemoteEndpoint().getPort()); |
NegativeBlack | 11:90554d22ade5 | 180 | |
NegativeBlack | 11:90554d22ade5 | 181 | // Close socket |
NegativeBlack | 11:90554d22ade5 | 182 | socket->close(); |
NegativeBlack | 9:a4c85bea2d77 | 183 | break; |
NegativeBlack | 11:90554d22ade5 | 184 | |
NegativeBlack | 11:90554d22ade5 | 185 | case -1: |
AlexNaylor | 12:4257f5720975 | 186 | pc.printf("Error while reading data from socket\n\r"); |
NegativeBlack | 11:90554d22ade5 | 187 | socket->close(); |
NegativeBlack | 11:90554d22ade5 | 188 | break; |
NegativeBlack | 11:90554d22ade5 | 189 | |
NegativeBlack | 9:a4c85bea2d77 | 190 | default: |
AlexNaylor | 12:4257f5720975 | 191 | pc.printf("Message from %s:%d\n\r", |
NegativeBlack | 11:90554d22ade5 | 192 | socket->getRemoteEndpoint().getAddress().toString().c_str(), |
NegativeBlack | 11:90554d22ade5 | 193 | socket->getRemoteEndpoint().getPort()); |
NegativeBlack | 11:90554d22ade5 | 194 | |
AlexNaylor | 12:4257f5720975 | 195 | pc.printf("%s\n\r", (char *)inbuffer.data()); |
AlexNaylor | 12:4257f5720975 | 196 | |
AlexNaylor | 12:4257f5720975 | 197 | // Get IMU data |
AlexNaylor | 12:4257f5720975 | 198 | accel.acquire_accel_data_g(accel_data); |
AlexNaylor | 12:4257f5720975 | 199 | gyro.acquire_gyro_data_dps(gyro_data); |
AlexNaylor | 12:4257f5720975 | 200 | mag.acquire_mag_data_uT(mag_data); |
AlexNaylor | 12:4257f5720975 | 201 | |
AlexNaylor | 12:4257f5720975 | 202 | // Print IMU data to serial port |
AlexNaylor | 12:4257f5720975 | 203 | pc.printf("Message to %s:%d", |
AlexNaylor | 12:4257f5720975 | 204 | socket->getRemoteEndpoint().getAddress().toString().c_str(), |
AlexNaylor | 12:4257f5720975 | 205 | socket->getRemoteEndpoint().getPort()); |
AlexNaylor | 12:4257f5720975 | 206 | pc.printf("ACC(g):%4.4f,%4.4f,%4.4f|GYRO(deg/s):%4.4f,%4.4f,%4.4f|MAG(uT):%4.4f,%4.4f,%4.4f\n", |
AlexNaylor | 12:4257f5720975 | 207 | accel_data[0], |
AlexNaylor | 12:4257f5720975 | 208 | accel_data[1], |
AlexNaylor | 12:4257f5720975 | 209 | accel_data[2], |
AlexNaylor | 12:4257f5720975 | 210 | gyro_data[0], |
AlexNaylor | 12:4257f5720975 | 211 | gyro_data[1], |
AlexNaylor | 12:4257f5720975 | 212 | gyro_data[2], |
AlexNaylor | 12:4257f5720975 | 213 | mag_data[0], |
AlexNaylor | 12:4257f5720975 | 214 | mag_data[1], |
AlexNaylor | 12:4257f5720975 | 215 | mag_data[2]); |
AlexNaylor | 12:4257f5720975 | 216 | sprintf(imuValStr,"ACC(g):%4.4f,%4.4f,%4.4f|GYRO(deg/s):%4.4f,%4.4f,%4.4f|MAG(uT):%4.4f,%4.4f,%4.4f\n", |
AlexNaylor | 12:4257f5720975 | 217 | accel_data[0], |
AlexNaylor | 12:4257f5720975 | 218 | accel_data[1], |
AlexNaylor | 12:4257f5720975 | 219 | accel_data[2], |
AlexNaylor | 12:4257f5720975 | 220 | gyro_data[0], |
AlexNaylor | 12:4257f5720975 | 221 | gyro_data[1], |
AlexNaylor | 12:4257f5720975 | 222 | gyro_data[2], |
AlexNaylor | 12:4257f5720975 | 223 | mag_data[0], |
AlexNaylor | 12:4257f5720975 | 224 | mag_data[1], |
AlexNaylor | 12:4257f5720975 | 225 | mag_data[2]); |
AlexNaylor | 12:4257f5720975 | 226 | |
AlexNaylor | 12:4257f5720975 | 227 | // Send data to the client |
AlexNaylor | 12:4257f5720975 | 228 | client[index].write(imuValStr,strlen(imuValStr)); |
NegativeBlack | 11:90554d22ade5 | 229 | break; |
NegativeBlack | 9:a4c85bea2d77 | 230 | } |
NegativeBlack | 9:a4c85bea2d77 | 231 | } |
NegativeBlack | 11:90554d22ade5 | 232 | |
NegativeBlack | 11:90554d22ade5 | 233 | } while (server.getStatus() == network::Socket::Listening); |
NegativeBlack | 6:33b57f606f2b | 234 | } |