altos: Add MMC5983 driver master
authorKeith Packard <keithp@keithp.com>
Sun, 1 Aug 2021 04:03:15 +0000 (22:03 -0600)
committerKeith Packard <keithp@keithp.com>
Sun, 1 Aug 2021 04:04:22 +0000 (22:04 -0600)
Signed-off-by: Keith Packard <keithp@keithp.com>
src/drivers/ao_mmc5983.c [new file with mode: 0644]
src/drivers/ao_mmc5983.h [new file with mode: 0644]
src/kernel/ao_data.h
src/kernel/ao_log.h

diff --git a/src/drivers/ao_mmc5983.c b/src/drivers/ao_mmc5983.c
new file mode 100644 (file)
index 0000000..06d4e9c
--- /dev/null
@@ -0,0 +1,191 @@
+/*
+ * Copyright © 2021 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <ao.h>
+#include <ao_mmc5983.h>
+#include <ao_exti.h>
+
+#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 (file)
index 0000000..5700c4c
--- /dev/null
@@ -0,0 +1,149 @@
+/*
+ * Copyright © 2021 Keith Packard <keithp@keithp.com>
+ *
+ * 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_ */
index dcd8fc31b8e9a33ee9debadf826bc9a6e6ed3303..60794cc2d110878e373aaf808ad54caa5e3339b6 100644 (file)
 #define AO_DATA_HMC5883        0
 #endif
 
+#if HAS_MMC5983
+#include <ao_mmc5983.h>
+#define AO_DATA_MMC5983        (1 << 3)
+#else
+#define AO_DATA_MMC5983        0
+#endif
+
 #if HAS_MMA655X
 #include <ao_mma655x.h>
 #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
index 5c150abcefac88a4fb2dc4444d01d4b8a9c0e190..103cb70b46597988f21eb07aedb17bc327b3155d 100644 (file)
@@ -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