From eb77758b7dcdd0bcef12cd1d56cf4d447cbe5c8c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 31 Jul 2021 22:03:15 -0600 Subject: [PATCH] altos: Add MMC5983 driver Signed-off-by: Keith Packard --- src/drivers/ao_mmc5983.c | 191 +++++++++++++++++++++++++++++++++++++++ src/drivers/ao_mmc5983.h | 149 ++++++++++++++++++++++++++++++ src/kernel/ao_data.h | 25 ++++- src/kernel/ao_log.h | 3 +- 4 files changed, 366 insertions(+), 2 deletions(-) create mode 100644 src/drivers/ao_mmc5983.c create mode 100644 src/drivers/ao_mmc5983.h diff --git a/src/drivers/ao_mmc5983.c b/src/drivers/ao_mmc5983.c new file mode 100644 index 00000000..06d4e9c9 --- /dev/null +++ b/src/drivers/ao_mmc5983.c @@ -0,0 +1,191 @@ +/* + * Copyright © 2021 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include + +#if HAS_MMC5983 + +#define AO_MMC5983_SPI_SPEED ao_spi_speed(10000000) + +static void +ao_mmc5983_start(void) { + ao_spi_get_bit(AO_MMC5983_SPI_CS_PORT, + AO_MMC5983_SPI_CS_PIN, + AO_MMC5983_SPI_INDEX, + AO_MMC5983_SPI_SPEED); +} + +static void +ao_mmc5983_stop(void) { + ao_spi_put_bit(AO_MMC5983_SPI_CS_PORT, + AO_MMC5983_SPI_CS_PIN, + AO_MMC5983_SPI_INDEX); +} + +struct ao_mmc5983_sample ao_mmc5983_current; + +static uint8_t ao_mmc5983_configured; + +static void +ao_mmc5983_reg_write(uint8_t addr, uint8_t data) +{ + uint8_t d[2]; + + d[0] = addr; + d[1] = data; + ao_mmc5983_start(); + ao_spi_send(d, 2, AO_MMC5983_SPI_INDEX); + ao_mmc5983_stop(); +} + +static uint8_t +ao_mmc5983_reg_read(uint8_t addr) +{ + uint8_t d[2]; + + d[0] = addr | MMC5983_READ; + ao_mmc5983_start(); + ao_spi_duplex(d, d, 2, AO_MMC5983_SPI_INDEX); + ao_mmc5983_stop(); + + return d[1]; +} + +static void +ao_mmc5983_duplex(uint8_t *dst, uint8_t len) +{ + ao_mmc5983_start(); + ao_spi_duplex(dst, dst, len, AO_MMC5983_SPI_INDEX); + ao_mmc5983_stop(); +} + +static uint8_t ao_mmc5983_done; + +static void +ao_mmc5983_isr(void) +{ + ao_exti_disable(AO_MMC5983_INT_PORT, AO_MMC5983_INT_PIN); + ao_mmc5983_done = 1; + ao_wakeup(&ao_mmc5983_done); +} + +static uint32_t ao_mmc5983_missed_irq; + +static void +ao_mmc5983_sample(struct ao_mmc5983_sample *sample) +{ + struct ao_mmc5983_raw raw; + + ao_mmc5983_done = 0; + ao_exti_enable(AO_MMC5983_INT_PORT, AO_MMC5983_INT_PIN); + ao_mmc5983_reg_write(MMC5983_CONTROL_0, + (1 << MMC5983_CONTROL_0_INT_MEAS_DONE_EN) | + (1 << MMC5983_CONTROL_0_TM_M)); + ao_arch_block_interrupts(); + while (!ao_mmc5983_done) + if (ao_sleep_for(&ao_mmc5983_done, AO_MS_TO_TICKS(10))) + ++ao_mmc5983_missed_irq; + ao_arch_release_interrupts(); + raw.addr = MMC5983_X_OUT_0 | MMC5983_READ; + ao_mmc5983_duplex((uint8_t *) &raw, sizeof (raw)); + + sample->x = raw.x0 << 10 | raw.x1 << 2 | ((raw.xyz2 >> 6) & 3); + sample->y = raw.y0 << 10 | raw.y1 << 2 | ((raw.xyz2 >> 4) & 3); + sample->z = raw.z0 << 10 | raw.z1 << 2 | ((raw.xyz2 >> 2) & 3); +} + +static uint8_t +ao_mmc5983_setup(void) +{ + uint8_t product_id; + + if (ao_mmc5983_configured) + return 1; + + /* Place device in 3-wire mode */ + ao_mmc5983_reg_write(MMC5983_CONTROL_3, + 1 << MMC5983_CONTROL_3_SPI_3W); + + /* Check product ID */ + product_id = ao_mmc5983_reg_read(MMC5983_PRODUCT_ID); + if (product_id != MMC5983_PRODUCT_ID_PRODUCT) + AO_SENSOR_ERROR(AO_DATA_MMC5983); + + /* Set high bandwidth to reduce sample collection time */ + ao_mmc5983_reg_write(MMC5983_CONTROL_1, + MMC5983_CONTROL_1_BW_800 << MMC5983_CONTROL_1_BW); + + /* Clear automatic measurement and 'set' operation */ + ao_mmc5983_reg_write(MMC5983_CONTROL_2, + 0); + + ao_mmc5983_configured = 1; + return 1; +} + +struct ao_mmc5983_sample ao_mmc5983_current; + +static void +ao_mmc5983(void) +{ + struct ao_mmc5983_sample sample; + ao_mmc5983_setup(); + for (;;) { + ao_mmc5983_sample(&sample); + ao_arch_block_interrupts(); + ao_mmc5983_current = sample; + AO_DATA_PRESENT(AO_DATA_MMC5983); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); + } +} + +static struct ao_task ao_mmc5983_task; + +static void +ao_mmc5983_show(void) +{ + printf ("X: %d Z: %d Y: %d missed irq: %lu\n", + ao_mmc5983_current.x, ao_mmc5983_current.z, ao_mmc5983_current.y, ao_mmc5983_missed_irq); +} + +static const struct ao_cmds ao_mmc5983_cmds[] = { + { ao_mmc5983_show, "M\0Show MMC5983 status" }, + { 0, NULL } +}; + +void +ao_mmc5983_init(void) +{ + ao_mmc5983_configured = 0; + + ao_spi_init_cs(AO_MMC5983_SPI_CS_PORT, (1 << AO_MMC5983_SPI_CS_PIN)); + + ao_enable_port(AO_MMC5983_INT_PORT); + ao_exti_setup(AO_MMC5983_INT_PORT, + AO_MMC5983_INT_PIN, + AO_EXTI_MODE_RISING | AO_EXTI_MODE_PULL_NONE, + ao_mmc5983_isr); + + ao_add_task(&ao_mmc5983_task, ao_mmc5983, "mmc5983"); + ao_cmd_register(&ao_mmc5983_cmds[0]); +} + +#endif diff --git a/src/drivers/ao_mmc5983.h b/src/drivers/ao_mmc5983.h new file mode 100644 index 00000000..5700c4cd --- /dev/null +++ b/src/drivers/ao_mmc5983.h @@ -0,0 +1,149 @@ +/* + * Copyright © 2021 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_MMC5983_H_ +#define _AO_MMC5983_H_ + +#define MMC5983_READ 0x80 + +#define MMC5983_CONFIG_A 0 + +#define MMC5983_CONFIG_A_MA 5 +#define MMC5983_CONFIG_A_MA_1 0 +#define MMC5983_CONFIG_A_MA_2 1 +#define MMC5983_CONFIG_A_MA_4 2 +#define MMC5983_CONFIG_A_MA_8 3 +#define MMC5983_CONFIG_A_MA_MASK 3 + +#define MMC5983_CONFIG_A_DO 2 +#define MMC5983_CONFIG_A_DO_0_75 0 +#define MMC5983_CONFIG_A_DO_1_5 1 +#define MMC5983_CONFIG_A_DO_3 2 +#define MMC5983_CONFIG_A_DO_7_5 3 +#define MMC5983_CONFIG_A_DO_15 4 +#define MMC5983_CONFIG_A_DO_30 5 +#define MMC5983_CONFIG_A_DO_75 6 +#define MMC5983_CONFIG_A_DO_MASK 7 + +#define MMC5983_CONFIG_A_MS 0 +#define MMC5983_CONFIG_A_MS_NORMAL 0 +#define MMC5983_CONFIG_A_MS_POSITIVE_BIAS 1 +#define MMC5983_CONFIG_A_MS_NEGATIVE_BIAS 2 +#define MMC5983_CONFIG_A_MS_MASK 3 + +#define MMC5983_CONFIG_B 1 + +#define MMC5983_CONFIG_B_GN 5 +#define MMC5983_CONFIG_B_GN_0_88 0 +#define MMC5983_CONFIG_B_GN_1_3 1 +#define MMC5983_CONFIG_B_GN_1_9 2 +#define MMC5983_CONFIG_B_GN_2_5 3 +#define MMC5983_CONFIG_B_GN_4_0 4 +#define MMC5983_CONFIG_B_GN_4_7 5 +#define MMC5983_CONFIG_B_GN_5_6 6 +#define MMC5983_CONFIG_B_GN_8_1 7 +#define MMC5983_CONFIG_B_GN_MASK 7 + +#define MMC5983_MODE 2 +#define MMC5983_MODE_CONTINUOUS 0 +#define MMC5983_MODE_SINGLE 1 +#define MMC5983_MODE_IDLE 2 + +#define MMC5983_X_OUT_0 0 +#define MMC5983_X_OUT_1 1 +#define MMC5983_Y_OUT_0 2 +#define MMC5983_Y_OUT_1 3 +#define MMC5983_Z_OUT_0 4 +#define MMC5983_Z_OUT_1 5 +#define MMC5983_XYZ_OUT_2 6 +#define MMC5983_T_OUT 7 + +#define MMC5983_STATUS 8 +# define MMC5983_STATUS_OTP_READ_DONE 4 +# define MMC5983_STATUS_MEAS_T_DONE 1 +# define MMC5983_STATUS_MEAS_M_DONE 0 + +#define MMC5983_CONTROL_0 9 +# define MMC5983_CONTROL_0_OTP_READ 6 +# define MMC5983_CONTROL_0_AUTO_SR_EN 5 +# define MMC5983_CONTROL_0_RESET 4 +# define MMC5983_CONTROL_0_SET 3 +# define MMC5983_CONTROL_0_INT_MEAS_DONE_EN 2 +# define MMC5983_CONTROL_0_TM_T 1 +# define MMC5983_CONTROL_0_TM_M 0 + +#define MMC5983_CONTROL_1 0xa +# define MMC5983_CONTROL_1_SW_RST 7 +# define MMC5983_CONTROL_1_YZ_INHIBIT 3 +# define MMC5983_CONTROL_1_X_INHIBIT 2 +# define MMC5983_CONTROL_1_BW 0 +# define MMC5983_CONTROL_1_BW_100 0 +# define MMC5983_CONTROL_1_BW_200 1 +# define MMC5983_CONTROL_1_BW_400 2 +# define MMC5983_CONTROL_1_BW_800 3 +#define MMC5983_CONTROL_2 0xb +# define MMC5983_CONTROL_2_EN_PRD_SET 7 +# define MMC5983_CONTROL_2_PRD_SET 4 +# define MMC5983_CONTROL_2_PRD_SET_1 0 +# define MMC5983_CONTROL_2_PRD_SET_25 1 +# define MMC5983_CONTROL_2_PRD_SET_75 2 +# define MMC5983_CONTROL_2_PRD_SET_100 3 +# define MMC5983_CONTROL_2_PRD_SET_250 4 +# define MMC5983_CONTROL_2_PRD_SET_500 5 +# define MMC5983_CONTROL_2_PRD_SET_1000 6 +# define MMC5983_CONTROL_2_PRD_SET_2000 7 +# define MMC5983_CONTROL_2_CMM_EN 3 +# define MMC5983_CONTROL_2_CM_FREQ 0 +# define MMC5983_CONTROL_2_CM_FREQ_OFF 0 +# define MMC5983_CONTROL_2_CM_FREQ_1HZ 1 +# define MMC5983_CONTROL_2_CM_FREQ_10HZ 2 +# define MMC5983_CONTROL_2_CM_FREQ_20HZ 3 +# define MMC5983_CONTROL_2_CM_FREQ_50HZ 4 +# define MMC5983_CONTROL_2_CM_FREQ_100HZ 5 +# define MMC5983_CONTROL_2_CM_FREQ_200HZ 6 +# define MMC5983_CONTROL_2_CM_FREQ_1000HZ 7 + +#define MMC5983_CONTROL_3 0xc +#define MMC5983_CONTROL_3_SPI_3W 6 +#define MMC5983_CONTROL_3_ST_ENM 2 +#define MMC5983_CONTROL_3_ST_ENP 1 + +#define MMC5983_PRODUCT_ID 0x2f +#define MMC5983_PRODUCT_ID_PRODUCT 0x30 + +struct ao_mmc5983_sample { + int16_t x, y, z; +}; + +struct ao_mmc5983_raw { + uint8_t addr; + uint8_t x0; + uint8_t x1; + uint8_t y0; + uint8_t y1; + uint8_t z0; + uint8_t z1; + uint8_t xyz2; +}; + +extern struct ao_mmc5983_sample ao_mmc5983_current; + +void +ao_mmc5983_init(void); + +#endif /* _AO_MMC5983_H_ */ diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h index dcd8fc31..60794cc2 100644 --- a/src/kernel/ao_data.h +++ b/src/kernel/ao_data.h @@ -55,6 +55,13 @@ #define AO_DATA_HMC5883 0 #endif +#if HAS_MMC5983 +#include +#define AO_DATA_MMC5983 (1 << 3) +#else +#define AO_DATA_MMC5983 0 +#endif + #if HAS_MMA655X #include #define AO_DATA_MMA655X (1 << 4) @@ -95,7 +102,7 @@ extern uint8_t ao_sensor_errors; #ifdef AO_DATA_RING -#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X|AO_DATA_MPU9250|AO_DATA_ADXL375|AO_DATA_BMX160) +#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X|AO_DATA_MPU9250|AO_DATA_ADXL375|AO_DATA_BMX160|AO_DATA_MMC5983) struct ao_data { uint16_t tick; @@ -121,6 +128,9 @@ struct ao_data { #if HAS_HMC5883 struct ao_hmc5883_sample hmc5883; #endif +#if HAS_MMC5983 + struct ao_mmc5983_sample mmc5983; +#endif #if HAS_MMA655X uint16_t mma655x; #endif @@ -378,6 +388,7 @@ typedef int16_t angle_t; /* in degrees */ /* X axis is aligned in the other board axis (across) */ /* Z axis is aligned perpendicular to the board (through) */ +#ifndef ao_data_along #define ao_data_along(packet) ((packet)->mpu6000.accel_y) #define ao_data_across(packet) ((packet)->mpu6000.accel_x) #define ao_data_through(packet) ((packet)->mpu6000.accel_z) @@ -385,6 +396,7 @@ typedef int16_t angle_t; /* in degrees */ #define ao_data_roll(packet) ((packet)->mpu6000.gyro_y) #define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x) #define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z) +#endif static inline float ao_convert_gyro(float sensor) { @@ -490,6 +502,14 @@ typedef int16_t ao_mag_t; /* in raw sample units */ #endif +#if !HAS_MAG && HAS_MMC5983 + +#define HAS_MAG 1 + +typedef int16_t ao_mag_t; /* in raw sample units */ + +#endif + #if !HAS_MAG && HAS_MPU9250 #define HAS_MAG 1 @@ -523,6 +543,9 @@ ao_data_fill(int head) { #if HAS_HMC5883 ao_data_ring[head].hmc5883 = ao_hmc5883_current; #endif +#if HAS_MMC5983 + ao_data_ring[head].mmc5983 = ao_mmc5983_current; +#endif #if HAS_MPU6000 ao_data_ring[head].mpu6000 = ao_mpu6000_current; #endif diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 5c150abc..103cb70b 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -60,6 +60,7 @@ extern enum ao_flight_state ao_log_state; #define AO_LOG_FORMAT_MICROPEAK2 18 /* 2-byte baro values with header */ #define AO_LOG_FORMAT_TELEMEGA_4 19 /* 32 byte typed telemega records with 32 bit gyro cal and Bmx160 */ #define AO_LOG_FORMAT_EASYMOTOR 20 /* ? byte typed easymotor records with pressure sensor and adxl375 */ +#define AO_LOG_FORMAT_TELEMEGA_5 21 /* 32 byte typed telemega records with 32 bit gyro cal, mpu6000 and mmc5983 */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ /* Return the flight number from the given log slot, 0 if none, -slot on failure */ @@ -533,7 +534,7 @@ struct ao_log_motor { } u; }; -#if AO_LOG_FORMAT == AO_LOG_FOMAT_TELEMEGA_OLD || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA_3 || AO_LOG_FORMAT == AO_LOG_FORMAT_EASYMEGA_2 || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA_4 +#if AO_LOG_FORMAT == AO_LOG_FOMAT_TELEMEGA_OLD || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA_3 || AO_LOG_FORMAT == AO_LOG_FORMAT_EASYMEGA_2 || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA_4 || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA_5 typedef struct ao_log_mega ao_log_type; #endif -- 2.30.2