From 9f2e80029c19e0057551cc0d3cc151a6fd9ba473 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Sep 2019 20:26:58 +0200 Subject: [PATCH] dzukuvx-bno055-secondary-imu --- make/source.mk | 2 + src/main/build/debug.h | 1 + src/main/drivers/accgyro/accgyro_bno055.c | 77 +++++++++++++++++++++++ src/main/drivers/accgyro/accgyro_bno055.h | 25 ++++++++ src/main/drivers/bus.h | 1 + src/main/fc/fc_core.c | 15 +++++ src/main/fc/fc_core.h | 1 + src/main/fc/fc_tasks.c | 7 +++ src/main/fc/settings.yaml | 2 +- src/main/scheduler/scheduler.h | 1 + src/main/target/common_hardware.c | 4 +- 11 files changed, 134 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_bno055.c create mode 100644 src/main/drivers/accgyro/accgyro_bno055.h diff --git a/make/source.mk b/make/source.mk index 3a0d90d1e1..1da9d25bd4 100644 --- a/make/source.mk +++ b/make/source.mk @@ -66,6 +66,8 @@ COMMON_SRC = \ drivers/temperature/ds18b20.c \ drivers/temperature/lm75.c \ drivers/pitotmeter_ms4525.c \ + drivers/pitotmeter_ms4525.c \ + drivers/accgyro/accgyro_bno055.c \ fc/cli.c \ fc/config.c \ fc/controlrate_profile.c \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b7453e8836..f4a03274d8 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -74,5 +74,6 @@ typedef enum { DEBUG_FFT, DEBUG_FFT_TIME, DEBUG_FFT_FREQ, + DEBUG_IMU2, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c new file mode 100644 index 0000000000..ef84a30090 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -0,0 +1,77 @@ +/* + * 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 +#include +#include "drivers/bus.h" +#include "drivers/time.h" +#include "build/debug.h" + +static busDevice_t * busDev; + +static bool deviceDetect(busDevice_t * busDev) +{ + for (int retry = 0; retry < 5; retry++) { + uint8_t sig; + + delay(150); + + bool ack = busRead(busDev, 0x00, &sig); + if (ack) { + return true; + } + }; + + return false; +} + +bool bno055Init(void) +{ + busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); + if (busDev == NULL) { + DEBUG_SET(DEBUG_IMU2, 2, 1); + return false; + } + + if (!deviceDetect(busDev)) { + DEBUG_SET(DEBUG_IMU2, 2, 2); + busDeviceDeInit(busDev); + return false; + } + + // /* Reset device */ + // busWrite(busDev, PCA9685_MODE1, 0x00); + + // /* Set refresh rate */ + // pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY); + + // delay(1); + + // for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) { + // pca9685setPWMOn(i, 0); + // pca9685setPWMOff(i, 1500); + // } + + return true; +} \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h new file mode 100644 index 0000000000..d62af1d53e --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -0,0 +1,25 @@ +/* + * 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/. + */ + +bool bno055Init(void); \ No newline at end of file diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 283d45e3f4..83a25a6c17 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -142,6 +142,7 @@ typedef enum { DEVHW_M25P16, // SPI NOR flash DEVHW_UG2864, // I2C OLED display DEVHW_SDCARD, // Generic SD-Card + DEVHW_BNO055, // BNO055 IMU } devHardwareType_e; typedef enum { diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 497e153be6..7066ed2e06 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -37,6 +37,7 @@ #include "drivers/time.h" #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/accgyro/accgyro_bno055.h" #include "sensors/sensors.h" #include "sensors/diagnostics.h" @@ -748,6 +749,20 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens } } +void taskSecondaryImu(timeUs_t currentTimeUs) +{ + static bool secondaryImuPresent = false; + static bool secondaryImuChecked = false; + + if (!secondaryImuChecked) { + secondaryImuPresent = bno055Init(); + secondaryImuChecked = true; + } + + DEBUG_SET(DEBUG_IMU2, 0, secondaryImuChecked); + DEBUG_SET(DEBUG_IMU2, 1, secondaryImuPresent); +} + void taskMainPidLoop(timeUs_t currentTimeUs) { cycleTime = getTaskDeltaTime(TASK_SELF); diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index a7317e48d1..7caf10d0f5 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -46,3 +46,4 @@ bool isCalibrating(void); float getFlightTime(void); void fcReboot(bool bootLoader); +void taskSecondaryImu(timeUs_t currentTimeUs); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 553c348ab6..26e5b0f903 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -343,6 +343,7 @@ void fcTasksInit(void) #ifdef USE_GLOBAL_FUNCTIONS setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true); #endif + setTaskEnabled(TASK_SECONDARY_IMU, true); } cfTask_t cfTasks[TASK_COUNT] = { @@ -564,4 +565,10 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif + [TASK_SECONDARY_IMU] = { + .taskName = "IMU2", + .taskFunc = taskSecondaryImu, + .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec + .staticPriority = TASK_PRIORITY_IDLE, + }, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 65e47fd1d0..fdd241c464 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -78,7 +78,7 @@ tables: values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", - "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ"] + "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "DEBUG_IMU2"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5cf996f02e..4b1d7c252a 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -116,6 +116,7 @@ typedef enum { #ifdef USE_GLOBAL_FUNCTIONS TASK_GLOBAL_FUNCTIONS, #endif + TASK_SECONDARY_IMU, /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 7c399dbd32..97eec0be1c 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -312,7 +312,9 @@ #define PCA9685_I2C_BUS BUS_I2C1 #endif BUSDEV_REGISTER_I2C(busdev_pca9685, DEVHW_PCA9685, PCA9685_I2C_BUS, 0x40, NONE, DEVFLAGS_NONE); - #endif + #endif` #endif + BUSDEV_REGISTER_I2C(busdev_bno055, DEVHW_BNO055, BUS_I2C1, 0x29, NONE, DEVFLAGS_NONE); + #endif // USE_TARGET_HARDWARE_DESCRIPTORS