diff --git a/CopterController/DSM2.cpp b/CopterController/DSM2.cpp index d7099dc..8313d8f 100644 --- a/CopterController/DSM2.cpp +++ b/CopterController/DSM2.cpp @@ -1,6 +1,7 @@ #include "DSM2.h" #include +#include #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++ ) - serialGetchar(fd); + + while( serialDataAvail(fd) > 16 ) + 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; } diff --git a/CopterController/DSM2.h b/CopterController/DSM2.h index 123c964..2a98ba2 100644 --- a/CopterController/DSM2.h +++ b/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 \ No newline at end of file diff --git a/CopterController/DSM2tester.cpp b/CopterController/DSM2tester.cpp index deae46d..39a8ea0 100644 --- a/CopterController/DSM2tester.cpp +++ b/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; } \ No newline at end of file diff --git a/CopterController/oneThatWorks.cpp b/CopterController/oneThatWorks.cpp index b2477b3..4c0f15b 100644 --- a/CopterController/oneThatWorks.cpp +++ b/CopterController/oneThatWorks.cpp @@ -3,6 +3,7 @@ #include #include #include +//#include #include #include #include @@ -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 << "\nRC RECEIVER\t\t\t\t\tMOTORS" << endl; - cout << "THRO\tAILE\tELEV\tRUDD\tGEAR\tAUX1\tENABLE\tTEST\tM0 \tM1 \tM2 \tM3" << endl; + cout << "\n**REAL TIME DATA**\n" << endl; + cout << "\t"; // FIFO + 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); }