|
|
本帖最后由 pijunkuan 于 2014-12-7 00:39 编辑
研究了mpu 6050 一个多月了，做了一个简单的展示上位机，对mpu传回来的数据做了一点小小的处理，废话不多说了，这里直接上程序，希望各位高手们多多指点一下，个人感觉程序里面有不少的bug，但是自己是学机械的，对电子和软件一窍不通，一直处于自学阶段，望得到高手们的帮助。
坐等各位高手们的回复。
Arduino程序：
- #include "Wire.h"
- #include "I2Cdev.h"
- #include "MPU6050.h"
- MPU6050 mpu;
- #define LED_PIN 13
- unsigned long last_read_time;
- float last_x_angle; // These are the filtered angles
- float last_y_angle;
- float last_z_angle;
- float last_gyro_x_angle; // Store the gyro angles to compare drift
- float last_gyro_y_angle;
- float last_gyro_z_angle;
- void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro)
- {
- last_read_time = time;
- last_x_angle = x;
- last_y_angle = y;
- last_z_angle = z;
- last_gyro_x_angle = x_gyro;
- last_gyro_y_angle = y_gyro;
- last_gyro_z_angle = z_gyro;
- }
- inline unsigned long get_last_time() {return last_read_time;}
- inline float get_last_x_angle() {return last_x_angle;}
- inline float get_last_y_angle() {return last_y_angle;}
- inline float get_last_z_angle() {return last_z_angle;}
- inline float get_last_gyro_x_angle() {return last_gyro_x_angle;}
- inline float get_last_gyro_y_angle() {return last_gyro_y_angle;}
- inline float get_last_gyro_z_angle() {return last_gyro_z_angle;}
- float base_x_gyro = 0;
- float base_y_gyro = 0;
- float base_z_gyro = 0;
- float base_x_accel = 0;
- float base_y_accel = 0;
- float base_z_accel = 0;
- float GYRO_FACTOR;
- float ACCEL_FACTOR;
- int16_t ax, ay, az;
- int16_t gx, gy, gz;
- char dataOut[256];
- void calibrate_sensors() {
- int num_readings = 10;
- // Discard the first reading (don't know if this is needed or
- // not, however, it won't hurt.)
- mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-
- // Read and average the raw values
- for (int i = 0; i < num_readings; i++) {
- mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
- base_x_gyro += gx;
- base_y_gyro += gy;
- base_z_gyro += gz;
- base_x_accel += ax;
- base_y_accel += ay;
- base_y_accel += az;
- }
-
- base_x_gyro /= num_readings;
- base_y_gyro /= num_readings;
- base_z_gyro /= num_readings;
- base_x_accel /= num_readings;
- base_y_accel /= num_readings;
- base_z_accel /= num_readings;
- }
- void setup() {
- Wire.begin();
- Serial.begin(57600);
- while (!Serial);
- mpu.initialize();
- Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
-
- pinMode(LED_PIN, OUTPUT);
-
- calibrate_sensors();
- set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);
- }
- void loop()
- {
- const float RADIANS_TO_DEGREES = 57.2958; //180/3.14159
- unsigned long t_now = millis();
- mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
- float gyro_x = (gx - base_x_gyro)/131.0;
- float gyro_y = (gy - base_y_gyro)/131.0;
- float gyro_z = (gz - base_z_gyro)/131.0;
- float accel_x = ax; // - base_x_accel;
- float accel_y = ay; // - base_y_accel;
- float accel_z = az; // - base_z_accel;
-
-
- float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
- float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
- float accel_angle_z = 0;
- // Compute the (filtered) gyro angles
- float dt =(t_now - get_last_time())/1000.0;
- float gyro_angle_x = gyro_x*dt + get_last_x_angle();
- float gyro_angle_y = gyro_y*dt + get_last_y_angle();
- float gyro_angle_z = gyro_z*dt + get_last_z_angle();
-
- // Compute the drifting gyro angles
- float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle();
- float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle();
- float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();
-
- // Apply the complementary filter to figure out the change in angle - choice of alpha is
- // estimated now. Alpha depends on the sampling rate...
- const float alpha = 0.96;
- float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
- float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
- float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle
-
- // Update the saved data with the latest values
- set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
-
- // Output complementary data and DMP data to the serial port. The signs on the data needed to be
- // fudged to get the angle direction correct.
- Serial.print(get_last_x_angle(), 2);
- Serial.print(",");
- Serial.print(get_last_y_angle(), 2);
- Serial.print(",");
- Serial.print(-get_last_z_angle(), 2);
- Serial.print(",");
- Serial.print(ax/16384.0, 2);
- Serial.print(",");
- Serial.print(ay/16384.0, 2);
- Serial.print(",");
- Serial.println(az/16384.0, 2);
-
- // blink LED to indicate activity
- delay(110);
-
- }
|
|
发表于 2014-12-6 19:07:17
楼主
