基本上，我正在统一校准和应用偏移量。但是旋转范围不准确，我想知道是不是因为错误的灵敏度应用。我需要帮助找出一种方法来稳定旋转并增加旋转范围，因为它仅从 0 到 90 并且抖动很大。
统一代码：
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO.Ports;
public class MPU6050_3 : MonoBehaviour
{
public GameObject target;
SerialPort stream = new SerialPort("COM3", 115200);
string[] accgyValues = new string[10];
int buffersize = 1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone = 2; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone = 1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
float ax, ay, az, gx, gy, gz, gx_f, gy_f, gz_f;
float mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;
float ax_offset = 0, ay_offset = 0, az_offset = 0, gx_offset = 0, gy_offset = 0, gz_offset = 0;
bool cal;
public float factor = 1;
public bool enableRotation;
public bool enableTranslation;
//float acc_normalizer_factor = 0.00025f;
float gyro_normalizer_factor = 1.0f/65.5f;
void Awake()
{
stream.Open();
//stream.ReadTimeout=50;
}
void Update()
{
if (state == 0)
{
accel();
Debug.Log("\nReading sensors for first time...");
state++;
Debug.Log("State is:"+state);
}
else if (state == 1)
{
Debug.Log("\nCalculating offsets...");
calibration();
state++;
Debug.Log("State is:" + state);
}
else if (state == 2)
{
accel();
Debug.Log("SensorW/Offset:" + mean_ax + ":" + mean_ay + ":" + mean_az + ":" + mean_gx + ":" + mean_gy + ":" + mean_gz);
Debug.Log("Offset:" + ax_offset + ":" + ay_offset + ":" + az_offset + ":" + gx_offset + ":" + gy_offset + ":" + gz_offset);
state++;
Debug.Log("State is:" + state);
}
else if (state == 3)
{
getaccgy();
//if (enableTranslation) target.transform.position = new Vector3(ax+ax_offset, az-az_offset, ay-ay_offset);
gx_f = (gx + gx_offset) * gyro_normalizer_factor;
gy_f = (gy + gy_offset) * gyro_normalizer_factor;
gz_f = -(gz + gz_offset) * gyro_normalizer_factor ;
if (enableRotation) target.transform.rotation = Quaternion.Euler(gx_f * factor, gz_f * factor, gy_f * factor);
Debug.Log("ValueRot "+ gx_f +" " + gy_f + " " + gz_f);
Debug.Log("State is:" + state);
}
}
public void getaccgy()
{
stream.Write("r");
Debug.Log("In accel");
string value;
accgyValues = new string[10];
value = stream.ReadLine();
accgyValues = value.Split(",");
ax = int.Parse(accgyValues[0]); //* acc_normalizer_factor;
ay = int.Parse(accgyValues[1]); //* acc_normalizer_factor;
az = int.Parse(accgyValues[2]); //* acc_normalizer_factor;
gx = int.Parse(accgyValues[3]); //* gyro_normalizer_factor;
gy = int.Parse(accgyValues[4]); //* gyro_normalizer_factor;
gz = int.Parse(accgyValues[5]); //* gyro_normalizer_factor;
Debug.Log("GetACCGY");
Debug.Log("SensorW/Offset:" + ax + ":" + ay + ":" + az + ":" + gx + ":" + gy + ":" + gz);
}
void accel()
{
float i = 0f, buff_ax = 0f, buff_ay = 0f, buff_az = 0f, buff_gx = 0f, buff_gy = 0f, buff_gz = 0f;
while (i < 1101)
{
getaccgy();
if (cal)
{
ax = ax + ax_offset;
ay = ay + ay_offset;
az = az + az_offset;
gx = gx + gx_offset;
gy = gy + gy_offset;
gz = gz + gz_offset;
}
if (i > 99 && i < (buffersize + 100))
{ //First 100 measures are discarded
buff_ax += ax;
buff_ay += ay;
buff_az += az;
buff_gx += gx;
buff_gy += gy;
buff_gz += gz;
}
if (i == (buffersize + 100))
{
mean_ax = buff_ax / buffersize;
mean_ay = buff_ay / buffersize;
mean_az = buff_az / buffersize;
mean_gx = buff_gx / buffersize;
mean_gy = buff_gy / buffersize;
mean_gz = buff_gz / buffersize;
Debug.Log("Mean:" + mean_ax + ":" + mean_ay + ":" + mean_az + ":" + mean_gx + ":" + mean_gy + ":" + mean_gz);
}
i++;
Debug.Log("Mean Sensor");
Debug.Log("Buffer Size is" + buffersize + "i mean" + i);
Debug.Log("SensorW/Offset:" + ax + ":" + ay + ":" + az + ":" + gx + ":" + gz);
}
}
void calibration()
{
cal = true;
Debug.Log("Callibration Entered");
ax_offset = -mean_ax / 8;
ay_offset = -mean_ay / 8;
az_offset = (16384 - mean_az) / 8;
gx_offset = -mean_gx / 4;
gy_offset = -mean_gy / 4;
gz_offset = -mean_gz / 4;
int ready = 0;
while (ready < 7)
{
//accelgyro.setXAccelOffset(ax_offset);
//accelgyro.setYAccelOffset(ay_offset);
//accelgyro.setZAccelOffset(az_offset);
//accelgyro.setXGyroOffset(gx_offset);
//accelgyro.setYGyroOffset(gy_offset);
//accelgyro.setZGyroOffset(gz_offset);
accel();
Debug.Log("..." + ready);
if (Math.Abs(mean_ax) <= acel_deadzone) ready++;
else ax_offset = ax_offset - mean_ax / acel_deadzone;
if (Math.Abs(mean_ay) <= acel_deadzone) ready++;
else ay_offset = ay_offset - mean_ay / acel_deadzone;
if (Math.Abs(16384 - mean_az) <= acel_deadzone) ready++;
else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;
if (Math.Abs(mean_gx) <= giro_deadzone) ready++;
else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);
if (Math.Abs(mean_gy) <= giro_deadzone) ready++;
else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);
if (Math.Abs(mean_gz) <= giro_deadzone) ready++;
else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);
if (ready == 6) break;
}
}
}
Arduino 代码：获取原始值的简单代码：
// I2Cdev and MPU6050 must be installed as libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
/////////////////////////////////// CONFIGURATION /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone=8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone=1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;
MPU6050 accelgyro(0x68); // <-- use for AD0 high
int16_t ax, ay, az,gx, gy, gz;
int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;
/////////////////////////////////// SETUP ////////////////////////////////////
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.
// initialize serial communication
Serial.begin(115200);
// initialize device
accelgyro.initialize();
//wait for ready
//while (Serial.available() && Serial.read()); // empty buffer
//while (!Serial.available()){
//Serial.println(F("Send any character to start sketch.\n"));
//delay(1500);
//}
//while (Serial.available() && Serial.read()); // empty buffer again
}
// start message
//Serial.println("\nMPU6050 Calibration Sketch");
//delay(2000);
//Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
//delay(3000);
// verify connection
//Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
//delay(1000);
// reset offsets
//accelgyro.setXAccelOffset(0);
//accelgyro.setYAccelOffset(0);
//accelgyro.setZAccelOffset(0);
//accelgyro.setXGyroOffset(0);
//accelgyro.setYGyroOffset(0);
//accelgyro.setZGyroOffset(0);
void loop() {
if(Serial.available() > 0)
{
char ltr = Serial.read();
if(ltr == 'r')
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Serial.print(ax);
Serial.print(",");
Serial.print(ay);
Serial.print(",");
Serial.print(az);
Serial.print(",");
Serial.print(gx);
Serial.print(",");
Serial.print(gy);
Serial.print(",");
Serial.println(gz);
Serial.flush();
}
}
}