Merge pull request #3868 from shellixyz/KAKUTEF7_target

Add KAKUTEF7 target
iNavFlight · Sep 17, 2018 · 3b1b1bf · 3b1b1bf
2 parents 855bce4 + 49a4d51
commit 3b1b1bf
Show file tree

Hide file tree

Showing 13 changed files with 441 additions and 4 deletions.
diff --git a/src/main/drivers/accgyro/accgyro_icm20689.c b/src/main/drivers/accgyro/accgyro_icm20689.c
@@ -0,0 +1,149 @@
+/*
+ * This file is part of iNavFlight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "drivers/system.h"
+#include "drivers/time.h"
+#include "drivers/io.h"
+#include "drivers/exti.h"
+#include "drivers/bus.h"
+
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+#include "drivers/accgyro/accgyro_icm20689.h"
+
+#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689))
+
+static uint8_t icm20689DeviceDetect(const busDevice_t *busDev)
+{
+    busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
+
+    busWrite(busDev, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
+
+    uint8_t attemptsRemaining = 20;
+    uint8_t in;
+    do {
+        delay(150);
+        busRead(busDev, MPU_RA_WHO_AM_I, &in);
+        switch (in) {
+        case ICM20601_WHO_AM_I_CONST:
+        case ICM20602_WHO_AM_I_CONST:
+        case ICM20608G_WHO_AM_I_CONST:
+        case ICM20689_WHO_AM_I_CONST:
+            return true;
+        }
+    } while (attemptsRemaining--);
+
+    return false;
+}
+
+static void icm20689AccInit(accDev_t *acc)
+{
+    acc->acc_1G = 512 * 4;
+}
+
+bool icm20689AccDetect(accDev_t *acc)
+{
+    acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM20689, acc->imuSensorToUse);
+    if (acc->busDev == NULL) {
+        return false;
+    }
+
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+    if (ctx->chipMagicNumber != 0x50D1) {
+        return false;
+    }
+
+    acc->initFn = icm20689AccInit;
+    acc->readFn = mpuAccReadScratchpad;
+
+    return true;
+}
+
+static void icm20689AccAndGyroInit(gyroDev_t *gyro)
+{
+    busDevice_t * busDev = gyro->busDev;
+    const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs);
+    gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
+
+    gyroIntExtiInit(gyro);
+
+    busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
+
+    busWrite(busDev, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
+    delay(100);
+    busWrite(busDev, MPU_RA_SIGNAL_PATH_RESET, 0x03);
+    delay(100);
+    busWrite(busDev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
+    delay(15);
+    busWrite(busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); // XXX
+    delay(15);
+    busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
+    delay(15);
+    busWrite(busDev, MPU_RA_CONFIG, config->gyroConfigValues[0]);
+    delay(15);
+    busWrite(busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]); // Get Divider Drops
+    delay(100);
+
+    // Data ready interrupt configuration
+    busWrite(busDev, MPU_RA_INT_PIN_CFG, 0x10);  // INT_ANYRD_2CLEAR, BYPASS_EN
+
+    delay(15);
+
+#ifdef USE_MPU_DATA_READY_SIGNAL
+    busWrite(busDev, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
+#endif
+}
+
+bool icm20689GyroDetect(gyroDev_t *gyro)
+{
+    gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM20689, gyro->imuSensorToUse, OWNER_MPU);
+    if (gyro->busDev == NULL) {
+        return false;
+    }
+
+    if (!icm20689DeviceDetect(gyro->busDev)) {
+        busDeviceDeInit(gyro->busDev);
+        return false;
+    }
+
+    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+    ctx->chipMagicNumber = 0x50D1;
+
+    gyro->initFn = icm20689AccAndGyroInit;
+    gyro->readFn = mpuGyroReadScratchpad;
+    gyro->intStatusFn = gyroCheckDataReady;
+    gyro->temperatureFn = mpuTemperatureReadScratchpad;
+    gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+
+    return true;
+}
+
+#endif
diff --git a/src/main/drivers/accgyro/accgyro_icm20689.h b/src/main/drivers/accgyro/accgyro_icm20689.h
@@ -0,0 +1,32 @@
+/*
+ * This file is part of iNavFlight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "drivers/bus.h"
+
+#define ICM20689_BIT_RESET                  (0x80)
+
+#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689))
+
+bool icm20689AccDetect(accDev_t *acc);
+bool icm20689GyroDetect(gyroDev_t *gyro);
+
+#endif
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
@@ -83,6 +83,7 @@ typedef enum {
     DEVHW_MPU6050,
     DEVHW_MPU6500,
     DEVHW_BMI160,
+    DEVHW_ICM20689,
 
     /* Combined ACC/GYRO/MAG chips */
     DEVHW_MPU9250,

diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
@@ -36,7 +36,7 @@
 #include "serial_uart_impl.h"
 
 static void usartConfigurePinInversion(uartPort_t *uartPort) {
-#if !defined(USE_UART_INVERTER) && !defined(STM32F303xC)
+#if !defined(USE_UART_INVERTER) && !defined(STM32F303xC) && !defined(STM32F7)
     UNUSED(uartPort);
 #else
     bool inverted = uartPort->port.options & SERIAL_INVERTED;

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
@@ -149,7 +149,7 @@ static const char * const featureNames[] = {
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
 // sync with gyroSensor_e
-static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "FAKE"};
+static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"};
 
 // sync this with sensors_e
 static const char * const sensorTypeNames[] = {

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -4,7 +4,7 @@ tables:
   - name: gyro_lpf
     values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"]
   - name: acc_hardware
-    values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "FAKE"]
+    values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB"]

diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
@@ -47,6 +47,7 @@
 #include "drivers/accgyro/accgyro_mma845x.h"
 #include "drivers/accgyro/accgyro_bma280.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
+#include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_fake.h"
 #include "drivers/logging.h"
 #include "drivers/sensor.h"
@@ -266,6 +267,23 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
+#ifdef USE_ACC_ICM20689
+    case ACC_ICM20689:
+        if (icm20689AccDetect(dev)) {
+#ifdef ACC_ICM20689_ALIGN
+            dev->accAlign = ACC_ICM20689_ALIGN;
+#endif
+            accHardware = ACC_ICM20689;
+            break;
+        }
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (accHardwareToUse != ACC_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+
 #ifdef USE_FAKE_ACC
     case ACC_FAKE:
         if (fakeAccDetect(dev)) {

diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
@@ -44,7 +44,8 @@ typedef enum {
     ACC_MPU6500 = 8,
     ACC_MPU9250 = 9,
     ACC_BMI160 = 10,
-    ACC_FAKE = 11,
+    ACC_ICM20689 = 11,
+    ACC_FAKE = 12,
     ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -48,6 +48,7 @@
 #include "drivers/accgyro/accgyro_mma845x.h"
 #include "drivers/accgyro/accgyro_bma280.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
+#include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_fake.h"
 #include "drivers/io.h"
 #include "drivers/logging.h"
@@ -221,6 +222,18 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
+#ifdef USE_GYRO_ICM20689
+    case GYRO_ICM20689:
+        if (icm20689GyroDetect(dev)) {
+            gyroHardware = GYRO_ICM20689;
+#ifdef GYRO_ICM20689_ALIGN
+            dev->gyroAlign = GYRO_ICM20689_ALIGN;
+#endif
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_FAKE_GYRO
     case GYRO_FAKE:
         if (fakeGyroDetect(dev)) {

diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
@@ -35,6 +35,7 @@ typedef enum {
     GYRO_MPU6500,
     GYRO_MPU9250,
     GYRO_BMI160,
+    GYRO_ICM20689,
     GYRO_FAKE
 } gyroSensor_e;
 

diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c
@@ -0,0 +1,46 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 <platform.h>
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/bus.h"
+
+BUSDEV_REGISTER_SPI_TAG(busdev_icm20689,     DEVHW_ICM20689,      ICM20689_SPI_BUS,    ICM20689_CS_PIN,     ICM20689_EXTI_PIN,       0,  DEVFLAGS_NONE);
+
+const timerHardware_t timerHardware[] = {
+    { TIM1,  IO_TAG(PE13), TIM_CHANNEL_3, 0, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_PPM }, // PPM
+
+    { TIM3,  IO_TAG(PB0),  TIM_CHANNEL_3, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR                    | TIM_USE_FW_MOTOR }, // MOTOR_1
+    { TIM3,  IO_TAG(PB1),  TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR                    | TIM_USE_FW_MOTOR }, // MOTOR_2
+    { TIM1,  IO_TAG(PE9),  TIM_CHANNEL_1, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR                    | TIM_USE_FW_SERVO }, // MOTOR_3
+    { TIM1,  IO_TAG(PE11), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR                    | TIM_USE_FW_SERVO }, // MOTOR_4
+    { TIM8,  IO_TAG(PC9),  TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR                    | TIM_USE_FW_SERVO }, // MOTOR_5
+    { TIM5,  IO_TAG(PA3),  TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR                    | TIM_USE_FW_SERVO }, // MOTOR_6
+
+    { TIM4,  IO_TAG(PD12), TIM_CHANNEL_1, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_LED }
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);