mirror of
https://github.com/brendanhaines/RasPi.git
synced 2024-09-19 19:55:50 -06:00
181 lines
4.6 KiB
C++
181 lines
4.6 KiB
C++
#include <iostream>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <unistd.h>
|
|
#include <sys/types.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <netdb.h>
|
|
#include <sys/ioctl.h>
|
|
#include "fcntl.h"
|
|
|
|
#include "DSM2.h"
|
|
|
|
using namespace std;
|
|
|
|
// TCP vars
|
|
#define TCP_PORT 51714
|
|
int sockfd, newsockfd;
|
|
char buffer[ 256 ];
|
|
struct sockaddr_in serv_addr, cli_addr;
|
|
socklen_t clilen;
|
|
|
|
// DSM2 receiver vars
|
|
const int RX_MAX[] = { 900, 900, 900, 900, 900, 900 };
|
|
const int RX_CENTER[] = { 511, 511, 511, 511, 511, 511 };
|
|
const int RX_MIN[] = { 122, 122, 122, 122, 122, 122 };
|
|
DSM2 dsm;
|
|
|
|
// Status vars
|
|
bool motorsEnabled;
|
|
int motorValues[ 4 ];
|
|
//Quaternion q;
|
|
|
|
bool setupTcp( int portNum )
|
|
{
|
|
struct sockaddr_in serv_addr, cli_addr;
|
|
socklen_t clilen;
|
|
|
|
cout << "setting up TCP..." << endl;
|
|
|
|
sockfd = socket( AF_INET, SOCK_STREAM, 0 );
|
|
if( sockfd < 0 )
|
|
{
|
|
cerr << "ERROR opening socket" << endl;
|
|
return false;
|
|
}
|
|
|
|
bzero( (char*) &serv_addr, sizeof(serv_addr) );
|
|
serv_addr.sin_family = AF_INET;
|
|
serv_addr.sin_addr.s_addr = INADDR_ANY;
|
|
serv_addr.sin_port = htons(portNum);
|
|
|
|
cout << "binding socket to port " << portNum << endl;
|
|
if( bind( sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr) ) )
|
|
{
|
|
cerr << "ERROR binding socket to port " << portNum << endl;
|
|
return false;
|
|
}
|
|
|
|
cout << "listening to socket" << endl;
|
|
listen( sockfd, 5 );
|
|
clilen = sizeof( cli_addr );
|
|
|
|
cout << "waiting for connection" << endl;
|
|
newsockfd = accept( sockfd, (struct sockaddr *) &cli_addr, &clilen );
|
|
if( newsockfd < 0 )
|
|
{
|
|
cerr << "ERROR on accept" << endl;
|
|
return false;
|
|
}
|
|
|
|
bzero( buffer, 256 );
|
|
fcntl( newsockfd, F_SETFL, O_NONBLOCK );
|
|
|
|
cout << "connection established" << endl;
|
|
return true;
|
|
}
|
|
|
|
void shutdown()
|
|
{
|
|
cout << "shutting down..." << endl;
|
|
|
|
close( newsockfd );
|
|
close( sockfd );
|
|
cout << "sockets closed" << endl;
|
|
|
|
exit( EXIT_SUCCESS );
|
|
}
|
|
|
|
void readTcp()
|
|
{
|
|
int n; // number of bytes read
|
|
int carotPos;
|
|
n = read( newsockfd, buffer, 255 );
|
|
if( n > 0 )
|
|
{
|
|
motorsEnabled = false;
|
|
carotPos = 0;
|
|
while( carotPos < n )
|
|
{
|
|
if( strncmp( buffer + carotPos, "DISCONNECT", strlen("DISCONNECT") ) == 0 && strlen("DISCONNECT") + carotPos <= n ) shutdown();
|
|
else if( strncmp( buffer + carotPos, "D", strlen("D") ) == 0 && strlen("D") + carotPos <= n ) shutdown();
|
|
else if( strncmp( buffer + carotPos, "H", strlen("H") ) == 0 && strlen("H") + carotPos <= n )
|
|
{
|
|
cout << "H " << flush;
|
|
carotPos++;
|
|
}
|
|
else if( strncmp( buffer + carotPos, "E", strlen("E") ) == 0 && strlen("E") + carotPos <= n )
|
|
{
|
|
motorsEnabled = true;
|
|
cout << "E " << flush;
|
|
carotPos++;
|
|
// carotPos += strlen("E");
|
|
}
|
|
else if( strncmp( buffer + carotPos, "M", strlen("M") ) == 0 && strlen("Mxx_xxxx") + carotPos <= n )
|
|
{
|
|
motorValues[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 );
|
|
carotPos += strlen("Mxx_xxxx");
|
|
}
|
|
/*else if( strncmp( buffer + carotPos, "C", strlen("C") ) == 0 && strlen("Cxx_xxxx") + carotPos <= n )
|
|
{
|
|
dsm.values[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 );
|
|
carotPos += strlen("Cxx_xxxx");
|
|
}*/
|
|
else carotPos++;
|
|
}
|
|
cout << endl;
|
|
bzero( buffer, 256 );
|
|
if( !motorsEnabled )
|
|
{
|
|
motorValues[ 0 ] = 0;
|
|
motorValues[ 1 ] = 0;
|
|
motorValues[ 2 ] = 0;
|
|
motorValues[ 3 ] = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
void writeTcp()
|
|
{
|
|
int i;
|
|
char temp[256];
|
|
char outBuffer[] = "H ";
|
|
|
|
if( motorsEnabled ) {
|
|
strcat( outBuffer, "E " );
|
|
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
sprintf( temp, "M%02d_%04d ", i, motorValues[ i ] );
|
|
strcat( outBuffer, temp );
|
|
}
|
|
}
|
|
|
|
for( i = 0; i < 6; i++ )
|
|
{
|
|
sprintf( temp, "C%02d_%04d ", i, dsm.values[ i ] );
|
|
strcat( outBuffer, temp );
|
|
}
|
|
}
|
|
|
|
int main( int argc, char* argv[] )
|
|
{
|
|
//if( !setupTcp( TCP_PORT ) ) exit( EXIT_FAILURE );
|
|
setupTcp( TCP_PORT );
|
|
while( true )
|
|
{
|
|
readTcp();
|
|
cout << "EN: " << motorsEnabled << "\tM0: " << motorValues[0] << "\tM1: " << motorValues[1] << "\tM2: " << motorValues[2] << "\tM3: " << motorValues[3] << endl;
|
|
writeTcp();
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|