working version 'oneThatWorks.cpp' fixes bug in DSM2 library and adds MPU6050 implementation to oneThatWorks.cpp

CopterController/DSM2.cpp

@@ -1,6 +1,7 @@
 #include "DSM2.h"
 
 #include <iostream>
+#include <cstdio>
 #include "wiringSerial.h"
 
 #define VALUE_MASK 0x3FF
@@ -28,12 +29,21 @@ void DSM2::update()
 
     if( serialDataAvail(fd) < 16 ) return;
 
-    while( serialDataAvail(fd) > 32 )
+
-        for( i = 0; i < 16; i++ )
+    while( serialDataAvail(fd) > 16 )
-            serialGetchar(fd);
+        read( fd, raw, 16 );
+
+    //while( serialDataAvail(fd) > 32 )
+        //for( i = 0; i < 16; i++ )
+            //read(fd);
+
+    //for( i = 0; i < 8; i++ )
+        //data[ i ] = ( (int)(serialGetchar(fd)) << 8 ) + serialGetchar(fd);
 
     for( i = 0; i < 8; i++ )
-        data[ i ] = ( (int)(serialGetchar(fd)) << 8 ) + serialGetchar(fd);
+        data[i] = ( ((int)raw[2*i]) << 8 ) + raw[ 2*i+1 ];
+
+    //printf( "%04X %04X %04X %04X %04X %04X %04X %04X\n", data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7] );
 
     *(values + 0) = data[7] & VALUE_MASK; //Throttle
     *(values + 1) = data[1] & VALUE_MASK; //Aileron
@@ -41,5 +51,7 @@ void DSM2::update()
     *(values + 3) = data[5] & VALUE_MASK; //Rudder
     *(values + 4) = data[4] & VALUE_MASK; //Gear
     *(values + 5) = data[2] & VALUE_MASK; //Aux1
+
+    //printf( "%04X %04X %04X %04X %04X %04X\n", values[0], values[1], values[2], values[3], values[4], values[5] );
     return;
 }

CopterController/DSM2.h

@@ -7,12 +7,13 @@ public:
     DSM2();
 
     void update();
-    int values[7];
+    int values[6];
 private:
     int fd;
     int valueMask;
     char thisByte, lastByte;
-    int data[7];
+    int data[8];
+    char raw[16];
 };
 
 #endif

CopterController/DSM2tester.cpp

@@ -9,7 +9,7 @@ int main(int argc, char const *argv[])
     while( true )
     {
         dsm.update();
-        cout << "THRO " << dsm.values[0] << "\tAILE " << dsm.values[1] << "\tELEV " << dsm.values[2] << "\tRUDD " << dsm.values[3] << "\tGEAR " << dsm.values[4] << "\tAUX1 " << dsm.values[5] << endl;
+        //cout << "THRO " << dsm.values[0] << "\tAILE " << dsm.values[1] << "\tELEV " << dsm.values[2] << "\tRUDD " << dsm.values[3] << "\tGEAR " << dsm.values[4] << "\tAUX1 " << dsm.values[5] << endl;
     }
     return 0;
 }

CopterController/oneThatWorks.cpp

@@ -3,6 +3,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <unistd.h>
+//#include <stdint.h>
 #include <sys/types.h> 
 #include <sys/socket.h>
 #include <netinet/in.h>
@@ -13,6 +14,7 @@
 #include "PCA9685.h"
 #include "DSM2.h"
 #include "PID.h"
+//#include "I2Cdev.h"
 #include "MPU6050_6Axis_MotionApps20.h"
 #include "wiringPi.h"
 
@@ -34,6 +36,16 @@ DSM2 dsm;   // also initializes
 // MPU stuff
 MPU6050 mpu;
 
+int packetSize;
+int fifoCount;
+uint8_t fifoBuffer[64];
+
+Quaternion q;
+VectorFloat gravity;
+float pitchAngle;
+float rollAngle;
+int16_t yawRate;
+
 // PCA9685 stuff
 PCA9685 pca;
 #define MOTOR_OFF 200
@@ -43,6 +55,7 @@ PCA9685 pca;
 int motorValues[4];
 bool motorsEnabled;
 bool motorTesting;
+int throttle = 300;
 
 // PID stuff
 PID pitch, roll, yaw;
@@ -114,6 +127,45 @@ void setupPCA()
         pca.setPwm( i, 0, MOTOR_OFF );
 }
 
+void setupMPU()
+{
+    mpu = MPU6050();
+    cout << "\nsetting up MPU6050..." << endl;
+    cout << "initializing MPU6050..." << endl;
+    mpu.initialize();
+    cout << "testing MPU6050 connection..." << flush;
+    if( mpu.testConnection() )
+    {
+        cout << "SUCCESS" << endl;
+    }
+    else
+    {
+        cout << "FAILURE" << endl;
+        exit( EXIT_FAILURE );
+    }
+    cout << "initializing DMP..." << flush;
+    if( mpu.dmpInitialize() == 0 )
+    {
+        cout << "SUCCESS" << endl;
+        cout << "Enabling DMP..." << endl;
+        mpu.setDMPEnabled( true );
+        packetSize = mpu.dmpGetFIFOPacketSize();
+    }
+    else
+    {
+        cout << "FAILURE" << endl;
+        exit( EXIT_FAILURE );
+    }
+    cout << "MPU6050 set up" << endl;
+}
+
+void setupPID()
+{
+    pitch = PID( pitchP, pitchI, pitchD );
+    roll = PID( rollP, rollI, rollD );
+    yaw = PID( yawP, yawI, yawD );
+}
+
 void readTCP()
 {
     uint8_t n;  // number of bytes read
@@ -164,33 +216,124 @@ void readTCP()
     return;
 }
 
+bool updateMPU()
+{
+    fifoCount = mpu.getFIFOCount();
+    if( fifoCount == 1024 )
+    {
+        mpu.resetFIFO();
+        cout << "FIFO overflow" << endl;
+    }
+    else if( fifoCount >= 42 )
+    {
+        while( fifoCount >= 84 )
+        {
+            mpu.getFIFOBytes( fifoBuffer, packetSize );
+            fifoCount = mpu.getFIFOCount();
+        }
+
+        mpu.getFIFOBytes( fifoBuffer, packetSize );
+        mpu.dmpGetQuaternion( &q, fifoBuffer );
+        mpu.dmpGetGravity( &gravity, &q );
+
+        pitchAngle = atan( gravity.x / sqrt( gravity.y * gravity.y + gravity.z * gravity.z) );
+        rollAngle = atan( gravity.y / sqrt( gravity.x * gravity.x + gravity.z * gravity.z ) );
+        yawRate = mpu.getRotationZ();
+        return true;
+    }
+    return false;
+}
+
 void updatePID()
-{}
+{
+    float pitchMod, rollMod, yawMod;
+
+    pitchMod = pitch.update( pitchAngle, 0 );
+    rollMod = roll.update( rollAngle, 0 );
+    yawMod = yaw.update( yawRate, 0 );
+
+    motorValues[0] = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * ( 1 + yawMod ) * throttle );
+    motorValues[1] = (int)( ( 1 + pitchMod ) * ( 1 - rollMod ) * ( 1 - yawMod ) * throttle );
+    motorValues[2] = (int)( ( 1 - pitchMod ) * ( 1 + rollMod ) * ( 1 - yawMod ) * throttle );
+    motorValues[3] = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * ( 1 + yawMod ) * throttle );
+}
 
 // main
 int main(int argc, char const *argv[])
 {
     int i;
+    bool wasPID = false;
 
-    setupTCP( TCP_PORT );
     setupPCA();
+    setupTCP( TCP_PORT );
+    setupMPU();
 
     // label lines
-    cout << "\n**REAL TIME DATA**" << endl;
+    cout << "\n**REAL TIME DATA**\n" << endl;
-    cout << "\nRC RECEIVER\t\t\t\t\tMOTORS" << endl;
+    cout << "\t";   // FIFO
-    cout << "THRO\tAILE\tELEV\tRUDD\tGEAR\tAUX1\tENABLE\tTEST\tM0  \tM1  \tM2  \tM3" << endl;
+    cout << "RC RECEIVER\t\t\t\t\t";
+    cout << "MOTORS\t\t\t\t\t\t";
+    cout << "ORIENTATION\t";
+    cout << endl;
+
+    cout << "FIFO\t";
+    cout << "THRO\tAILE\tELEV\tRUDD\tGEAR\tAUX1\t";
+    cout << "ENABLE\tTEST\tM0  \tM1  \tM2  \tM3\t";
+    cout << "PITCH\t\tROLL\t\tYAW\t";
+    cout << endl;
+
     while( true )
     {
         dsm.update();
         readTCP();
 
+        fifoCount = mpu.getFIFOCount();
+
+        updateMPU();
+
         if( motorsEnabled && !motorTesting )
-            updatePID();
+        {
+            if( wasPID )
+            {
+                updatePID();
+            }
+            else
+            {
+                setupPID();
+                wasPID = true;
+            }
+        }
+        else
+        {
+            wasPID = false;
+        }
         
+        // constrain motor values if enabled: MOTOR_MIN < value < MOTOR_MAX
+        if( motorsEnabled && !motorTesting )
+        {
+            for( i = 0; i < 4; i++ )
+            {
+                if( motorValues[i] > MOTOR_MAX )
+                    motorValues[i] = MOTOR_MAX;
+                else if( motorValues[i] < MOTOR_MIN )
+                    motorValues[i] = MOTOR_MIN;
+            }
+        }
+        else if( !motorTesting )
+        {
+            for( i = 0; i < 4; i++ )
+                motorValues[i] = MOTOR_OFF;
+        }
+
         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;
+        cout << "\r";
+        cout << fifoCount << "\t";
+        cout << dsm.values[0] << "  \t" << dsm.values[1] << "  \t" << dsm.values[2] << "  \t" << dsm.values[3] << "  \t" << dsm.values[4] << "  \t" << dsm.values[5] << "\t";
+        cout << motorsEnabled << "  \t" << motorTesting << "  \t" << motorValues[0] << "  \t" << motorValues[1] << "  \t" << motorValues[2] << "  \t" << motorValues[3] << "  \t";
+        printf( "%+1.5f\t%+1.5f\t%+06d\t", pitchAngle, rollAngle, yawRate );
+        cout << flush;
         delay(100);
     }