理工酷
资源下载
使用MPU9250获取实时运动数据
文件列表（压缩包大小 7.09M)
免费
概述
需要的元件
Arduino mega 2560
SparkFun IMU Breakout -MPU- 9250
Arduino IDE
原理及流程
运动处理是一个重要的概念。如果要与实时数据进行交互，则应该能够与运动参数进行交互，例如：线性加速度，角加速度，磁力。
MPU9250具有加速度计，陀螺仪和磁力计。我们可以从MPU9250获得的信息是：偏航，俯仰角和侧倾角。鉴于此，我将仅在本文中处理偏航。
处理来自MPU9250的数据
MPU的三个传感器均具有一个16位寄存器。它们临时存储来自传感器的数据，然后再通过I2C进行中继。
读取数据
我们一次接收8位数据，然后将它们连接在一起以再次形成16位。如下面的kriswiners代码片段所示：
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
gyro_bias[0] += (int32_t) gyro_temp[0];
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
校准原始数据
然后必须根据用户环境校准接收到的数据。需要对磁力计进行校准以补偿磁偏角。校正的确切值取决于位置。有两个必须校准的变量：偏航和磁偏角。
下面显示了特定磁偏角的偏航校准（印度钦奈的Potheri ）。磁偏角数据可从不同站点获得：
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] – q[2] * q[2] – q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] – q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] – q[1] * q[1] – q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw += 1.34; /* Declination at Potheri, Chennail ,India Model Used: IGRF12 Help
Latitude: 12.823640° N
Longitude: 80.043518° E
Date Declination
2016-04-09 1.34° W changing by 0.06° E per year (+ve for west )*/
请参见下面的代码片段[给定的代码片段，用于校正偏斜的数据来自另一个函数（magcalMPU9250(float * dest1, float * dest2)）：
readMagData(magCount); // Read the x/y/z adc values
getMres();
// magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
// magbias[1] = +120.; // User environmental x-axis correction in milliGauss
// magbias[2] = +125.; // User environmental x-axis correction in milliGauss
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float)magCount[0]*mRes*magCalibration[0] – magBias[0]; // get actual magnetometer value, this depends on scale being set
my = (float)magCount[1]*mRes*magCalibration[1] – magBias[1];
mz = (float)magCount[2]*mRes*magCalibration[2] – magBias[2];
}
自动校准磁力计
这是MPU代码中最简单和重要的部分之一。 当你在移动传感器时，该功能将magcalMPU9250(float * dest1, float * dest2)校准磁力计。它存储最大和最小读数并取平均值。
void magcalMPU9250(float * dest1, float * dest2) {
uint16_t ii = 0, sample_count = 0;
int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
int16_t mag_max[3] = {0x8000, 0x8000, 0x8000}, mag_min[3] = {0x7FFF, 0x7FFF, 0x7FFF}, mag_temp[3] = {0, 0, 0};
Serial.println(“Mag Calibration: Wave device in a figure eight until done!”);
sample_count = 128;
for(ii = 0; ii < sample_count; ii++) {
readMagData(mag_temp); // Read the mag data
for (int jj = 0; jj < 3; jj++) {
if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
}
delay(135); // at 8 Hz ODR, new mag data is available every 125 ms
}
// Get hard iron correction
mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0]; // save mag biases in G for main program
dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];
dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];
// Get soft iron correction estimate
mag_scale[0] = (mag_max[0] – mag_min[0])/2; // get average x axis max chord length in counts
mag_scale[1] = (mag_max[1] – mag_min[1])/2; // get average y axis max chord length in counts
mag_scale[2] = (mag_max[2] – mag_min[2])/2; // get average z axis max chord length in counts
float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
avg_rad /= 3.0;
dest2[0] = avg_rad/((float)mag_scale[0]);
dest2[1] = avg_rad/((float)mag_scale[1]);
dest2[2] = avg_rad/((float)mag_scale[2]);
Serial.println(“Mag Calibration done!”);
}
有关更多详细信息，请访问源：
https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration
永久校准特定位置
如果你不想每次都进行自动校准，则只需在计算了magbias []后记下平均值，并使用以下代码段即可：
readMagData(magCount); // Read the x/y/z adc values
getMres();
magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
magbias[1] = +120.; // User environmental x-axis correction in milliGauss magbias[2] = +125.; // User environmental x-axis correction in milliGauss
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float)magCount[0]*mRes*magCalibration[0] – magBias[0]; // get actual magnetometer value, this depends on scale being set
my = (float)magCount[1]*mRes*magCalibration[1] – magBias[1];
mz = (float)magCount[2]*mRes*magCalibration[2] – magBias[2];
}
对于我的位置，值470、120、125是固定的，因此在执行此操作后，就不需要void magcalMPU9250（float * dest1，float * dest2）函数，因此你可以将其注释掉或将其删除。同样也不要忘记注释掉调用语句：
delay(1000);
// Get magnetometer calibration from AK8963 ROM
initAK8963(magCalibration); Serial.println(“AK8963 initialized for active data mode….”); // Initialize device for active mode read of magnetometer
getMres();
//magcalMPU9250(magBias,magScale); // commented call statement
if(SerialDebug) {
// Serial.println(“Calibration values: “);
Serial.print(“X-Axis sensitivity adjustment value “); Serial.println(magCalibration[0], 2);
Serial.print(“Y-Axis sensitivity adjustment value “); Serial.println(magCalibration[1], 2);
Serial.print(“Z-Axis sensitivity adjustment value “); Serial.println(magCalibration[2], 2);
}
筛选
因为原始数据包含很多噪声，所以我们在传感器的输出上使用某些滤波器将其转换为四元数（Madgwick / Mahony / Kalman）：
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth’s magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 – _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 – mx * q3q3 – mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 – _2q1mz * q2 + _2q2mx * q3 – my * q2q2 + my * q3q3 + _2q3 * mz * q4 – my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 – mz * q2q2 + _2q3 * my * q4 – mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 – _2q1q3 – ax) + _2q2 * (2.0f * q1q2 + _2q3q4 – ay) – _2bz * q3 * (_2bx * (0.5f – q3q3 – q4q4) + _2bz * (q2q4 – q1q3) – mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 – q1q4) + _2bz * (q1q2 + q3q4) – my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f – q2q2 – q3q3) – mz);
s2 = _2q4 * (2.0f * q2q4 – _2q1q3 – ax) + _2q1 * (2.0f * q1q2 + _2q3q4 – ay) – 4.0f * q2 * (1.0f – 2.0f * q2q2 – 2.0f * q3q3 – az) + _2bz * q4 * (_2bx * (0.5f – q3q3 – q4q4) + _2bz * (q2q4 – q1q3) – mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 – q1q4) + _2bz * (q1q2 + q3q4) – my) + (_2bx * q4 – _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f – q2q2 – q3q3) – mz);
s3 = -_2q1 * (2.0f * q2q4 – _2q1q3 – ax) + _2q4 * (2.0f * q1q2 + _2q3q4 – ay) – 4.0f * q3 * (1.0f – 2.0f * q2q2 – 2.0f * q3q3 – az) + (-_4bx * q3 – _2bz * q1) * (_2bx * (0.5f – q3q3 – q4q4) + _2bz * (q2q4 – q1q3) – mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 – q1q4) + _2bz * (q1q2 + q3q4) – my) + (_2bx * q1 – _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f – q2q2 – q3q3) – mz);
s4 = _2q2 * (2.0f * q2q4 – _2q1q3 – ax) + _2q3 * (2.0f * q1q2 + _2q3q4 – ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f – q3q3 – q4q4) + _2bz * (q2q4 – q1q3) – mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 – q1q4) + _2bz * (q1q2 + q3q4) – my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f – q2q2 – q3q3) – mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx – q3 * gy – q4 * gz) – beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz – q4 * gy) – beta * s2;
qDot3 = 0.5f * (q1 * gy – q2 * gz + q4 * gx) – beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy – q3 * gx) – beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
// measured ones.
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth’s magnetic field
hx = 2.0f * mx * (0.5f – q3q3 – q4q4) + 2.0f * my * (q2q3 – q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f – q2q2 – q4q4) + 2.0f * mz * (q3q4 – q1q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 – q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f – q2q2 – q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 – q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 – q2q2 – q3q3 + q4q4;
wx = 2.0f * bx * (0.5f – q3q3 – q4q4) + 2.0f * bz * (q2q4 – q1q3);
wy = 2.0f * bx * (q2q3 – q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f – q2q2 – q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz – az * vy) + (my * wz – mz * wy);
ey = (az * vx – ax * vz) + (mz * wx – mx * wz);
ez = (ax * vy – ay * vx) + (mx * wy – my * wx);
if (Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx – q3 * gy – q4 * gz) * (0.5f * deltat);
q2 = pa + (q1 * gx + pb * gz – pc * gy) * (0.5f * deltat);
q3 = pb + (q1 * gy – pa * gz + pc * gx) * (0.5f * deltat);
q4 = pc + (q1 * gz + pa * gy – pb * gx) * (0.5f * deltat);
// Normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
数据平均
由于数据变化非常快，我们采样了一段时间（50毫秒）并取平均值。
count = millis();
digitalWrite(myLed, !digitalRead(myLed)); // toggle led
}
}
else {
// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = millis() – count;
if (delt_t > 50) { // update once per half-second independent of read rate
if(SerialDebug) {
提取现实世界的信息
最后，我们从四元数获得偏航，俯仰和滚动形式的读数。
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] – q[2] * q[2] – q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] – q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] – q[1] * q[1] – q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw += 1.34; /* Declination at Potheri, Chennail ,India Model Used: IGRF12 Help
Latitude: 12.823640° N
Longitude: 80.043518° E
Date Declination
2016-04-09 1.34° W changing by 0.06° E per year (+ve for west )*/
roll *= 180.0f / PI;
Serial.print(“Yaw, Pitch, Roll: “);
Serial.print(yaw+180, 2);
Serial.print(“, “);
Serial.print(pitch, 2);
Serial.print(“, “);
Serial.println(roll, 2);
使用Arduino Mega 2560从MPU9250获取读数
我们有许多MPU9250的库。kriswiner提供了一种流行的方法：Kriswiner提供的MPU-9250 Arduino库
将库保存到Arduino文件夹后，即可开始使用。打开示例MPU9250BasicAHRS.ino.还准备好此设置：
MPU9250突破——— Arduino
- VIN ———————— 5V
- SDA ———————– SDA（引脚20）
- SCL ———————– SCL（引脚21）
- GND ———————- GND
这些导线不应太长，因为I2C连接不适用于长导线。
现在缩减MPU9250BasicAHRS代码。它具有LCD代码，但我们不需要它，因此请删除不必要的行。另外，我还添加了一部分自动校准代码。这是修改后的代码，没有不必要的代码，并且添加了自动校准：Github。
现在将代码上传到你的Arduino并进行上述连接。打开串行终端，并将波特率更改为115200。你应该看到以下输出：
MPU9250
9-DOF 16-bit
motion sensor
60 ug LSB
MPU9250 I AM 71 I should be 71
MPU9250 is online…
x-axis self test: acceleration trim within : 0.8% of factory value
y-axis self test: acceleration trim within : -1.9% of factory value
z-axis self test: acceleration trim within : 1.8% of factory value
x-axis self test: gyration trim within : -0.2% of factory value
y-axis self test: gyration trim within : 0.3% of factory value
z-axis self test: gyration trim within : 0.6% of factory value
MPU9250 bias
x y z
254913-660mg1.1-0.11.2o/s
MPU9250 initialized for active data mode….
AK8963 I AM 48 I should be 48
AK8963 initialized for active data mode….
Mag Calibration: Wave device in a figure eight until done!
如果看到以下内容：
MPU9250
9-DOF 16-bit
motion sensor
60 ug LSB
MPU9250 I AM FF I should be 71
Could not connect to MPU9250: 0xFF
这意味着肯定存在接线问题（或在最坏的情况下，MPu / arduino故障）请在尝试之前纠正此问题。
如果一切顺利，你会看到“ MPU处于联机状态”和“ Mag Calibration：图8中的波形设备，直到完成！” 然后一切正常，你应该将MPU调整为八字形，直到完成自动校准。一段时间后，你应该获得如下的偏航，俯仰和侧倾输出：
Mag Calibration: Wave device in a figure eight until done!
Mag Calibration done!
X-Axis sensitivity adjustment value 1.19
Y-Axis sensitivity adjustment value 1.19
Z-Axis sensitivity adjustment value 1.15
AK8963
ASAX
1.19
ASAY
1.19
ASAZ
1.15
Yaw, Pitch, Roll: 11.34, 28.62, 50.03
Yaw, Pitch, Roll: 20.47, 25.15, 52.88
Yaw, Pitch, Roll: 26.94, 19.02, 52.70
Yaw, Pitch, Roll: 28.22, 15.02, 50.15
Yaw, Pitch, Roll: 27.10, 13.94, 44.68
Yaw, Pitch, Roll: 23.11, 13.69, 37.51
Yaw, Pitch, Roll: 14.29, 13.22, 27.61
Yaw, Pitch, Roll: 357.03, 8.21, 16.72
Yaw, Pitch, Roll: 342.29, 0.69, 9.19
Yaw, Pitch, Roll: 328.42, -4.80, 3.16
Yaw, Pitch, Roll: 317.19, -10.51, -0.58
Yaw, Pitch, Roll: 311.88, -16.57, -3.64
Yaw, Pitch, Roll: 327.71, -23.45, -16.82
Yaw, Pitch, Roll: 325.74, -22.02, -23.51
Yaw, Pitch, Roll: 325.99, -28.17, -26.95
Yaw, Pitch, Roll: 324.57, -24.96, -23.21
Yaw, Pitch, Roll: 320.01, -26.42, -22.25
Yaw, Pitch, Roll: 322.50, -26.04, -26.62
Yaw, Pitch, Roll: 322.85, -23.43, -29.17
Yaw, Pitch, Roll: 323.46, -19.20, -31.48
这就意味着你有数据来
使用P-控制器对RTPT进行自动方位角（偏航）校准
我们首先通过执行以下操作将偏航从（-180到+180）转换为（0到360）：
yaw = yaw + 180;
然后，我们只需使用简单的比例控制器在偏航中找到错误，然后将错误添加回偏航，然后使用新偏航进行伺服映射：
nyaw = 360 – yaw; //”yaw” comes from MPU which is “actual”
Azim = Azimuth – nyaw; /*”Azimuth” is the absolute azimuth which comes from calculations from RA and DEC which assumes our device is already aligned to North….by doing the subtraction we get the proportional error*/
Azim -= 90; //adding 90° because my titlt servo is mounted at an offset of 90°
while (Azim < 0)
Azim = 360.0 – abs(Azim); /*we use the error proportionally for our servo to auto adjust */
Azi = map(Azim, 0, 360, 5, 29);
Az = (int)Azi;
Elev = map(Elevation, -90, 90, 2, 178);
El = (int)Elev;
这样就完成了该项目。
所有代码在下载区均可找到。
最后
所有需要的文件在下载区均可找到。
理工酷提示：
如果遇到文件不能下载或其他产品问题，请添加管理员微信：ligongku001，并备注：产品反馈
评论（0）
0/250
