main.cpp 的流程
//setting up IMU
imu.set_com();
imu.initialize();
//setting up SPI
ArduSPI.initialize();
Set_default_PID_config();
//Say I am ready
Beep(3);
Blink_led(10);
//Starting Timer
Timer.start();
while(1){
sleep(20000);
}
1. imu.set_com
void DMP::set_com() {
// initialize device
mpu.initialize();
// verify connection
printf("Testing device connections...\n");
printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
// load and configure the DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
printf("DMP ready!\n");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
printf("DMP Initialization failed (code %d)\n", devStatus);
}
2. initialize()
void DMP::initialize(){
//This routine waits for the yaw angle to stop
//drifting
if (!dmpReady) return;
printf("Initializing IMU...\n");
//float gyr_old = 10;
int n=0;
do {
// wait for FIFO count > 42 bits
do {
fifoCount = mpu.getFIFOCount();
}while (fifoCount<42);
if (fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt
//(this should happen frequently)
} else {
//read packet from fifo
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetGyro(g, fifoBuffer);
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
}
}
n++;
}while (fabs(gyro[ROLL]) + fabs(gyro[PITCH]) > 0.03 && n<5000);
// 發覺已經沒有振動了
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
for (int i=0;i<DIM;i++) m_ypr_off[i] = ypr[i];
printf("IMU init done; offset values are :\n");
printf("yaw = %f, pitch = %f, roll = %f, n= %d\n\n",
ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,
ypr[ROLL]*180/M_PI,n);
initialized = true;
}
3. ArduSPI.initilize()
/*
SPI::SPI()
{
_devName = "/dev/spidev0.0";
_speed = 1000000;
_bits = 8;
_delay = 0;
_mode = 0;
}
*/
int SPI::initialize(){
int ret = 0;
_fd = open(_devName, O_RDWR); // _devName = "/dev/spidev0.0";
if (_fd < 0)
printf("SPI initialize : can't open device\n");
/*
* spi mode
*/
ret = ioctl(_fd, SPI_IOC_WR_MODE, &_mode); // _mode =0
if (ret == -1)
printf("SPI initialize : can't set spi mode\n");
/*
* bits per word
*/
ret = ioctl(_fd, SPI_IOC_WR_BITS_PER_WORD, &_bits); // bit = 8
if (ret == -1)
printf("SPI initialize : can't set bits per word\n");
/*
* max speed hz
*/
ret = ioctl(_fd, SPI_IOC_WR_MAX_SPEED_HZ, &_speed); // _speed = 1000000;
if (ret == -1)
printf("SPI initialize : can't set max speed hz\n");
printf("SPI initialization finished\n");
return ret;
}
4. Set_default_PID_config()
void Set_default_PID_config(){
//manual initialization of PID constants
yprRATE[YAW].set_Kpid(6.0, 0.0, 0.0);
for (int i=1;i<3;i++){ // only roll and pitch
yprSTAB[i].set_Kpid(2.0, 0.001, 0.1);
yprRATE[i].set_Kpid(2.0, 0.0, 0.0);
}
}
5. Timer.start()
void TimerClass::sig_handler_(int signum)
{
pthread_mutex_lock(&TimerMutex_);
//output to a log file
//open log file
fstream logfile;
logfile.open("quadpilot.log", ios::out|ios::app);
if (logfile.fail()) // Check for file creation and return error.
{
cout << "Error opening output.\n";
}
float RCinput[N_RC_CHAN],PIDout[3];
uint16_t ESC[N_SERVO];
//------------------------------------------------------
//1-Get Remote values using SPI
union bytes{
uint8_t u8[2];
uint16_t u16;
} rc_union;
uint8_t checksum=0,recv_checksum=1;
while (checksum != recv_checksum) {
checksum=0;
for (int i=0;i<4;i++){
ArduSPI.writeByte((uint8_t) (i+1)*10);
rc_union.u8[0] = ArduSPI.rwByte((uint8_t) (i+1)*10+1);
rc_union.u8[1] = ArduSPI.rwByte('S');
//transaction ended
RCinput[i] = (float) rc_union.u16;
checksum+=rc_union.u8[0];
checksum+=rc_union.u8[1];
}
//Control checksum
ArduSPI.writeByte('C');
recv_checksum = ArduSPI.rwByte('S');
}
//Bounding RC values to avoid division by zeros fex.
for (int i=1;i<4;i++){
if ( RCinput[i] > RC_MAX) RCinput[i] = RC_MAX;
if ( RCinput[i] < RC_MIN) RCinput[i] = RC_MIN;
}
//outputing values to logfile
logfile << RCinput[0] << " " << RCinput[1] << " "
<< RCinput[2] << " " << RCinput[3] << " ";
// //convert into PID usable values
RCinput[0] = (RCinput[0] - THR_MIN)/(THR_MAX-THR_MIN) * 100.0;
RCinput[1] = -(RCinput[1] -(RC_MAX+RC_MIN)/2.) /
(RC_MAX-RC_MIN) * K_YAW;
RCinput[2] = (RCinput[2] -(RC_MAX+RC_MIN)/2.)/
(RC_MAX-RC_MIN) * K_PITCH;
RCinput[3] = (RCinput[3] -(RC_MAX+RC_MIN)/2.)/
(RC_MAX-RC_MIN) * K_ROLL;
//outputing values to logfile
logfile << RCinput[0] << " " << RCinput[1] << " "
<< RCinput[2] << " " << RCinput[3] << " ";
#ifdef XMODE
//Switch to Xmode instead of +mode
//orders are given in a ref frame rotated by 90deg.
float cs45 = sqrt(2.)/2.;
float Px = RCinput[2]*cs45 + RCinput[3]*cs45;
float Rx = - RCinput[2]*cs45 + RCinput[3]*cs45;
RCinput[2] = Px;
RCinput[3] = Rx;
#endif
// printf("Received : %6.3f %6.3f %6.3f %6.3f\n", RCinput[0],
// RCinput[1], RCinput[2], RCinput[3]);
//------------------------------------------------------
//2- Get attitude of the drone
while (imu.getAttitude() < 0){
};
//Compensate lost of Thrust due to angle of drone
RCinput[0] = RCinput[0]/cos(imu.ypr[ROLL]/180*M_PI)
/cos(imu.ypr[PITCH]/180*M_PI);
//output to logfile
// logfile << imu.ypr[YAW] << " " << imu.ypr[PITCH] << " "
// << imu.ypr[ROLL] << " "
// << imu.gyro[YAW] << " " << imu.gyro[PITCH] << " "
// << imu.gyro[ROLL] << " ";
// printf("ATTITUDE: %7.2f %7.2f %7.2f\n",imu.ypr[YAW],
// imu.ypr[PITCH],
// imu.ypr[ROLL]);
// printf(" %7.2f %7.2f %7.2f\n",imu.gyro[YAW],
// imu.gyro[PITCH],
// imu.gyro[ROLL]);
//------------------------------------------------------
//3- Timer dt
Timer.calcdt_();
//printf("dt : %f \n",Timer.dt);
//------------------------------------------------------
//4-1 Calculate PID on attitude
#ifdef PID_STAB
for (int i=1;i<DIM;i++){
//yprSTAB[i].updateKpKi(RCinput[i+1],imu.ypr[i]);
PIDout[i] =
yprSTAB[i].update_pid_std(RCinput[i+1],
imu.ypr[i],
Timer.dt);
}
//yaw is rate PID only
PIDout[YAW] = RCinput[YAW+1];
// printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1],
// imu.ypr[PITCH],
// PIDout[PITCH]);
// printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1],
// imu.ypr[ROLL],
// PIDout[ROLL]);
for (int i=0;i<DIM;i++){
PIDout[i] =
yprRATE[i].update_pid_std(PIDout[i],
imu.gyro[i],
Timer.dt);
}
// printf("YAW: %7.2f %7.2f %7.2f\n",RCinput[YAW+1],
// imu.gyro[YAW],
// PIDout[YAW]);
// printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1],
// imu.gyro[PITCH],
// PIDout[PITCH]);
// printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1],
// imu.gyro[ROLL],
// PIDout[ROLL]);
#endif
//4-2 Calculate PID on rotational rate
#ifdef PID_RATE
for (int i=0;i<DIM;i++){
PIDout[i] =
yprRATE[i].update_pid_std(RCinput[i+1],
imu.gyro[i],
Timer.dt);
}
//printf("%7.2f %7.2f\n",imu.gyro[PITCH],Timer.PIDout[PITCH]);
#endif
// logfile << PIDout[YAW] << " " << PIDout[PITCH] << " "
// << PIDout[ROLL] << " ";
//------------------------------------------------------
//5- Send ESC update via SPI
//compute each new ESC value
//if THR is low disable PID and be sure that ESC receive Zero
//printf("%f \n",RCinput[0]);
if (RCinput[0] < 7.0) {
for (int i=0;i<4;i++){
ESC[i] = (uint16_t)0;
}
} else {
ESC[1] = (uint16_t)(RCinput[0]*10+1000
+ PIDout[ROLL] + PIDout[YAW]);
ESC[3] = (uint16_t)(RCinput[0]*10+1000
- PIDout[ROLL] + PIDout[YAW]);
ESC[0] = (uint16_t)(RCinput[0]*10+1000
+ PIDout[PITCH] - PIDout[YAW]);
ESC[2] = (uint16_t)(RCinput[0]*10+1000
- PIDout[PITCH] - PIDout[YAW]);
}
checksum = 0;
for (int iesc=0;iesc < N_SERVO; iesc++) {
ArduSPI.writeByte(ESC[iesc] & 0xff);
checksum+=ESC[iesc] & 0xff;
ArduSPI.writeByte((ESC[iesc] >> 8) & 0xff);
checksum+=(ESC[iesc] >> 8) & 0xff;
}
ArduSPI.writeByte(checksum);
//sending Proccess it
ArduSPI.writeByte('P');
// printf(" Sent : %4d %4d %4d %4d\n", ESC[0],
// ESC[1], ESC[2], ESC[3]);
//------------------------------------------------------
//6-compensate dt
Timer.compensate_();
//ouputting ESC values to logfile
logfile << ESC[0] << " " << ESC[1] << " "
<< ESC[2] << " " << ESC[3] << " " << endl;
//closing logfile
logfile.close();
pthread_mutex_unlock(&TimerMutex_);
//end of interrupt
}
沒有留言:
張貼留言