amiro-os / include / amiro / gyro / l3g4200d.hpp @ b4885314
History | View | Annotate | Download (5.017 KB)
| 1 | 58fe0e0b | Thomas Schöpping | #ifndef L3G4200D_HPP_
|
|---|---|---|---|
| 2 | #define L3G4200D_HPP_
|
||
| 3 | |||
| 4 | #include <ch.hpp> |
||
| 5 | |||
| 6 | namespace amiro {
|
||
| 7 | |||
| 8 | class HWSPIDriver; |
||
| 9 | |||
| 10 | class L3G4200D : public chibios_rt::BaseStaticThread<256> { |
||
| 11 | |||
| 12 | public:
|
||
| 13 | enum {
|
||
| 14 | DR_100_HZ = 0x00,
|
||
| 15 | DR_200_HZ = 0x40,
|
||
| 16 | DR_400_HZ = 0x80,
|
||
| 17 | DR_800_HZ = 0xC0,
|
||
| 18 | b4885314 | Thomas Schöpping | DR_MASK = 0xC0,
|
| 19 | 58fe0e0b | Thomas Schöpping | }; |
| 20 | enum {
|
||
| 21 | BW_12_5 = 0x00,
|
||
| 22 | BW_20 = 0x00,
|
||
| 23 | BW_25 = 0x10,
|
||
| 24 | BW_30 = 0x00,
|
||
| 25 | BW_35 = 0x10,
|
||
| 26 | BW_50 = 0x20,
|
||
| 27 | BW_70 = 0x30,
|
||
| 28 | BW_110 = 0x30,
|
||
| 29 | b4885314 | Thomas Schöpping | BW_MASK = 0x30,
|
| 30 | 58fe0e0b | Thomas Schöpping | }; |
| 31 | enum {
|
||
| 32 | b4885314 | Thomas Schöpping | PD = 0x08,
|
| 33 | ZEN = 0x04,
|
||
| 34 | YEN = 0x02,
|
||
| 35 | XEN = 0x01,
|
||
| 36 | EN_MASK = 0x0F,
|
||
| 37 | 58fe0e0b | Thomas Schöpping | }; |
| 38 | enum {
|
||
| 39 | HPM_NORMAL_RST = 0x00,
|
||
| 40 | HPM_REFERENCE = 0x10,
|
||
| 41 | HPM_NORMAL = 0x20,
|
||
| 42 | HPM_AUTO_RST = 0x30,
|
||
| 43 | }; |
||
| 44 | enum {
|
||
| 45 | HPCF_2 = 0x00,
|
||
| 46 | HPCF_4 = 0x01,
|
||
| 47 | HPCF_8 = 0x02,
|
||
| 48 | HPCF_16 = 0x03,
|
||
| 49 | HPCF_32 = 0x04,
|
||
| 50 | HPCF_64 = 0x05,
|
||
| 51 | HPCF_128 = 0x06,
|
||
| 52 | HPCF_256 = 0x07,
|
||
| 53 | HPCF_512 = 0x08,
|
||
| 54 | HPCF_1024 = 0x09,
|
||
| 55 | }; |
||
| 56 | enum {
|
||
| 57 | I1_INT1 = 0x80,
|
||
| 58 | I1_BOOT = 0x40,
|
||
| 59 | H_IACTIVE = 0x20,
|
||
| 60 | PP_OD = 0x10,
|
||
| 61 | I2_DRDY = 0x08,
|
||
| 62 | I2_WTM = 0x04,
|
||
| 63 | I2_ORUN = 0x02,
|
||
| 64 | I2_EMPTY = 0x01,
|
||
| 65 | }; |
||
| 66 | enum {
|
||
| 67 | BDU_CONT = 0x00,
|
||
| 68 | BDU_SINGLE = 0x80,
|
||
| 69 | BLE_MSB = 0x40,
|
||
| 70 | BLE_LSB = 0x00,
|
||
| 71 | FS_250_DPS = 0x00,
|
||
| 72 | FS_500_DPS = 0x10,
|
||
| 73 | FS_2000_DPS = 0x20,
|
||
| 74 | ST_SIGN_M = 0x04,
|
||
| 75 | ST_SIGN_P = 0x00,
|
||
| 76 | ST_EN = 0x02,
|
||
| 77 | SIM_3W = 0x01,
|
||
| 78 | SIM_4W = 0x00,
|
||
| 79 | b4885314 | Thomas Schöpping | FS_MASK = 0x20,
|
| 80 | 58fe0e0b | Thomas Schöpping | }; |
| 81 | enum {
|
||
| 82 | BOOT = 0x80,
|
||
| 83 | FIFO_EN = 0x40,
|
||
| 84 | HP_EN = 0x10,
|
||
| 85 | INT1_SEL_NOHP = 0x00,
|
||
| 86 | INT1_SEL_HP = 0x04,
|
||
| 87 | INT1_SEL_LP = 0x08,
|
||
| 88 | OUT_SEL_NOHP = 0x00,
|
||
| 89 | OUT_SEL_HP = 0x01,
|
||
| 90 | OUT_SEL_LP = 0x02,
|
||
| 91 | }; |
||
| 92 | enum {
|
||
| 93 | ZYXOR = 0x80,
|
||
| 94 | ZOR = 0x40,
|
||
| 95 | YOR = 0x20,
|
||
| 96 | XOR = 0x10,
|
||
| 97 | ZYXDA = 0x08,
|
||
| 98 | ZDA = 0x04,
|
||
| 99 | YDA = 0x02,
|
||
| 100 | XDA = 0x01,
|
||
| 101 | }; |
||
| 102 | enum {
|
||
| 103 | FM_BYPASS = 0x00,
|
||
| 104 | FM_FMMODE = 0x20,
|
||
| 105 | FM_STREAM = 0x40,
|
||
| 106 | FM_STREAM2FIFO = 0x60,
|
||
| 107 | FM_BYPASS2STREAM = 0x80,
|
||
| 108 | WTM_MASK = 0x1F,
|
||
| 109 | }; |
||
| 110 | enum {
|
||
| 111 | WTM = 0x80,
|
||
| 112 | OVRN = 0x40,
|
||
| 113 | EMPTY = 0x20,
|
||
| 114 | FSS_MASK = 0x1F,
|
||
| 115 | }; |
||
| 116 | enum {
|
||
| 117 | ANDOR = 0x80,
|
||
| 118 | LIR = 0x40,
|
||
| 119 | ZHIE = 0x20,
|
||
| 120 | ZLIE = 0x10,
|
||
| 121 | YHIE = 0x08,
|
||
| 122 | YLIE = 0x04,
|
||
| 123 | XHIE = 0x02,
|
||
| 124 | XLIE = 0x01,
|
||
| 125 | }; |
||
| 126 | enum {
|
||
| 127 | IA = 0x40,
|
||
| 128 | ZH = 0x20,
|
||
| 129 | ZL = 0x10,
|
||
| 130 | YH = 0x08,
|
||
| 131 | YL = 0x04,
|
||
| 132 | XH = 0x02,
|
||
| 133 | XL = 0x01,
|
||
| 134 | }; |
||
| 135 | enum {
|
||
| 136 | THS_L_MASK = 0x7F
|
||
| 137 | }; |
||
| 138 | enum {
|
||
| 139 | INT1_WAIT = 0x80,
|
||
| 140 | INT1_DURATION_MASK = 0x7F,
|
||
| 141 | }; |
||
| 142 | |||
| 143 | /**
|
||
| 144 | * Return types of getCheck()
|
||
| 145 | */
|
||
| 146 | enum {
|
||
| 147 | CHECK_OK = 0x00u,
|
||
| 148 | CHECK_FAIL = 0x01u,
|
||
| 149 | }; |
||
| 150 | |||
| 151 | /**
|
||
| 152 | * Top view of the AMiRo with charger in the back (F:Front, B:Back).
|
||
| 153 | * Z is pointing into the ground (Apply right-hand-rule):
|
||
| 154 | * ___________
|
||
| 155 | * / F \
|
||
| 156 | * / X \
|
||
| 157 | * | | |
|
||
| 158 | * | Z---Y |
|
||
| 159 | * | |
|
||
| 160 | * \ /
|
||
| 161 | * \____B______/
|
||
| 162 | */
|
||
| 163 | enum {
|
||
| 164 | AXIS_X = 0x00u,
|
||
| 165 | AXIS_Y = 0x01u,
|
||
| 166 | AXIS_Z = 0x02u,
|
||
| 167 | }; |
||
| 168 | |||
| 169 | //private:
|
||
| 170 | |||
| 171 | struct registers {
|
||
| 172 | uint8_t RESERVED_0x00_0x0E[0x0Fu];
|
||
| 173 | uint8_t WHO_AM_I; |
||
| 174 | uint8_t RESERVED_0x10_0x1F[0x10u];
|
||
| 175 | uint8_t CTRL_REG1; |
||
| 176 | uint8_t CTRL_REG2; |
||
| 177 | uint8_t CTRL_REG3; |
||
| 178 | uint8_t CTRL_REG4; |
||
| 179 | uint8_t CTRL_REG5; |
||
| 180 | uint8_t REFERENCE; |
||
| 181 | |||
| 182 | uint8_t OUT_TEMP; |
||
| 183 | |||
| 184 | uint8_t STATUS_REG; |
||
| 185 | |||
| 186 | uint16_t OUT_X; /* LE */
|
||
| 187 | uint16_t OUT_Y; /* LE */
|
||
| 188 | uint16_t OUT_Z; /* LE */
|
||
| 189 | |||
| 190 | uint8_t FIFO_CTRL_REG; |
||
| 191 | uint8_t FIFO_SRC_REG; |
||
| 192 | |||
| 193 | uint8_t INT1_CFG; |
||
| 194 | uint8_t INT1_SRC; |
||
| 195 | uint8_t INT1_THS_XH; |
||
| 196 | uint8_t INT1_THS_XL; |
||
| 197 | uint8_t INT1_THS_YH; |
||
| 198 | uint8_t INT1_THS_YL; |
||
| 199 | uint8_t INT1_THS_ZH; |
||
| 200 | uint8_t INT1_THS_ZL; |
||
| 201 | uint8_t INT1_DURATION; |
||
| 202 | uint8_t RESERVED_0x38_0x3F[0x08u];
|
||
| 203 | }__attribute__((packed)); |
||
| 204 | |||
| 205 | enum {
|
||
| 206 | L3G4200D_ID = 0xD3u
|
||
| 207 | }; |
||
| 208 | |||
| 209 | enum {
|
||
| 210 | SPI_MULT = 0x40u,
|
||
| 211 | SPI_READ = 0x80u,
|
||
| 212 | SPI_WRITE = 0x00u,
|
||
| 213 | }; |
||
| 214 | |||
| 215 | public:
|
||
| 216 | |||
| 217 | struct L3G4200DConfig {
|
||
| 218 | uint8_t ctrl1; |
||
| 219 | uint8_t ctrl2; |
||
| 220 | uint8_t ctrl3; |
||
| 221 | uint8_t ctrl4; |
||
| 222 | uint8_t ctrl5; |
||
| 223 | }; |
||
| 224 | |||
| 225 | L3G4200D(HWSPIDriver* driver); |
||
| 226 | virtual ~L3G4200D();
|
||
| 227 | |||
| 228 | chibios_rt::EvtSource* getEventSource(); |
||
| 229 | msg_t configure(const L3G4200DConfig* config);
|
||
| 230 | int16_t angularRate[AXIS_Z - AXIS_X + 1];
|
||
| 231 | b4885314 | Thomas Schöpping | int32_t angular[AXIS_Z - AXIS_X + 1];
|
| 232 | 58fe0e0b | Thomas Schöpping | |
| 233 | int16_t getAngularRate(const uint8_t axis);
|
||
| 234 | b4885314 | Thomas Schöpping | |
| 235 | int32_t getAngularRate_udps(const uint8_t axis);
|
||
| 236 | int32_t getAngular(const uint8_t axis);
|
||
| 237 | int32_t getAngular_ud(const uint8_t axis);
|
||
| 238 | void angularReset();
|
||
| 239 | 58fe0e0b | Thomas Schöpping | |
| 240 | |||
| 241 | /**
|
||
| 242 | * Check the presence of the accelerometer by reading
|
||
| 243 | * the identifier register and comparing it to the standard
|
||
| 244 | * value
|
||
| 245 | */
|
||
| 246 | uint8_t getCheck(); |
||
| 247 | |||
| 248 | protected:
|
||
| 249 | virtual msg_t main();
|
||
| 250 | |||
| 251 | private:
|
||
| 252 | inline void updateSensorData(); |
||
| 253 | b4885314 | Thomas Schöpping | inline void calcAngular(); |
| 254 | 58fe0e0b | Thomas Schöpping | |
| 255 | private:
|
||
| 256 | |||
| 257 | HWSPIDriver* driver; |
||
| 258 | chibios_rt::EvtSource eventSource; |
||
| 259 | b4885314 | Thomas Schöpping | uint32_t integrationTic; |
| 260 | uint32_t udpsPerTic; // Resolution: Micro-degree-per-second per digit
|
||
| 261 | uint32_t period_us; |
||
| 262 | uint32_t period_ms; |
||
| 263 | systime_t period_st; |
||
| 264 | 58fe0e0b | Thomas Schöpping | |
| 265 | }; |
||
| 266 | |||
| 267 | } /* amiro */
|
||
| 268 | |||
| 269 | #endif /* L3G4200D_HPP_ */ |