RasPi/CopterController/echoServerAdvanced.cpp

131 lines
3.4 KiB
C++
Raw Normal View History

// For use with v1 GroundStation
2015-02-06 15:16:40 -07:00
#include <stdio.h>
#include <stdlib.h>
#include <cstring>
2015-02-06 15:16:40 -07:00
#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 "wiringPi.h"
#include "PCA9685.h"
#define MOTOR_MIN 200
#define MOTOR_MAX 600
#define SERVO_FREQ 60
int sockfd, newsockfd;
socklen_t clilen;
char buffer[256];
struct sockaddr_in serv_addr, cli_addr;
int nread, nwrite;
void setupTCP( int portno )
{
printf( "Setting up socket\n" );
sockfd = socket( AF_INET, SOCK_STREAM, 0 );
if( sockfd < 0 )
printf( "ERROR opening socket" );
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(portno);
printf( "Binding socket to portno\n" );
if( bind( sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr) ) < 0 )
printf("ERROR on binding");
printf( "Listening\n" );
listen( sockfd, 5 );
clilen = sizeof(cli_addr);
printf( "Waiting for connection\n" );
newsockfd = accept( sockfd,
(struct sockaddr *) &cli_addr,
&clilen );
if( newsockfd < 0 )
printf( "ERROR on accept" );
printf( "Connected\n" );
bzero( buffer, 256 );
fcntl( newsockfd, F_SETFL, O_NONBLOCK );
}
int main( int argc, char* argv[] )
{
setupTCP( 51717 );
wiringPiSetup();
PCA9685 servo = PCA9685( 0x40 );
servo.setFrequency( SERVO_FREQ );
servo.setPwm( 0, 0, MOTOR_MIN );
servo.setPwm( 1, 0, MOTOR_MIN );
servo.setPwm( 2, 0, MOTOR_MIN );
servo.setPwm( 3, 0, MOTOR_MIN );
while( true )
{
nread = read( newsockfd, buffer, 255 );
if( nread < 0 )
{
//printf( "ERROR on read" );
}
else if( nread > 0 )
2015-02-06 15:16:40 -07:00
{
nwrite = write( newsockfd, buffer, strlen(buffer) );
if( nwrite < 0 )
printf( "ERROR on write" );
//PARSE
printf( "Just read: %s", buffer );
if( strncmp( buffer, "DISCONNECT", strlen("DISCONNECT") ) == 0 )
{
printf( "Closing Socket\n" );
close( newsockfd );
close( sockfd );
return 0;
}
else if( strncmp( buffer, "HEARTBEAT", strlen("HEARTBEAT") ) == 0 ) // hearbeat transmission
{
// format:
// HEARTBEAT FCENABLE_x
// FCENABLE_x flight controller enable/disable x=0: disabled x=1: enabled
}
else if( strncmp( buffer, "SET_MOTOR_", strlen("SET_MOTOR_") ) == 0 )
{
int motor = atoi( buffer + 10 );
int speed = atoi( buffer + 13 );
2015-02-06 15:16:40 -07:00
if( speed < MOTOR_MIN )
{
speed = MOTOR_MIN;
}
else if( speed > MOTOR_MAX )
{
speed = MOTOR_MAX;
}
if( motor >= 0 && motor <= 3 )
{
printf( "setting motor %d to %d\n", motor, speed );
servo.setPwm( motor, 0, speed );
delay(5);
printf( "motor %d set to %d\n", motor, speed );
}
}
bzero( buffer, 256 );
nread = 0;
2015-02-06 15:16:40 -07:00
}
delay(5);
}
}