|
|
單片機：STM32F4_MD6
上位機：Python
#ifdef COMPASS_ENABLED
void send_status_compass() {
long data[3] = { 0 };
int8_t accuracy = { 0 };
unsigned long timestamp;
inv_get_compass_set(data, &accuracy, (inv_time_t*) ×tamp);
MPL_LOGI("Compass: %7.4f %7.4f %7.4f ",
data[0]/65536.f, data[1]/65536.f, data[2]/65536.f);
MPL_LOGI("Accuracy= %d\r\n", accuracy);
}
#endif
/* Handle sensor on/off combinations. */
static void setup_gyro(void)
{
unsigned char mask = 0, lp_accel_was_on = 0;
if (hal.sensors & ACCEL_ON)
mask |= INV_XYZ_ACCEL;
if (hal.sensors & GYRO_ON) {
mask |= INV_XYZ_GYRO;
lp_accel_was_on |= hal.lp_accel_mode;
}
#ifdef COMPASS_ENABLED
if (hal.sensors & COMPASS_ON) {
mask |= INV_XYZ_COMPASS;
lp_accel_was_on |= hal.lp_accel_mode;
}
#endif
/* If you need a power transition, this function should be called with a
* mask of the sensors still enabled. The driver turns off any sensors
* excluded from this mask.
*/
mpu_set_sensors(mask);
mpu_configure_fifo(mask);
if (lp_accel_was_on) {
unsigned short rate;
hal.lp_accel_mode = 0;
/* Switching out of LP accel, notify MPL of new accel sampling rate. */
mpu_get_sample_rate(&rate);
inv_set_accel_sample_rate(1000000L / rate);
}
}
static void tap_cb(unsigned char direction, unsigned char count)
{
switch (direction) {
case TAP_X_UP:
MPL_LOGI("Tap X+ ");
break;
case TAP_X_DOWN:
MPL_LOGI("Tap X- ");
break;
case TAP_Y_UP:
MPL_LOGI("Tap Y+ ");
break;
case TAP_Y_DOWN:
MPL_LOGI("Tap Y- ");
break;
case TAP_Z_UP:
MPL_LOGI("Tap Z+ ");
break;
case TAP_Z_DOWN:
MPL_LOGI("Tap Z- ");
break;
default:
return;
}
MPL_LOGI("x%d\n", count);
return;
}
static void android_orient_cb(unsigned char orientation)
{
switch (orientation) {
case ANDROID_ORIENT_PORTRAIT:
MPL_LOGI("Portrait\n");
break;
case ANDROID_ORIENT_LANDSCAPE:
MPL_LOGI("Landscape\n");
break;
case ANDROID_ORIENT_REVERSE_PORTRAIT:
MPL_LOGI("Reverse Portrait\n");
break;
case ANDROID_ORIENT_REVERSE_LANDSCAPE:
MPL_LOGI("Reverse Landscape\n");
break;
default:
return;
}
}
static inline void run_self_test(void)
{
int result;
long gyro[3], accel[3];
#if defined (MPU6500) || defined (MPU9250)
result = mpu_run_6500_self_test(gyro, accel, 0);
#elif defined (MPU6050) || defined (MPU9150)
result = mpu_run_self_test(gyro, accel);
#endif
if (result == 0x7) {
MPL_LOGI("Passed!\n");
MPL_LOGI("accel: %7.4f %7.4f %7.4f\n",
accel[0]/65536.f,
accel[1]/65536.f,
accel[2]/65536.f);
MPL_LOGI("gyro: %7.4f %7.4f %7.4f\n",
gyro[0]/65536.f,
gyro[1]/65536.f,
gyro[2]/65536.f);
/* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
#ifdef USE_CAL_HW_REGISTERS
/*
* This portion of the code uses the HW offset registers that are in the MPUxxxx devices
* instead of pushing the cal data to the MPL software library
*/
unsigned char i = 0;
for(i = 0; i<3; i++) {
gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps
accel[i] *= 2048.f; //convert to +-16G
accel[i] = accel[i] >> 16;
gyro[i] = (long)(gyro[i] >> 16);
}
mpu_set_gyro_bias_reg(gyro);
#if defined (MPU6500) || defined (MPU9250)
mpu_set_accel_bias_6500_reg(accel);
#elif defined (MPU6050) || defined (MPU9150)
mpu_set_accel_bias_6050_reg(accel);
#endif
#else
/* Push the calibrated data to the MPL library.
*
* MPL expects biases in hardware units << 16, but self test returns
* biases in g's << 16.
*/
unsigned short accel_sens;
float gyro_sens;
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
inv_set_accel_bias(accel, 3);
mpu_get_gyro_sens(&gyro_sens);
gyro[0] = (long) (gyro[0] * gyro_sens);
gyro[1] = (long) (gyro[1] * gyro_sens);
gyro[2] = (long) (gyro[2] * gyro_sens);
inv_set_gyro_bias(gyro, 3);
#endif
}
else {
if (!(result & 0x1))
MPL_LOGE("Gyro failed.\n");
if (!(result & 0x2))
MPL_LOGE("Accel failed.\n");
if (!(result & 0x4))
MPL_LOGE("Compass failed.\n");
}
}
|
評分
-
查看全部評分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
