working rc receiver, tcp rx, and motor control. oneThatWorks.cpp

CopterController/DSM2.cpp

@@ -28,6 +28,10 @@ void DSM2::update()
 
     if( serialDataAvail(fd) < 16 ) return;
 
+    while( serialDataAvail(fd) > 32 )
+        for( i = 0; i < 16; i++ )
+            serialGetchar(fd);
+
     for( i = 0; i < 8; i++ )
         data[ i ] = ( (int)(serialGetchar(fd)) << 8 ) + serialGetchar(fd);
 

CopterController/Makefile

@@ -1,4 +1,6 @@
-all: PID.o I2Cdev.o MPU6050.o PCA9685.o DSM2.o PID.o
+all: libraries oneThatWorks
+
+libraries: PID.o I2Cdev.o MPU6050.o PCA9685.o DSM2.o PID.o v2Parser.o
 
 PCA_HDRS = PCA9685.h PCA9685_Addresses.h
 MPU_HDRS = helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.h
@@ -38,5 +40,9 @@ SatelliteReceiver.o SatelliteReceiverTest.o: SatelliteReceiver.h
 SatelliteReceiverTest: SatelliteReceiver.o SatelliteReceiverTest.o
 	$(CXX) $^ -lwiringPi -o $@
 
+oneThatWorks.o: $(PCA_HDRS) $(MPU_HDRS) $(DSM_HDRS) $(PID_HDRS)
+oneThatWorks: oneThatWorks.o PID.o I2Cdev.o MPU6050.o PCA9685.o DSM2.o PID.o
+	$(CXX) $^ -lwiringPi -o $@
+
 clean:
 	rm I2Cdev.o MPU6050.o MPU6050dmp.o MPU6050dmp_tester.o MPU6050dmp_tester PID.o main.o main PCA9685tester

CopterController/PCA9685.cpp

@@ -10,9 +10,7 @@
 #include "wiringPiI2C.h"
 
 PCA9685::PCA9685()
-{
-    PCA9685( PCA9685_DEFAULT_ADDRESS );
-}
+{}
 
 PCA9685::PCA9685( int devAddr )
 {

CopterController/oneThatWorks.cpp

@@ -0,0 +1,208 @@
+#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 "PCA9685.h"
+#include "DSM2.h"
+#include "PID.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "wiringPi.h"
+
+using namespace std;
+
+// TCP vars
+#define TCP_PORT 51719
+int sockfd, newsockfd;
+char buffer[ 256 ];
+struct sockaddr_in serv_addr, cli_addr;
+socklen_t clilen;
+
+// DSM2 stuff
+const int RX_MAX[]    = { 922, 923, 922, 921, 919, 919 };
+const int RX_CENTER[] = { 511, 511, 511, 511, 511, 511 };
+const int RX_MIN[]    = { 100, 103, 103, 101, 103, 103 };
+DSM2 dsm;   // also initializes
+
+// MPU stuff
+MPU6050 mpu;
+
+// PCA9685 stuff
+PCA9685 pca;
+#define MOTOR_OFF 200
+#define MOTOR_MIN 220
+#define MOTOR_MAX 600
+
+int motorValues[4];
+bool motorsEnabled;
+bool motorTesting;
+
+// PID stuff
+PID pitch, roll, yaw;
+double pitchP   = 2;
+double pitchI   = 0;
+double pitchD   = 0;
+double rollP    = 2;
+double rollI    = 0;
+double rollD    = 0;
+double yawP     = 0.0001;
+double yawI     = 0;
+double yawD     = 0;
+
+// functions
+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 setupPCA()
+{
+    int i;
+    pca = PCA9685(0x40);
+    pca.setFrequency( 60 );
+    for( i = 0; i < 4; i++ )
+        pca.setPwm( i, 0, MOTOR_OFF );
+    delay(250);
+    for( i = 0; i < 4; i++ )
+        pca.setPwm( i, 0, MOTOR_OFF );
+}
+
+void readTCP()
+{
+    uint8_t n;  // number of bytes read
+    uint8_t carotPos;
+    n = read( newsockfd, buffer, 255 );
+    if( n > 0 )
+    {
+        motorsEnabled = false;
+        motorTesting = false;
+        carotPos = 0;
+        while( carotPos < n )
+        {
+            if( strncmp( buffer + carotPos, "DISCONNECT", strlen("DISCONNECT") ) == 0 && strlen("DISCONNECT") + carotPos <= n )
+            {
+                cout << endl;
+                exit( EXIT_SUCCESS );
+            }
+            else if( strncmp( buffer + carotPos, "D", strlen("D") ) == 0 && strlen("D") + carotPos <= n )
+            {
+                cout << endl;
+                exit( EXIT_SUCCESS );
+            }
+            else if( strncmp( buffer + carotPos, "H", strlen("H") ) == 0 && strlen("H") + carotPos <= n ) carotPos++;
+            else if( strncmp( buffer + carotPos, "E", strlen("E") ) == 0 && strlen("E") + carotPos <= n )
+            {
+                motorsEnabled = true;
+                carotPos++;
+            }
+            else if( strncmp( buffer + carotPos, "M", strlen("M") ) == 0 && strlen("Mxx_xxxx") + carotPos <= n )
+            {
+                motorsEnabled = true;
+                motorTesting = true;
+                //cout << atoi( buffer + carotPos + 1 ) << "\t" << atoi( buffer + carotPos + 4 ) << endl;
+                motorValues[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 ) * (MOTOR_MAX - MOTOR_OFF) / 100 + MOTOR_OFF;
+                carotPos += strlen("Mxx_xxxx");
+            }
+            else carotPos++;
+        }
+        bzero( buffer, 256 );
+        if( !motorsEnabled )
+        {
+            motorValues[ 0 ] = MOTOR_OFF;
+            motorValues[ 1 ] = MOTOR_OFF;
+            motorValues[ 2 ] = MOTOR_OFF;
+            motorValues[ 3 ] = MOTOR_OFF;
+        }
+    }
+    return;
+}
+
+void updatePID()
+{}
+
+// main
+int main(int argc, char const *argv[])
+{
+    int i;
+
+    setupTCP( TCP_PORT );
+    setupPCA();
+
+    // label lines
+    cout << "\n**REAL TIME DATA**" << endl;
+    cout << "\nRC RECEIVER\t\t\t\t\tMOTORS" << endl;
+    cout << "THRO\tAILE\tELEV\tRUDD\tGEAR\tAUX1\tENABLE\tTEST\tM0  \tM1  \tM2  \tM3" << endl;
+    while( true )
+    {
+        dsm.update();
+        readTCP();
+
+        if( motorsEnabled && !motorTesting )
+            updatePID();
+        
+        for( i = 0; i < 4; i++ )
+            pca.setPwm( i, 0, motorValues[i] );
+
+        cout << "\r" << dsm.values[0] << "  \t" << dsm.values[1] << "  \t" << dsm.values[2] << "  \t" << dsm.values[3] << "  \t" << dsm.values[4] << "  \t" << dsm.values[5] << "  \t" << motorsEnabled << "  \t" << motorTesting << "  \t" << motorValues[0] << "  \t" << motorValues[1] << "  \t" << motorValues[2] << "  \t" << motorValues[3] << "  " << flush;
+        delay(100);
+    }
+
+    return 0;
+}
+
+
+
+
+
+
+
+
+
+