Merge pull request #6623 from iNavFlight/de_rm3100_mtk450can

digitalentity · web-flow · commit 3c97e916fed9 · 2021-03-17T09:43:27.000+01:00
Add MATEKF405CAN board; Add support for RM3100 compass on SPI bus
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
@@ -154,6 +154,8 @@ main_sources(COMMON_SRC
     drivers/compass/compass_mpu9250.h
     drivers/compass/compass_qmc5883l.c
     drivers/compass/compass_qmc5883l.h
+    drivers/compass/compass_rm3100.c
+    drivers/compass/compass_rm3100.h
     drivers/compass/compass_msp.c
     drivers/compass/compass_msp.h
 
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
@@ -118,6 +118,7 @@ typedef enum {
     DEVHW_QMC5883,
     DEVHW_MAG3110,
     DEVHW_LIS3MDL,
+    DEVHW_RM3100,
 
     /* Temp sensor chips */
     DEVHW_LM75_0,
diff --git a/src/main/drivers/compass/compass_rm3100.c b/src/main/drivers/compass/compass_rm3100.c
@@ -0,0 +1,179 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file 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, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef USE_MAG_RM3100
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/time.h"
+#include "drivers/bus_i2c.h"
+
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+
+#include "drivers/sensor.h"
+#include "drivers/compass/compass.h"
+
+#include "drivers/compass/compass_rm3100.h"
+
+#define RM3100_REG_POLL        0x00
+#define RM3100_REG_CMM         0x01
+#define RM3100_REG_CCX1        0x04
+#define RM3100_REG_CCX0        0x05
+#define RM3100_REG_CCY1        0x06
+#define RM3100_REG_CCY0        0x07
+#define RM3100_REG_CCZ1        0x08
+#define RM3100_REG_CCZ0        0x09
+#define RM3100_REG_TMRC        0x0B
+#define RM3100_REG_MX          0x24
+#define RM3100_REG_MY          0x27
+#define RM3100_REG_MZ          0x2A
+#define RM3100_REG_BIST        0x33
+#define RM3100_REG_STATUS      0x34
+#define RM3100_REG_HSHAKE      0x35
+#define RM3100_REG_REVID       0x36
+
+#define RM3100_REVID           0x22
+
+#define CCX_DEFAULT_MSB        0x00
+#define CCX_DEFAULT_LSB        0xC8
+#define CCY_DEFAULT_MSB        CCX_DEFAULT_MSB
+#define CCY_DEFAULT_LSB        CCX_DEFAULT_LSB
+#define CCZ_DEFAULT_MSB        CCX_DEFAULT_MSB
+#define CCZ_DEFAULT_LSB        CCX_DEFAULT_LSB
+#define CMM_DEFAULT            0x71    // Continuous mode
+#define TMRC_DEFAULT           0x94
+
+
+static bool deviceInit(magDev_t * mag)
+{
+    busWrite(mag->busDev, RM3100_REG_TMRC, TMRC_DEFAULT);
+
+    busWrite(mag->busDev, RM3100_REG_CMM, CMM_DEFAULT);
+
+    busWrite(mag->busDev, RM3100_REG_CCX1, CCX_DEFAULT_MSB);
+    busWrite(mag->busDev, RM3100_REG_CCX0, CCX_DEFAULT_LSB);
+
+    busWrite(mag->busDev, RM3100_REG_CCY1, CCY_DEFAULT_MSB);
+    busWrite(mag->busDev, RM3100_REG_CCY0, CCY_DEFAULT_LSB);
+
+    busWrite(mag->busDev, RM3100_REG_CCZ1, CCZ_DEFAULT_MSB);
+    busWrite(mag->busDev, RM3100_REG_CCZ0, CCZ_DEFAULT_LSB);
+
+    return true;
+}
+
+static bool deviceRead(magDev_t * mag)
+{
+    uint8_t status;
+
+#pragma pack(push, 1)
+    struct {
+        uint8_t x[3];
+        uint8_t y[3];
+        uint8_t z[3];
+    } rm_report;
+#pragma pack(pop)
+
+    mag->magADCRaw[X] = 0;
+    mag->magADCRaw[Y] = 0;
+    mag->magADCRaw[Z] = 0;
+
+    /* Check if new measurement is ready */
+    bool ack = busRead(mag->busDev, RM3100_REG_STATUS, &status);
+
+    if (!ack || (status & 0x80) == 0) {
+        return false;
+    }
+
+    ack = busReadBuf(mag->busDev, RM3100_REG_MX, (uint8_t *)&rm_report, sizeof(rm_report));
+    if (!ack) {
+        return false;
+    }
+
+    int32_t xraw;
+    int32_t yraw;
+    int32_t zraw;
+
+    /* Rearrange mag data */
+    xraw = ((rm_report.x[0] << 24) | (rm_report.x[1] << 16) | (rm_report.x[2]) << 8);
+    yraw = ((rm_report.y[0] << 24) | (rm_report.y[1] << 16) | (rm_report.y[2]) << 8);
+    zraw = ((rm_report.z[0] << 24) | (rm_report.z[1] << 16) | (rm_report.z[2]) << 8);
+
+    /* Truncate to 16-bit integers and pass along */
+    mag->magADCRaw[X] = (int16_t)(xraw >> 16);
+    mag->magADCRaw[Y] = (int16_t)(yraw >> 16);
+    mag->magADCRaw[Z] = (int16_t)(zraw >> 16);
+
+    return true;
+}
+
+#define DETECTION_MAX_RETRY_COUNT   5
+static bool deviceDetect(magDev_t * mag)
+{
+    for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
+        uint8_t revid = 0;
+        bool ack = busRead(mag->busDev, RM3100_REG_REVID, &revid);
+
+        if (ack && revid == RM3100_REVID) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool rm3100MagDetect(magDev_t * mag)
+{
+    busSetSpeed(mag->busDev, BUS_SPEED_STANDARD);
+
+    mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_RM3100, mag->magSensorToUse, OWNER_COMPASS);
+    if (mag->busDev == NULL) {
+        return false;
+    }
+
+    if (!deviceDetect(mag)) {
+        busDeviceDeInit(mag->busDev);
+        return false;
+    }
+
+    mag->init = deviceInit;
+    mag->read = deviceRead;
+
+    return true;
+}
+
+#endif
diff --git a/src/main/drivers/compass/compass_rm3100.h b/src/main/drivers/compass/compass_rm3100.h
@@ -0,0 +1,27 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file 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, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+bool rm3100MagDetect(magDev_t *mag);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -10,7 +10,7 @@ tables:
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"]
     enum: rangefinderType_e
   - name: mag_hardware
-    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"]
+    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"]
     enum: magSensor_e
   - name: opflow_hardware
     values: ["NONE", "CXOF", "MSP", "FAKE"]
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
@@ -40,6 +40,7 @@
 #include "drivers/compass/compass_qmc5883l.h"
 #include "drivers/compass/compass_mpu9250.h"
 #include "drivers/compass/compass_lis3mdl.h"
+#include "drivers/compass/compass_rm3100.h"
 #include "drivers/compass/compass_msp.h"
 #include "drivers/io.h"
 #include "drivers/light_led.h"
@@ -264,6 +265,22 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
         }
         FALLTHROUGH;
 
+    case MAG_RM3100:
+#ifdef USE_MAG_RM3100
+        if (rm3100MagDetect(dev)) {
+#ifdef MAG_RM3100_ALIGN
+            dev->magAlign.onBoard = MAG_RM3100_ALIGN;
+#endif
+            magHardware = MAG_RM3100;
+            break;
+        }
+#endif
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (magHardwareToUse != MAG_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+
     case MAG_FAKE:
 #ifdef USE_FAKE_MAG
         if (fakeMagDetect(dev)) {
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
@@ -42,7 +42,8 @@ typedef enum {
     MAG_IST8308 = 10,
     MAG_LIS3MDL = 11,
     MAG_MSP = 12,
-    MAG_FAKE = 13,
+    MAG_RM3100 = 13,
+    MAG_FAKE = 14,
     MAG_MAX = MAG_FAKE
 } magSensor_e;
 
diff --git a/src/main/target/MATEKF405CAN/CMakeLists.txt b/src/main/target/MATEKF405CAN/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(MATEKF405CAN)
diff --git a/src/main/target/MATEKF405CAN/config.c b/src/main/target/MATEKF405CAN/config.c
@@ -0,0 +1,31 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+#include "config/config_master.h"
+#include "config/feature.h"
+#include "io/serial.h"
+
+
+void targetConfiguration(void)
+{
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS;
+    // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200;
+
+}
diff --git a/src/main/target/MATEKF405CAN/target.c b/src/main/target/MATEKF405CAN/target.c
@@ -0,0 +1,44 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <platform.h>
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+
+const timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 1), // S1 D(2,2,7)
+    DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 1), // S2 D(2,3,7)
+    DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 1), // S3 D(2,4,7)
+    DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S4 D(2,7,7)
+    DEF_TIM(TIM3,  CH3, PB0,  TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S5 D(1,7,5)
+    DEF_TIM(TIM3,  CH4, PB1,  TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S6 D(1,2,5)
+
+    DEF_TIM(TIM4,  CH1, PB6,  TIM_USE_MC_MOTOR  | TIM_USE_FW_MOTOR,   1, 0), // S7 D(1,0,2)
+    DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_MC_MOTOR  | TIM_USE_FW_MOTOR,   1, 0), // S8 D(1,3,2)
+
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_BEEPER,  0, 0), // BEEPER PWM
+
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_LED,     0, 0), //2812LED  D(1,5,3)
+
+    DEF_TIM(TIM9,  CH2, PA3,  TIM_USE_PPM,     0, 0), //RX2
+    DEF_TIM(TIM5,  CH3, PA2,  TIM_USE_ANY,     0, 0), //TX2  softserial1_Tx
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
+
diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
