Unverified Commit 209e47c3 authored by Paweł Spychalski's avatar Paweł Spychalski Committed by GitHub
Browse files

Merge pull request #6802 from iNavFlight/dzikuvx-bno055-serial

Serial interface for BNO055 IMU
parents 0356952c 611e5a22
......@@ -105,6 +105,8 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro_mpu9250.h
drivers/accgyro/accgyro_bno055.c
drivers/accgyro/accgyro_bno055.h
drivers/accgyro/accgyro_bno055_serial.c
drivers/accgyro/accgyro_bno055_serial.h
drivers/adc.c
drivers/adc.h
......
......@@ -32,47 +32,6 @@
#ifdef USE_IMU_BNO055
#define BNO055_ADDR_PWR_MODE 0x3E
#define BNO055_ADDR_OPR_MODE 0x3D
#define BNO055_ADDR_CALIB_STAT 0x35
#define BNO055_PWR_MODE_NORMAL 0x00
#define BNO055_OPR_MODE_CONFIG 0x00
#define BNO055_OPR_MODE_NDOF 0x0C
#define BNO055_ADDR_EUL_YAW_LSB 0x1A
#define BNO055_ADDR_EUL_YAW_MSB 0x1B
#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
#define BNO055_ADDR_EUL_PITCH_MSB 0x1F
#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A
#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
#define BNO055_ADDR_ACC_RADIUS_LSB 0x67
#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61
#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B
#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55
static busDevice_t *busDev;
static bool deviceDetect(busDevice_t *busDev)
......@@ -99,18 +58,45 @@ static void bno055SetMode(uint8_t mode)
delay(25);
}
static void bno055SetCalibrationData(bno055CalibrationData_t data)
{
uint8_t buf[12];
//Prepare gains
//We do not restore gyro offsets, they are quickly calibrated at startup
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
bufferBit += 2;
}
}
busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
//Prepare radius
buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
//Write to the device
busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
}
bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration)
{
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;
}
......@@ -192,33 +178,4 @@ bno055CalibrationData_t bno055GetCalibrationData(void)
return data;
}
void bno055SetCalibrationData(bno055CalibrationData_t data)
{
uint8_t buf[12];
//Prepare gains
//We do not restore gyro offsets, they are quickly calibrated at startup
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
bufferBit += 2;
}
}
busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
//Prepare radius
buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
//Write to the device
busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
}
#endif
\ No newline at end of file
......@@ -25,6 +25,47 @@
#include "common/vector.h"
#define BNO055_ADDR_PWR_MODE 0x3E
#define BNO055_ADDR_OPR_MODE 0x3D
#define BNO055_ADDR_CALIB_STAT 0x35
#define BNO055_PWR_MODE_NORMAL 0x00
#define BNO055_OPR_MODE_CONFIG 0x00
#define BNO055_OPR_MODE_NDOF 0x0C
#define BNO055_ADDR_EUL_YAW_LSB 0x1A
#define BNO055_ADDR_EUL_YAW_MSB 0x1B
#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
#define BNO055_ADDR_EUL_PITCH_MSB 0x1F
#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A
#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
#define BNO055_ADDR_ACC_RADIUS_LSB 0x67
#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61
#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B
#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55
typedef struct {
uint8_t sys;
uint8_t gyr;
......@@ -47,5 +88,4 @@ bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration);
fpVector3_t bno055GetEurlerAngles(void);
void bno055FetchEulerAngles(int16_t * buffer);
bno055CalibStat_t bno055GetCalibStat(void);
bno055CalibrationData_t bno055GetCalibrationData(void);
void bno055SetCalibrationData(bno055CalibrationData_t data);
\ No newline at end of file
bno055CalibrationData_t bno055GetCalibrationData(void);
\ No newline at end of file
/*
* 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"
#ifdef USE_IMU_BNO055
#define BNO055_BAUD_RATE 115200
#define BNO055_FRAME_MAX_TIME_MS 10
#include <stdbool.h>
#include <stdint.h>
#include "io/serial.h"
#include "drivers/accgyro/accgyro_bno055.h"
#include "build/debug.h"
#include "drivers/time.h"
#include "flight/secondary_imu.h"
static serialPort_t * bno055SerialPort = NULL;
static uint8_t receiveBuffer[22];
typedef enum {
BNO055_RECEIVE_IDLE,
BNO055_RECEIVE_HEADER,
BNO055_RECEIVE_LENGTH,
BNO055_RECEIVE_PAYLOAD,
BNO055_RECEIVE_ACK,
} bno055ReceiveState_e;
typedef enum {
BNO055_FRAME_ACK,
BNO055_FRAME_DATA,
} bno055FrameType_e;
typedef enum {
BNO055_DATA_TYPE_NONE,
BNO055_DATA_TYPE_EULER,
BNO055_DATA_TYPE_CALIBRATION_STATS,
} bno055DataType_e;
static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE;
static uint8_t bno055FrameType;
static uint8_t bno055FrameLength;
static uint8_t bno055FrameIndex;
static timeMs_t bno055FrameStartAtMs = 0;
static uint8_t bno055DataType = BNO055_DATA_TYPE_NONE;
static void bno055SerialWriteBuffer(const uint8_t reg, const uint8_t *data, const uint8_t count) {
bno055ProtocolState = BNO055_RECEIVE_IDLE;
serialWrite(bno055SerialPort, 0xAA); // Start Byte
serialWrite(bno055SerialPort, 0x00); // Write command
serialWrite(bno055SerialPort, reg);
serialWrite(bno055SerialPort, count);
for (uint8_t i = 0; i < count; i++) {
serialWrite(bno055SerialPort, data[i]);
}
}
static void bno055SerialWrite(const uint8_t reg, const uint8_t data) {
uint8_t buff[1];
buff[0] = data;
bno055SerialWriteBuffer(reg, buff, 1);
}
static void bno055SerialRead(const uint8_t reg, const uint8_t len) {
bno055ProtocolState = BNO055_RECEIVE_IDLE;
serialWrite(bno055SerialPort, 0xAA); // Start Byte
serialWrite(bno055SerialPort, 0x01); // Read command
serialWrite(bno055SerialPort, reg);
serialWrite(bno055SerialPort, len);
}
void bno055SerialDataReceive(uint16_t c, void *data) {
UNUSED(data);
const uint8_t incoming = (uint8_t) c;
//Failsafe for stuck frames
if (bno055ProtocolState != BNO055_RECEIVE_IDLE && millis() - bno055FrameStartAtMs > BNO055_FRAME_MAX_TIME_MS) {
bno055ProtocolState = BNO055_RECEIVE_IDLE;
}
if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xEE) {
bno055FrameStartAtMs = millis();
bno055ProtocolState = BNO055_RECEIVE_HEADER;
bno055FrameType = BNO055_FRAME_ACK;
} else if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xBB) {
bno055FrameStartAtMs = millis();
bno055ProtocolState = BNO055_RECEIVE_HEADER;
bno055FrameType = BNO055_FRAME_DATA;
} else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_ACK) {
receiveBuffer[0] = incoming;
bno055ProtocolState = BNO055_RECEIVE_IDLE;
} else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_DATA) {
bno055FrameLength = incoming;
bno055FrameIndex = 0;
bno055ProtocolState = BNO055_RECEIVE_LENGTH;
} else if (bno055ProtocolState == BNO055_RECEIVE_LENGTH) {
receiveBuffer[bno055FrameIndex] = incoming;
bno055FrameIndex++;
if (bno055FrameIndex == bno055FrameLength) {
bno055ProtocolState = BNO055_RECEIVE_IDLE;
if (bno055DataType == BNO055_DATA_TYPE_EULER) {
secondaryImuState.eulerAngles.raw[0] = ((int16_t)((receiveBuffer[3] << 8) | receiveBuffer[2])) / 1.6f;
secondaryImuState.eulerAngles.raw[1] = ((int16_t)((receiveBuffer[5] << 8) | receiveBuffer[4])) / -1.6f; //Pitch has to be reversed to match INAV notation
secondaryImuState.eulerAngles.raw[2] = ((int16_t)((receiveBuffer[1] << 8) | receiveBuffer[0])) / 1.6f;
secondaryImuProcess();
} else if (bno055DataType == BNO055_DATA_TYPE_CALIBRATION_STATS) {
secondaryImuState.calibrationStatus.mag = receiveBuffer[0] & 0b00000011;
secondaryImuState.calibrationStatus.acc = (receiveBuffer[0] >> 2) & 0b00000011;
secondaryImuState.calibrationStatus.gyr = (receiveBuffer[0] >> 4) & 0b00000011;
secondaryImuState.calibrationStatus.sys = (receiveBuffer[0] >> 6) & 0b00000011;
}
bno055DataType = BNO055_DATA_TYPE_NONE;
}
}
}
static void bno055SerialSetCalibrationData(bno055CalibrationData_t data)
{
uint8_t buf[12];
//Prepare gains
//We do not restore gyro offsets, they are quickly calibrated at startup
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
bufferBit += 2;
}
}
bno055SerialWriteBuffer(BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
delay(25);
//Prepare radius
buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
//Write to the device
bno055SerialWriteBuffer(BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
delay(25);
}
bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration) {
bno055SerialPort = NULL;
serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_IMU2);
if (!portConfig) {
return false;
}
bno055SerialPort = openSerialPort(
portConfig->identifier,
FUNCTION_IMU2,
bno055SerialDataReceive,
NULL,
BNO055_BAUD_RATE,
MODE_RXTX,
SERIAL_NOT_INVERTED | SERIAL_UNIDIR | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO
);
if (!bno055SerialPort) {
return false;
}
bno055SerialRead(0x00, 1); // Read ChipID
delay(5);
//Check ident
if (bno055FrameType != BNO055_FRAME_DATA || receiveBuffer[0] != 0xA0 || bno055ProtocolState != BNO055_RECEIVE_IDLE) {
return false; // Ident does not match, leave
}
bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
delay(25);
if (setCalibration) {
bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG);
delay(25);
bno055SerialSetCalibrationData(calibrationData);
}
bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF);
delay(25);
return true;
}
/*
* This function is non-blocking and response will be processed by bno055SerialDataReceive
*/
void bno055SerialFetchEulerAngles() {
bno055DataType = BNO055_DATA_TYPE_EULER;
bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6);
}
/*
* This function is non-blocking and response will be processed by bno055SerialDataReceive
*/
void bno055SerialGetCalibStat(void) {
bno055DataType = BNO055_DATA_TYPE_CALIBRATION_STATS;
bno055SerialRead(BNO055_ADDR_CALIB_STAT, 1);
}
/*
* This function is blocking and should not be used during flight conditions!
*/
bno055CalibrationData_t bno055SerialGetCalibrationData(void) {
bno055CalibrationData_t data;
bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG);
delay(25);
bno055SerialRead(BNO055_ADDR_ACC_OFFSET_X_LSB, 22);
delay(50);
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
data.offset[sensorIndex][axisIndex] = (int16_t)((receiveBuffer[bufferBit + 1] << 8) | receiveBuffer[bufferBit]);
bufferBit += 2;
}
}
data.radius[ACC] = (int16_t)((receiveBuffer[19] << 8) | receiveBuffer[18]);
data.radius[MAG] = (int16_t)((receiveBuffer[21] << 8) | receiveBuffer[20]);
bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF);
delay(25);
return data;
}
#endif
\ No newline at end of file
/*
* 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/.
*/
#pragma once
#include "common/vector.h"
#include "drivers/accgyro/accgyro_bno055.h"
bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration);
void bno055SerialFetchEulerAngles(void);
void bno055SerialGetCalibStat(void);
bno055CalibrationData_t bno055SerialGetCalibrationData(void);
\ No newline at end of file
......@@ -10,7 +10,7 @@ tables:
values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"]
enum: rangefinderType_e
- name: secondary_imu_hardware
values: ["NONE", "BNO055"]
values: ["NONE", "BNO055", "BNO055_SERIAL"]
enum: secondaryImuType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"]
......
......@@ -29,10 +29,13 @@
#include "common/utils.h"
#include "common/axis.h"
#include "build/debug.h"
#include "scheduler/scheduler.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro_bno055.h"
#include "drivers/accgyro/accgyro_bno055_serial.h"
#include "fc/settings.h"
......@@ -94,6 +97,17 @@ void secondaryImuInit(void)
if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
if (secondaryImuState.active) {
rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(10));
}
} else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
if (secondaryImuState.active) {
rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50));
}
}
if (!secondaryImuState.active) {
......@@ -102,19 +116,7 @@ void secondaryImuInit(void)
}
void taskSecondaryImu(timeUs_t currentTimeUs)
{
if (!secondaryImuState.active)
{
return;
}
/*
* Secondary IMU works in decidegrees
*/
UNUSED(currentTimeUs);
bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
void secondaryImuProcess(void) {
//Apply mag declination
secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
......@@ -141,17 +143,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
secondaryImuState.eulerAngles.values.pitch = rotated.y;
secondaryImuState.eulerAngles.values.yaw = rotated.z;
/*
* Every 10 cycles fetch current calibration state
*/
static uint8_t tick = 0;
tick++;
if (tick == 10)
{
secondaryImuState.calibrationStatus = bno055GetCalibStat();
tick = 0;
}
DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll);
DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch);
DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw);
......@@ -162,8 +153,56 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination);
}
void taskSecondaryImu(timeUs_t currentTimeUs)
{
static uint8_t tick = 0;
tick++;
if (!secondaryImuState.active)
{
return;
}
/*
* Secondary IMU works in decidegrees
*/
UNUSED(currentTimeUs);
if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
secondaryImuProcess();
/*
* Every 2 seconds fetch current calibration state
*/
if (tick == 20)
{