85 lines
2.3 KiB
C
85 lines
2.3 KiB
C
/* Board */
|
|
#include "board.h"
|
|
#include "clock_config.h"
|
|
#include "pin_mux.h"
|
|
|
|
/* Debug Console */
|
|
#include "fsl_debug_console.h"
|
|
|
|
/* Private FSPI */
|
|
#include "app_fspi.h"
|
|
#include "app_mpu_impl.h"
|
|
|
|
#define RAW16_TO_ACTUAL(raw, fs) ((float)raw * (float)fs / 32768.0)
|
|
|
|
mpu6500_t s_mpu = {
|
|
.ops =
|
|
{
|
|
.read_register = app_mpu_impl_read_reg,
|
|
.write_register = app_mpu_impl_write_reg,
|
|
.delay = app_mpu_impl_delay,
|
|
},
|
|
.user_data = NULL,
|
|
};
|
|
|
|
int main(void) {
|
|
/* Init board hardware. */
|
|
BOARD_ConfigMPU();
|
|
BOARD_InitBootPins();
|
|
BOARD_InitBootClocks();
|
|
BOARD_InitDebugConsole();
|
|
|
|
PRINTF("FSPI example start:\r\n");
|
|
|
|
status_t ret = fspi_init();
|
|
if (ret != kStatus_Success) {
|
|
PRINTF("FSPI init failed.\r\n");
|
|
goto dead_loop;
|
|
}
|
|
|
|
if (mpu6500_init(&s_mpu) != MPU6500_RET_SUCCESS) {
|
|
PRINTF("MPU6500 initialization failed.\r\n");
|
|
goto dead_loop;
|
|
}
|
|
|
|
mpu6500_result_t sensor_result;
|
|
|
|
uint16_t accel_fs;
|
|
uint16_t gyro_fs;
|
|
|
|
if (mpu6500_get_accel_fullscale(&s_mpu, &accel_fs) != MPU6500_RET_SUCCESS) {
|
|
PRINTF("Failed to get accel range.\r\n");
|
|
goto dead_loop;
|
|
}
|
|
|
|
if (mpu6500_get_gyro_fullscale(&s_mpu, &gyro_fs) != MPU6500_RET_SUCCESS) {
|
|
PRINTF("Failed to get gyro range.\r\n");
|
|
goto dead_loop;
|
|
}
|
|
|
|
for (;;) {
|
|
SDK_DelayAtLeastUs(10 * 1000, CLOCK_GetCoreSysClkFreq());
|
|
if (mpu6500_read_result(&s_mpu, &sensor_result) != MPU6500_RET_SUCCESS) {
|
|
PRINTF("Sensor read failed.\r\n");
|
|
continue;
|
|
}
|
|
|
|
float accel_x = RAW16_TO_ACTUAL(sensor_result.accel_x_raw, accel_fs) * 9.80;
|
|
float accel_y = RAW16_TO_ACTUAL(sensor_result.accel_y_raw, accel_fs) * 9.80;
|
|
float accel_z = RAW16_TO_ACTUAL(sensor_result.accel_z_raw, accel_fs) * 9.80;
|
|
|
|
float gyro_x = RAW16_TO_ACTUAL(sensor_result.gyro_x_raw, gyro_fs);
|
|
float gyro_y = RAW16_TO_ACTUAL(sensor_result.gyro_y_raw, gyro_fs);
|
|
float gyro_z = RAW16_TO_ACTUAL(sensor_result.gyro_z_raw, gyro_fs);
|
|
|
|
PRINTF(
|
|
"/* %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6d */\r\n",
|
|
accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, sensor_result.temp_raw);
|
|
}
|
|
|
|
dead_loop:
|
|
for (;;) {
|
|
/* -- */
|
|
}
|
|
}
|