brendanhaines/RasPi
1
0
Fork 0
mirror of https://github.com/brendanhaines/RasPi.git synced 2025-04-11 19:24:51 -06:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Brendan Haines 2015-03-17 23:25:13 -06:00
parent d24053f4bf
commit 52fea904ca
4 changed files with 170 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
CopterController/DSM2.cpp
View File

@ -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++ )
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;
}

5
CopterController/DSM2.h
View File

@ -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

2
CopterController/DSM2tester.cpp
View File

@ -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;
}

157
CopterController/oneThatWorks.cpp
View File

@ -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 << "\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);
}