Skip to content

Commit 997779e

Browse files
hxw804781317hxw804781317
committed
add mag mmc5603 driver.
1 parent bffc05b commit 997779e

4 files changed

Lines changed: 355 additions & 0 deletions

File tree

‎src/fw/board/boards/board_obelix_bb.h‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,26 @@ extern I2CSlavePort* const I2C_MMC5603;
117117
extern I2CSlavePort* const I2C_W1160;
118118

119119

120+
121+
static const BoardConfigMag BOARD_CONFIG_MAG = {
122+
.mag_config = {
123+
.axes_offsets[AXIS_X] = 1,
124+
.axes_offsets[AXIS_Y] = 0,
125+
.axes_offsets[AXIS_Z] = 2,
126+
.axes_inverts[AXIS_X] = false,
127+
.axes_inverts[AXIS_Y] = true,
128+
.axes_inverts[AXIS_Z] = true,
129+
},
130+
//.mag_int_gpio = { GPIOF, GPIO_Pin_14 },
131+
//.mag_int = { EXTI_PortSourceGPIOF, 14 },
132+
};
133+
134+
extern TouchSensor * const BOARD_CONFIG_TOUCH;
135+
136+
137+
138+
139+
120140
#if 0
121141

122142
static const BoardConfigButton BOARD_CONFIG_BUTTON = {

‎src/fw/drivers/imu/imu_obelix_bb.c‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
/*
2+
* Copyright 2024 Google LLC
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*/
16+
17+
#include <inttypes.h>
18+
#include <stdbool.h>
19+
#include <stdio.h>
20+
#include "system/logging.h"
21+
//#include "drivers/imu/lis3dh/lis3dh.h"
22+
#include "drivers/imu/mmc5603/mmc5603.h"
23+
24+
25+
void imu_init(void) {
26+
// Init accelerometer
27+
//lis3dh_init();
28+
// Init magnetometer
29+
mmc5603_init();
30+
}
31+
32+
void imu_power_up(void) {
33+
//lis3dh_power_up();
34+
}
35+
36+
void imu_power_down(void) {
37+
//lis3dh_power_down();
38+
}

‎src/fw/drivers/imu/mmc5603/mmc5603.c‎

Lines changed: 277 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,277 @@
1+
/*
2+
* Copyright 2024 Google LLC
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*/
16+
#include "board/board.h"
17+
#include "console/prompt.h"
18+
#include "drivers/exti.h"
19+
#include "drivers/gpio.h"
20+
#include "drivers/i2c.h"
21+
#include "drivers/mag.h"
22+
#include "drivers/periph_config.h"
23+
//#include "kernel/events.h"
24+
#include "system/logging.h"
25+
#include "os/mutex.h"
26+
#include "system/passert.h"
27+
#include "kernel/util/sleep.h"
28+
#include <mcu.h>
29+
#include <inttypes.h>
30+
#include <stdint.h>
31+
#include "mmc5603.h"
32+
#include "services/common/new_timer/new_timer.h"
33+
34+
35+
//////////////////////////////////////////////////////////////////////
36+
#define MMC5603_REG_DATA 0x00
37+
#define MMC5603_REG_XL 0x00
38+
#define MMC5603_REG_XH 0x01
39+
#define MMC5603_REG_YL 0x02
40+
#define MMC5603_REG_YH 0x03
41+
#define MMC5603_REG_ZL 0x04
42+
#define MMC5603_REG_ZH 0x05
43+
#define MMC5603_REG_STATUS1 0x18
44+
#define MMC5603_REG_STATUS0 0x19
45+
#define MMC5603_REG_ODR 0x1A
46+
#define MMC5603_REG_CTRL0 0x1B
47+
#define MMC5603_REG_CTRL1 0x1C
48+
#define MMC5603_REG_CTRL2 0x1D
49+
#define MMC5603_REG_X_THD 0x1E
50+
#define MMC5603_REG_Y_THD 0x1F
51+
#define MMC5603_REG_Z_THD 0x20
52+
#define MMC5603_REG_ST_X_VAL 0x27
53+
#define MMC5603_REG_ST_Y_VAL 0x28
54+
#define MMC5603_REG_ST_Z_VAL 0x29
55+
#define MMC5603_REG_WHOAMI 0x39
56+
57+
#undef __BIT
58+
#define __BIT(x) (1uL<<(x))
59+
60+
61+
62+
//////////////////////////////////////////////////////////////////////
63+
static PebbleMutex *s_mag_mutex;
64+
static bool s_initialized = false;
65+
static int s_use_refcount = 0;
66+
67+
static uint8_t s_freq_hz = 5;
68+
static TimerID s_event_timer_id = 0;
69+
70+
static bool mmc5603_read(uint8_t reg_addr, uint8_t data_len, uint8_t *data) {
71+
return i2c_read_register_block(I2C_MMC5603, reg_addr, data_len, data);
72+
}
73+
74+
static bool mmc5603_write(uint8_t reg_addr, uint8_t data) {
75+
return i2c_write_register_block(I2C_MMC5603, reg_addr, 1, &data);
76+
}
77+
78+
79+
//! Move the mag into standby mode, which is a low power mode where we're not actively sampling
80+
//! the sensor or firing interrupts.
81+
static bool prv_enter_standby_mode(void) {
82+
// Ask to enter standby mode
83+
if (!mmc5603_write(MMC5603_REG_CTRL2, 0x00)) {
84+
return false;
85+
}
86+
return true;
87+
#if 0
88+
// Wait for the sysmod register to read that we're now in standby mode. This can take up to
89+
// 1/ODR to respond. Since we only support speeds as slow as 5Hz, that means we may be waiting
90+
// for up to 200ms for this part to become ready.
91+
const int NUM_ATTEMPTS = 300; // 200ms + some padding for safety
92+
for (int i = 0; i < NUM_ATTEMPTS; ++i) {
93+
uint8_t sysmod = 0;
94+
if (!mmc5603_read(SYSMOD_REG, 1, &sysmod)) {
95+
return false;
96+
}
97+
98+
if (sysmod == 0) {
99+
// We're done and we're now in standby!
100+
return true;
101+
}
102+
103+
// Wait at least 1ms before asking again
104+
psleep(2);
105+
}
106+
return false;
107+
#endif
108+
}
109+
110+
// Ask the compass for a 8-bit value that's programmed into the IC at the
111+
// factory. Useful as a sanity check to make sure everything came up properly.
112+
bool mmc5603_check_whoami(void) {
113+
static const uint8_t COMPASS_WHOAMI_BYTE = 0x10;
114+
uint8_t whoami = 0;
115+
116+
PBL_LOG(LOG_LEVEL_ALWAYS, "mmc5603_check_whoami");
117+
mag_use();
118+
mmc5603_read(MMC5603_REG_WHOAMI, 1, &whoami);
119+
PBL_LOG(LOG_LEVEL_ALWAYS, "whoami=%d", whoami);
120+
mag_release();
121+
PBL_LOG(LOG_LEVEL_ALWAYS, "Read mmc5603 whoami byte 0x%x, expecting 0x%x", whoami, COMPASS_WHOAMI_BYTE);
122+
return (whoami == COMPASS_WHOAMI_BYTE);
123+
}
124+
125+
void mmc5603_init(void) {
126+
if (s_initialized) {
127+
return;
128+
}
129+
130+
#if 0
131+
PBL_LOG(LOG_LEVEL_ALWAYS, "mmc5603_init");
132+
s_mag_mutex = mutex_create();
133+
s_initialized = true;
134+
135+
if (!mmc5603_check_whoami()) {
136+
PBL_LOG(LOG_LEVEL_ALWAYS, "Failed to query Mag");
137+
return;
138+
}
139+
140+
// configure mmc5603
141+
mmc5603_write(MMC5603_REG_CTRL1, 0);
142+
mmc5603_write(MMC5603_REG_ODR, s_freq_hz);
143+
mmc5603_write(MMC5603_REG_CTRL0, __BIT(7)|__BIT(5));
144+
mmc5603_write(MMC5603_REG_CTRL2, 0);
145+
#endif
146+
}
147+
148+
// @brief 定时器函数
149+
void mmc5603_timer_handler(void *data)
150+
{
151+
if (s_use_refcount == 0) {
152+
return;
153+
}
154+
PBL_LOG(LOG_LEVEL_ALWAYS, "--------------->mag event");
155+
156+
#if 0
157+
PebbleEvent e = {
158+
.type = PEBBLE_ECOMPASS_SERVICE_EVENT,
159+
};
160+
event_put(&e);
161+
#endif
162+
}
163+
164+
void mag_use(void) {
165+
PBL_ASSERTN(s_initialized);
166+
167+
PBL_LOG(LOG_LEVEL_ALWAYS, "---111--");
168+
mutex_lock(s_mag_mutex);
169+
PBL_LOG(LOG_LEVEL_ALWAYS, "---222--s_use_refcount=%d", s_use_refcount);
170+
if (s_use_refcount == 0) {
171+
i2c_use(I2C_MMC5603);
172+
PBL_LOG(LOG_LEVEL_ALWAYS, "---333--s_event_timer_id=%ld", s_event_timer_id);
173+
//根据频率启动定时器
174+
if (s_event_timer_id == 0) {
175+
s_event_timer_id = new_timer_create();
176+
PBL_LOG(LOG_LEVEL_ALWAYS, "---444--s_event_timer_id=%ld", s_event_timer_id);
177+
PBL_ASSERTN(s_event_timer_id);
178+
}
179+
uint32_t timeout_ms = 1000 / s_freq_hz;
180+
PBL_LOG(LOG_LEVEL_ALWAYS, "---555--timeout_ms=%ld", timeout_ms);
181+
new_timer_start(s_event_timer_id, timeout_ms, mmc5603_timer_handler, NULL, TIMER_START_FLAG_REPEATING);
182+
}
183+
++s_use_refcount;
184+
PBL_LOG(LOG_LEVEL_ALWAYS, "---666--");
185+
mutex_unlock(s_mag_mutex);
186+
PBL_LOG(LOG_LEVEL_ALWAYS, "---777--");
187+
}
188+
189+
// @brief
190+
void mag_release(void) {
191+
PBL_ASSERTN(s_initialized && s_use_refcount != 0);
192+
193+
mutex_lock(s_mag_mutex);
194+
--s_use_refcount;
195+
if (s_use_refcount == 0) {
196+
if (s_event_timer_id) {
197+
new_timer_stop(s_event_timer_id);
198+
new_timer_delete(s_event_timer_id);
199+
s_event_timer_id = 0;
200+
}
201+
// 进入低功耗模式
202+
prv_enter_standby_mode();
203+
uint8_t raw_data[6];
204+
mmc5603_read(MMC5603_REG_DATA, sizeof(raw_data), raw_data);
205+
i2c_release(I2C_MMC5603);
206+
}
207+
mutex_unlock(s_mag_mutex);
208+
}
209+
210+
// @brief 转换坐标系
211+
static int16_t align_coord_system(int axis, uint8_t *raw_data) {
212+
int offset = 2 * BOARD_CONFIG_MAG.mag_config.axes_offsets[axis];
213+
bool do_invert = BOARD_CONFIG_MAG.mag_config.axes_inverts[axis];
214+
int16_t mag_field_strength = ((raw_data[offset] << 8) | raw_data[offset + 1]);
215+
mag_field_strength *= (do_invert ? -1 : 1);
216+
return (mag_field_strength);
217+
}
218+
219+
// @brief 读取数据
220+
MagReadStatus mag_read_data(MagData *data) {
221+
mutex_lock(s_mag_mutex);
222+
223+
if (s_use_refcount == 0) {
224+
mutex_unlock(s_mag_mutex);
225+
return (MagReadMagOff);
226+
}
227+
228+
MagReadStatus rv = MagReadSuccess;
229+
uint8_t raw_data[6];
230+
if (!mmc5603_read(MMC5603_REG_DATA, sizeof(raw_data), raw_data)) {
231+
rv = MagReadCommunicationFail;
232+
goto done;
233+
}
234+
235+
// map raw data to watch coord system
236+
data->x = align_coord_system(0, &raw_data[0]);
237+
data->y = align_coord_system(1, &raw_data[0]);
238+
data->z = align_coord_system(2, &raw_data[0]);
239+
240+
done:
241+
mutex_unlock(s_mag_mutex);
242+
return (rv);
243+
}
244+
245+
// @brief 修改采样率
246+
bool mag_change_sample_rate(MagSampleRate rate) {
247+
mutex_lock(s_mag_mutex);
248+
249+
if (s_use_refcount == 0) {
250+
mutex_unlock(s_mag_mutex);
251+
return (true);
252+
}
253+
254+
bool success = false;
255+
if (rate == MagSampleRate5Hz) {
256+
s_freq_hz = 5;
257+
} else if (rate == MagSampleRate20Hz) {
258+
s_freq_hz = 20;
259+
} else {
260+
goto done;
261+
}
262+
263+
mmc5603_write(MMC5603_REG_CTRL1, 0);
264+
mmc5603_write(MMC5603_REG_ODR, s_freq_hz);
265+
success = true;
266+
done:
267+
mutex_unlock(s_mag_mutex);
268+
return (success);
269+
}
270+
271+
// @brief 开始采样
272+
void mag_start_sampling(void) {
273+
mag_use();
274+
mmc5603_write(MMC5603_REG_CTRL0, __BIT(7)|__BIT(5));
275+
mmc5603_write(MMC5603_REG_CTRL2, __BIT(4));
276+
mag_change_sample_rate(MagSampleRate5Hz);
277+
}

‎src/fw/drivers/imu/mmc5603/mmc5603.h‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
/*
2+
* Copyright 2024 Google LLC
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*/
16+
#pragma once
17+
18+
19+
void mmc5603_init(void);
20+

0 commit comments

Comments
 (0)