Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Hardwares/IMUManager.h@64:43ab429a37e0, 2017-04-10 (annotated)

Committer:
hazheng
Date:
Mon Apr 10 16:44:31 2017 +0000
Revision:
64:43ab429a37e0
Parent:
63:d9a81b3d69f5 
Auto terminate when hit the end of the loop. Commented out all the IMU code, because of the memory space problem.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 64:43ab429a37e0 1 #if 0
hazheng 64:43ab429a37e0 2
hazheng 59:b709711bc566 3 #pragma once
hazheng 59:b709711bc566 4 #ifndef IMU_MANAGER_H
hazheng 59:b709711bc566 5 #define IMU_MANAGER_H
hazheng 59:b709711bc566 6
hazheng 59:b709711bc566 7 #define ACCEL_MAG_SA0_0
hazheng 59:b709711bc566 8 //#define ACCEL_MAG_SA0_1
hazheng 59:b709711bc566 9 #define ACCEL_MAG_SA1_0
hazheng 59:b709711bc566 10 //#define ACCEL_MAG_SA1_1
hazheng 59:b709711bc566 11
hazheng 59:b709711bc566 12 #if defined(ACCEL_MAG_SA1_0) && defined(ACCEL_MAG_SA0_0)
hazheng 59:b709711bc566 13 #define ACCEL_MAG_SA1 0
hazheng 59:b709711bc566 14 #define ACCEL_MAG_SA0 0
hazheng 59:b709711bc566 15 #define FXOS8700CQ_SLAVE_ADDR 0x1E
hazheng 59:b709711bc566 16 #elif defined(ACCEL_MAG_SA1_0) && defined(ACCEL_MAG_SA0_1)
hazheng 59:b709711bc566 17 #define ACCEL_MAG_SA1 0
hazheng 59:b709711bc566 18 #define ACCEL_MAG_SA0 1
hazheng 59:b709711bc566 19 #define FXOS8700CQ_SLAVE_ADDR 0x1D
hazheng 59:b709711bc566 20 #elif defined(ACCEL_MAG_SA1_1) && defined(ACCEL_MAG_SA0_0)
hazheng 59:b709711bc566 21 #define ACCEL_MAG_SA1 1
hazheng 59:b709711bc566 22 #define ACCEL_MAG_SA0 0
hazheng 59:b709711bc566 23 #define FXOS8700CQ_SLAVE_ADDR 0x1C
hazheng 59:b709711bc566 24 #elif defined(ACCEL_MAG_SA1_1) && defined(ACCEL_MAG_SA0_1)
hazheng 59:b709711bc566 25 #define ACCEL_MAG_SA1 1
hazheng 59:b709711bc566 26 #define ACCEL_MAG_SA0 1
hazheng 59:b709711bc566 27 #define FXOS8700CQ_SLAVE_ADDR 0x1F
hazheng 59:b709711bc566 28 #else
hazheng 59:b709711bc566 29 #error Must choose a SA0 and SA1 value for Accel and Mag
hazheng 59:b709711bc566 30 #endif
hazheng 59:b709711bc566 31
hazheng 62:bc5caf59fe39 32 #define FXOS8700CQ_STATUS 0x00
hazheng 62:bc5caf59fe39 33
hazheng 62:bc5caf59fe39 34 #define FXOS8700CQ_F_SETUP 0x09
hazheng 59:b709711bc566 35
hazheng 62:bc5caf59fe39 36 #define FXOS8700CQ_SYSMOD 0x0B
hazheng 62:bc5caf59fe39 37
hazheng 62:bc5caf59fe39 38 #define FXOS8700CQ_WHOAMI 0x0D
hazheng 62:bc5caf59fe39 39 #define FXOS8700CQ_WHOAMI_VAL 0xC7
hazheng 59:b709711bc566 40
hazheng 59:b709711bc566 41 #define FXOS8700CQ_XYZ_DATA_CFG 0x0E
hazheng 62:bc5caf59fe39 42 #define FXOS8700CQ_XYZ_DATA_2G 0x00
hazheng 62:bc5caf59fe39 43 #define FXOS8700CQ_XYZ_DATA_4G 0x01
hazheng 62:bc5caf59fe39 44 #define FXOS8700CQ_XYZ_DATA_8G 0x02
hazheng 59:b709711bc566 45
hazheng 62:bc5caf59fe39 46 #define FXOS8700CQ_XYZ_DATA_SC FXOS8700CQ_XYZ_DATA_2G
hazheng 59:b709711bc566 47
hazheng 62:bc5caf59fe39 48 #if FXOS8700CQ_XYZ_DATA_SC == FXOS8700CQ_XYZ_DATA_2G
hazheng 63:d9a81b3d69f5 49 #define ACCELER_SCALE_F_MG 0.244f
hazheng 62:bc5caf59fe39 50 #elif FXOS8700CQ_XYZ_DATA_SC == FXOS8700CQ_XYZ_DATA_4G
hazheng 63:d9a81b3d69f5 51 #define ACCELER_SCALE_F_MG 0.488f
hazheng 62:bc5caf59fe39 52 #elif FXOS8700CQ_XYZ_DATA_SC == FXOS8700CQ_XYZ_DATA_8G
hazheng 63:d9a81b3d69f5 53 #define ACCELER_SCALE_F_MG 0.976f
hazheng 62:bc5caf59fe39 54 #else
hazheng 62:bc5caf59fe39 55 #error Must define a scale for accelerometer!
hazheng 62:bc5caf59fe39 56 #endif
hazheng 59:b709711bc566 57
hazheng 64:43ab429a37e0 58 #define FXOS8700CQ_HP_FILTER_CUTOFF 0x0F
hazheng 64:43ab429a37e0 59
hazheng 62:bc5caf59fe39 60 #define FXOS8700CQ_CTRL_REG1 0x2A
hazheng 63:d9a81b3d69f5 61 #define FXOS8700CQ_CTRL_REG1_v 0x05
hazheng 63:d9a81b3d69f5 62
hazheng 63:d9a81b3d69f5 63 #define FXOS8700CQ_CTRL_REG2 0x2B
hazheng 63:d9a81b3d69f5 64 #define FXOS8700CQ_RESET_MASK 0x40
hazheng 63:d9a81b3d69f5 65
hazheng 63:d9a81b3d69f5 66 #define FXOS8700CQ_OFF_X 0x2F
hazheng 63:d9a81b3d69f5 67
hazheng 63:d9a81b3d69f5 68 #define FXOS8700CQ_OFF_Y 0x30
hazheng 63:d9a81b3d69f5 69
hazheng 63:d9a81b3d69f5 70 #define FXOS8700CQ_OFF_Z 0x31
hazheng 63:d9a81b3d69f5 71 #define OFFSET_SCALE_F 2.0f
hazheng 62:bc5caf59fe39 72
hazheng 64:43ab429a37e0 73 #define FXOS8700CQ_TEMP 0x51
hazheng 64:43ab429a37e0 74
hazheng 62:bc5caf59fe39 75 #define FXOS8700CQ_M_CTRL_REG1 0x5B
hazheng 62:bc5caf59fe39 76
hazheng 62:bc5caf59fe39 77 #define FXOS8700CQ_M_CTRL_REG2 0x5C
hazheng 59:b709711bc566 78
hazheng 64:43ab429a37e0 79 #define FXOS8700CQ_READ_LEN 7 // status plus 6 channels = 13 bytes
hazheng 59:b709711bc566 80
hazheng 63:d9a81b3d69f5 81 #define IMU_UPDATE_TICK_RATE 0.002f
hazheng 63:d9a81b3d69f5 82 #define IMU_DEFAULT_G 9.8f
hazheng 63:d9a81b3d69f5 83
hazheng 59:b709711bc566 84 #include <mbed.h>
hazheng 59:b709711bc566 85
hazheng 59:b709711bc566 86 #ifdef __cplusplus
hazheng 59:b709711bc566 87 extern "C" {
hazheng 59:b709711bc566 88 #endif
hazheng 59:b709711bc566 89
hazheng 59:b709711bc566 90 struct imu_vec3
hazheng 59:b709711bc566 91 {
hazheng 62:bc5caf59fe39 92 float x;
hazheng 62:bc5caf59fe39 93 float y;
hazheng 62:bc5caf59fe39 94 float z;
hazheng 59:b709711bc566 95 };
hazheng 59:b709711bc566 96
hazheng 59:b709711bc566 97 uint8_t imu_manager_init();
hazheng 59:b709711bc566 98
hazheng 63:d9a81b3d69f5 99 void imu_manager_calibrate();
hazheng 63:d9a81b3d69f5 100
hazheng 63:d9a81b3d69f5 101 void imu_manager_begin_tick();
hazheng 63:d9a81b3d69f5 102
hazheng 59:b709711bc566 103 void imu_manager_update();
hazheng 59:b709711bc566 104
hazheng 59:b709711bc566 105 const volatile struct imu_vec3* imu_manager_get_accl();
hazheng 59:b709711bc566 106
hazheng 63:d9a81b3d69f5 107 const volatile struct imu_vec3* imu_manager_get_velocity();
hazheng 63:d9a81b3d69f5 108
hazheng 63:d9a81b3d69f5 109 const volatile struct imu_vec3* imu_manager_get_position();
hazheng 63:d9a81b3d69f5 110
hazheng 64:43ab429a37e0 111 float imu_manager_get_temp();
hazheng 64:43ab429a37e0 112
hazheng 62:bc5caf59fe39 113 //const volatile struct imu_vec3* imu_manager_get_magt();
hazheng 59:b709711bc566 114
hazheng 59:b709711bc566 115 #ifdef __cplusplus
hazheng 59:b709711bc566 116 }
hazheng 59:b709711bc566 117 #endif
hazheng 59:b709711bc566 118
hazheng 64:43ab429a37e0 119 #endif //IMU_MANAGER_H
hazheng 64:43ab429a37e0 120
hazheng 64:43ab429a37e0 121 #endif //if 0