Matthias Grob
My fully self designed first stable working Quadrocopter Software.
Revision 8:e79c7939d6de, committed 2014-07-14
- Comitter:
- maetugr
- Date:
- Mon Jul 14 09:06:43 2014 +0000
- Parent:
- 7:ac2895479e34
- Child:
- 9:0e37618c7446
- Child:
- 10:14390c90c3f5
- Commit message:
- Perfect flight in acro mode and X-Config (JFYI oscillations were because of controller mount, had to screw the controller pretty stiff to the frame but with rubber washers from an old CD-Drive)
Changed in this revision
| IMU/Sensors/Gyro_Acc/MPU6050.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/IMU/Sensors/Gyro_Acc/MPU6050.cpp Sat Jul 12 12:21:11 2014 +0000
+++ b/IMU/Sensors/Gyro_Acc/MPU6050.cpp Mon Jul 14 09:06:43 2014 +0000
@@ -5,7 +5,19 @@
// Turns on the MPU6050's gyro and initializes it
// register datasheet: http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf
writeRegister(MPU6050_RA_PWR_MGMT_1, 0x01); // wake up from sleep and chooses Gyro X-Axis as Clock source (stadard sleeping and with inacurate clock is 0x40)
- writeRegister(MPU6050_RA_CONFIG, 0x03);
+ writeRegister(MPU6050_RA_CONFIG, 0x03);
+ /*
+ last 3 Bits of |Accelerometer(Fs=1kHz) |Gyroscope
+ MPU6050_RA_CONFIG|Bandwidth(Hz)|Delay(ms)|Bandwidth(Hz)|Delay(ms)|Fs(kHz)
+ -------------------------------------------------------------------------
+ 0 |260 |0 |256 |0.98 |8
+ 1 |184 |2.0 |188 |1.9 |1
+ 2 |94 |3.0 |98 |2.8 |1
+ 3 |44 |4.9 |42 |4.8 |1
+ 4 |21 |8.5 |20 |8.3 |1
+ 5 |10 |13.8 |10 |13.4 |1
+ 6 |5 |19.0 |5 |18.6 |1
+ */
writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18); // scales gyros range to +-2000dps
writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00); // scales accelerometers range to +-2g
}
--- a/main.cpp Sat Jul 12 12:21:11 2014 +0000
+++ b/main.cpp Mon Jul 14 09:06:43 2014 +0000
@@ -1,3 +1,9 @@
+/* X- Configuration +-Configuration
+ m0 m3 m1 -- >
+ \ / | / \ /
+ / \ m2-------m0 V |
+ m1 m2 | \
+ m3 PITCH ROLL*/
#include "mbed.h"
#include "LED.h" // LEDs framework for blinking ;)
#include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe)
@@ -22,12 +28,14 @@
#define PITCH 1
#define YAW 2
+#define SQRT2 0.7071067811865
+
//#define CONSTRAIN(VAL,LIMIT) ((VAL)<(-LIMIT)?(-LIMIT):((VAL)>(LIMIT)?(LIMIT):(VAL)))
bool armed = false; // is for security (when false no motor rotates any more)
bool debug = true; // shows if we want output for the computer
bool RC_present = false; // shows if an RC is present
-float P_R = 4, I_R = 11, D_R = 0;
+float P_R = 0, I_R = 0, D_R = 0;
float P_A = 1.865, I_A = 1.765, D_A = 0;
//float P = 13.16, I = 8, D = 2.73; // PID values
float PY = 3.2, IY = 0, DY = 0;
@@ -91,9 +99,9 @@
// Setting PID Values from auxiliary RC channels
if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000)
- P_R = 0 + (((float)RC[CHANNEL8].read()) * 5 / 1000);
- if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000)
- I_R = 0 + (((float)RC[CHANNEL7].read()) * 12 / 1000);
+ P_R = 0 + (((float)RC[CHANNEL8].read()) * 3 / 1000);
+ /*if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000)
+ I_R = 0 + (((float)RC[CHANNEL7].read()) * 12 / 1000);*/
for(int i=0;i<3;i++)
Controller_Angle[i].setPID(P_A,I_A,D_A);
for(int i=0;i<2;i++)
@@ -141,7 +149,10 @@
Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values and get his advice to correct
}
Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
- Controller_Rate[2].compute(-(RC[2].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct
+ if (RC[THROTTLE].read() > 20)
+ Controller_Rate[2].compute(-(RC[2].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct
+ else
+ Controller_Rate[2].compute(0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct
/*for(int i=0;i<3;i++) {
Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct
@@ -164,10 +175,15 @@
Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);*/
// Mixing
- Motor_speed[2] = RC[THROTTLE].read() + Controller_Rate[PITCH].Value;
- Motor_speed[0] = RC[THROTTLE].read() - Controller_Rate[PITCH].Value;
- Motor_speed[1] = RC[THROTTLE].read() + Controller_Rate[ROLL].Value;
- Motor_speed[3] = RC[THROTTLE].read() - Controller_Rate[ROLL].Value;
+ /*Motor_speed[2] = RC[THROTTLE].read() + Controller_Rate[PITCH].Value; // PITCH in direction + Configuration
+ Motor_speed[0] = RC[THROTTLE].read() - Controller_Rate[PITCH].Value; // PITCH against direction
+ Motor_speed[1] = RC[THROTTLE].read() + Controller_Rate[ROLL].Value; // ROLL in direction
+ Motor_speed[3] = RC[THROTTLE].read() - Controller_Rate[ROLL].Value; // ROLL against direction*/
+
+ Motor_speed[0] = RC[THROTTLE].read() +SQRT2*Controller_Rate[PITCH].Value +SQRT2*Controller_Rate[ROLL].Value; // PITCH in direction X Configuration
+ Motor_speed[1] = RC[THROTTLE].read() +SQRT2*Controller_Rate[PITCH].Value -SQRT2*Controller_Rate[ROLL].Value; // PITCH against direction
+ Motor_speed[2] = RC[THROTTLE].read() -SQRT2*Controller_Rate[PITCH].Value -SQRT2*Controller_Rate[ROLL].Value; // ROLL in direction
+ Motor_speed[3] = RC[THROTTLE].read() -SQRT2*Controller_Rate[PITCH].Value +SQRT2*Controller_Rate[ROLL].Value; // ROLL against direction
Motor_speed[0] -= Controller_Rate[YAW].Value;
Motor_speed[2] -= Controller_Rate[YAW].Value;