MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Dependencies:   ros_lib_kinetic

HLComms.cpp@7:5b6a2cefbf3b, 2018-08-03 (annotated)

Committer:
WD40andTape
Date:
Fri Aug 03 14:58:36 2018 +0000
Revision:
7:5b6a2cefbf3b
Child:
9:cd3607ba5643
Moved HL comms into a separate class/file.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // HLComms.cpp
WD40andTape 7:5b6a2cefbf3b 2
WD40andTape 7:5b6a2cefbf3b 3 // Error codes: https://github.com/ARMmbed/mbed-os/blob/master/features/netsocket/nsapi_types.h
WD40andTape 7:5b6a2cefbf3b 4 // http://man7.org/linux/man-pages/man3/errno.3.html
WD40andTape 7:5b6a2cefbf3b 5
WD40andTape 7:5b6a2cefbf3b 6 #include "HLComms.h"
WD40andTape 7:5b6a2cefbf3b 7
WD40andTape 7:5b6a2cefbf3b 8 HLComms::HLComms(short int port) { // Constructor
WD40andTape 7:5b6a2cefbf3b 9 _port = port;
WD40andTape 7:5b6a2cefbf3b 10 }
WD40andTape 7:5b6a2cefbf3b 11
WD40andTape 7:5b6a2cefbf3b 12 int HLComms::setup_server(void) {
WD40andTape 7:5b6a2cefbf3b 13 int error_code;
WD40andTape 7:5b6a2cefbf3b 14 printf("Starting TCP server...\n\r");
WD40andTape 7:5b6a2cefbf3b 15 if( !IS_DHCP ) {
WD40andTape 7:5b6a2cefbf3b 16 const char* ip = "10.101.79.135"; //"10.101.81.125"; // Get first 3 parts from "ifconfig" on PC
WD40andTape 7:5b6a2cefbf3b 17 const char* mask = "255.255.255.0"; // Get from "ifconfig" on PC
WD40andTape 7:5b6a2cefbf3b 18 const char* gateway = "10.101.79.1"; //"10.101.81.1"; // Get from "route -n" on PC
WD40andTape 7:5b6a2cefbf3b 19 error_code = interfaces.eth.set_network(ip, mask, gateway); // Dont use DHCP
WD40andTape 7:5b6a2cefbf3b 20 if( error_code < 0 ) {
WD40andTape 7:5b6a2cefbf3b 21 printf("Error %i. Could not network interface to use a static IP address. "
WD40andTape 7:5b6a2cefbf3b 22 "Perhaps the network is already connected?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 23 return -1;
WD40andTape 7:5b6a2cefbf3b 24 }
WD40andTape 7:5b6a2cefbf3b 25 }
WD40andTape 7:5b6a2cefbf3b 26 error_code = interfaces.eth.connect();
WD40andTape 7:5b6a2cefbf3b 27 if( error_code < 0 ) {
WD40andTape 7:5b6a2cefbf3b 28 printf("Error %i. Could not start the Ethernet interface.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 29 return -1;
WD40andTape 7:5b6a2cefbf3b 30 }
WD40andTape 7:5b6a2cefbf3b 31 const char* server_ip = interfaces.eth.get_ip_address();
WD40andTape 7:5b6a2cefbf3b 32 if( server_ip == NULL ) {
WD40andTape 7:5b6a2cefbf3b 33 error_code = -1;
WD40andTape 7:5b6a2cefbf3b 34 printf("Error %i. Ethernet interface is not yet connected.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 35 return -1;
WD40andTape 7:5b6a2cefbf3b 36 }
WD40andTape 7:5b6a2cefbf3b 37 printf("The target IP address is '%s'\n\r", server_ip);
WD40andTape 7:5b6a2cefbf3b 38 error_code = interfaces.srv.open(&interfaces.eth); // Open the server on ethernet stack
WD40andTape 7:5b6a2cefbf3b 39 if( error_code < 0 ) {
WD40andTape 7:5b6a2cefbf3b 40 printf("Error %i. Could not open server socket.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 41 return -1;
WD40andTape 7:5b6a2cefbf3b 42 }
WD40andTape 7:5b6a2cefbf3b 43 error_code = interfaces.srv.bind(interfaces.eth.get_ip_address(), _port); // Bind the HTTP port (TCP 80) to the server
WD40andTape 7:5b6a2cefbf3b 44 if( error_code < 0 ) {
WD40andTape 7:5b6a2cefbf3b 45 printf("Error %i. Could not bind server socket to port '%d'.\n\r", error_code, _port);
WD40andTape 7:5b6a2cefbf3b 46 return -1;
WD40andTape 7:5b6a2cefbf3b 47 }
WD40andTape 7:5b6a2cefbf3b 48 error_code = interfaces.srv.listen(1); // Listen for 1 simultaneous connection
WD40andTape 7:5b6a2cefbf3b 49 if( error_code < 0 ) {
WD40andTape 7:5b6a2cefbf3b 50 printf("Error %i. Could not put server socket into listening mode.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 51 return -1;
WD40andTape 7:5b6a2cefbf3b 52 }
WD40andTape 7:5b6a2cefbf3b 53 return 0;
WD40andTape 7:5b6a2cefbf3b 54 }
WD40andTape 7:5b6a2cefbf3b 55
WD40andTape 7:5b6a2cefbf3b 56
WD40andTape 7:5b6a2cefbf3b 57 int HLComms::accept_connection(void) {
WD40andTape 7:5b6a2cefbf3b 58 printf("Waiting to accept a connection on the TCP socket.\n\r");
WD40andTape 7:5b6a2cefbf3b 59 int error_code;
WD40andTape 7:5b6a2cefbf3b 60 error_code = interfaces.srv.accept(&interfaces.clt_sock, &interfaces.clt_addr); // Blocks until data is sent
WD40andTape 7:5b6a2cefbf3b 61 if( error_code < 0 ) {
WD40andTape 7:5b6a2cefbf3b 62 printf("Error %i. Could not create a network socket using the specified socket instance. "
WD40andTape 7:5b6a2cefbf3b 63 "Perhaps the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 64 return -1;
WD40andTape 7:5b6a2cefbf3b 65 }
WD40andTape 7:5b6a2cefbf3b 66 printf("Accepted %s:%d\n\r", interfaces.clt_addr.get_ip_address(), interfaces.clt_addr.get_port());
WD40andTape 7:5b6a2cefbf3b 67 return 0;
WD40andTape 7:5b6a2cefbf3b 68 }
WD40andTape 7:5b6a2cefbf3b 69
WD40andTape 7:5b6a2cefbf3b 70
WD40andTape 7:5b6a2cefbf3b 71 void HLComms::close_server(void) {
WD40andTape 7:5b6a2cefbf3b 72 interfaces.clt_sock.close();
WD40andTape 7:5b6a2cefbf3b 73 interfaces.srv.close();
WD40andTape 7:5b6a2cefbf3b 74 interfaces.eth.disconnect();
WD40andTape 7:5b6a2cefbf3b 75 return;
WD40andTape 7:5b6a2cefbf3b 76 }
WD40andTape 7:5b6a2cefbf3b 77
WD40andTape 7:5b6a2cefbf3b 78
WD40andTape 7:5b6a2cefbf3b 79 msg_format HLComms::consume_message( unsigned char * msg ) { //HLComms::msg_format
WD40andTape 7:5b6a2cefbf3b 80 // Break message string into chunks at each comma
WD40andTape 7:5b6a2cefbf3b 81 vector<string> tokens;
WD40andTape 7:5b6a2cefbf3b 82 char * delim = ",";
WD40andTape 7:5b6a2cefbf3b 83 char * token = strtok((char *)msg, delim);
WD40andTape 7:5b6a2cefbf3b 84 while(token != NULL) {
WD40andTape 7:5b6a2cefbf3b 85 tokens.push_back(string(token));
WD40andTape 7:5b6a2cefbf3b 86 //printf("%s\n\r",token);
WD40andTape 7:5b6a2cefbf3b 87 token = strtok(NULL, delim);
WD40andTape 7:5b6a2cefbf3b 88 }
WD40andTape 7:5b6a2cefbf3b 89 // Cast into doubles and assign to variables
WD40andTape 7:5b6a2cefbf3b 90 struct msg_format input;
WD40andTape 7:5b6a2cefbf3b 91 stringstream(tokens[0]) >> input.psi[0][0];
WD40andTape 7:5b6a2cefbf3b 92 stringstream(tokens[1]) >> input.psi[0][1];
WD40andTape 7:5b6a2cefbf3b 93 stringstream(tokens[2]) >> input.psi[0][2];
WD40andTape 7:5b6a2cefbf3b 94 stringstream(tokens[3]) >> input.psi[1][0];
WD40andTape 7:5b6a2cefbf3b 95 stringstream(tokens[4]) >> input.psi[1][1];
WD40andTape 7:5b6a2cefbf3b 96 stringstream(tokens[5]) >> input.psi[1][2];
WD40andTape 7:5b6a2cefbf3b 97 stringstream(tokens[6]) >> input.psi[2][0];
WD40andTape 7:5b6a2cefbf3b 98 stringstream(tokens[7]) >> input.psi[2][1];
WD40andTape 7:5b6a2cefbf3b 99 stringstream(tokens[8]) >> input.psi[2][2];
WD40andTape 7:5b6a2cefbf3b 100 stringstream(tokens[9]) >> input.duration;
WD40andTape 7:5b6a2cefbf3b 101
WD40andTape 7:5b6a2cefbf3b 102 if(input.psi[0][0] < 0.0) input.psi[0][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 103 if(input.psi[0][1] < 0.0) input.psi[0][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 104 if(input.psi[0][2] < 0.0) input.psi[0][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 105 if(input.psi[1][0] < 0.0) input.psi[1][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 106 if(input.psi[1][1] < 0.0) input.psi[1][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 107 if(input.psi[1][2] < 0.0) input.psi[1][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 108 if(input.psi[2][0] < 0.0) input.psi[2][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 109 if(input.psi[2][1] < 0.0) input.psi[2][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 110 if(input.psi[2][2] < 0.0) input.psi[2][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 111 if(input.duration < 0.0) input.duration = 0.0;
WD40andTape 7:5b6a2cefbf3b 112 /*if( input.psi[2][0] > 0.14 ) {
WD40andTape 7:5b6a2cefbf3b 113 printf("Hi\n\r");
WD40andTape 7:5b6a2cefbf3b 114 }*/
WD40andTape 7:5b6a2cefbf3b 115 //throw std::invalid_argument( "received negative value" );
WD40andTape 7:5b6a2cefbf3b 116 //return 0;
WD40andTape 7:5b6a2cefbf3b 117 return input;
WD40andTape 7:5b6a2cefbf3b 118 } // End of consume_message()