Alex Naylor / Mbed 2 deprecated FullIMUscriptsTCP

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?

UserRevisionLine numberNew 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 }