Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Drop IMU2 functionality

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2022-10-23 09:47:13 +02:00
parent d1f6f19aa1
commit 12e5c90fd5
31 changed files with 10 additions and 1154 deletions
Download patch file Download diff file Expand all files Collapse all files

3
docs/Cli.md
View file

@ -89,7 +89,6 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0
| `gpspassthrough` | Passthrough gps to serial |
| `gvar` | Configure global variables |
| `help` | Displays CLI help and command parameters / options |
| `imu2` | Secondary IMU |
| `led` | Configure leds |
| `logic` | Configure logic conditions |
| `map` | Configure rc channel order |
@ -155,7 +154,7 @@ A shorter form is also supported to enable and disable a single function using `
| DJI_HD_OSD | 21 | 2097152 |
| SERVO_SERIAL | 22 | 4194304 |
| TELEMETRY_SMARTPORT_MASTER | 23 | 8388608 |
| IMU2 | 24 | 16777216 |
| UNUSED | 24 | 16777216 |
| MSP_DISPLAYPORT | 25 | 33554432 |
Thus, to enable MSP and LTM on a port, one would use the function **value** of 17 (1 << 0)+(1<<4), aka 1+16, aka 17.

2
docs/development/IDE - Visual Studio Code with Windows 10.md
View file

@ -54,8 +54,6 @@ Edit file `./.vscode/c_cpp_properties.json` to setup enabled `defines`
"USE_RPM_FILTER",
"USE_GLOBAL_FUNCTIONS",
"USE_DYNAMIC_FILTERS",
"USE_IMU_BNO055",
"USE_SECONDARY_IMU",
"USE_DSHOT",
"FLASH_SIZE 480",
"USE_I2C_IO_EXPANDER",

2
docs/development/Windows 11 - VS Code - WSL2 - Hardware Debugging.md
View file

@ -249,8 +249,6 @@ sudo udevadm control --reload-rules
"USE_RPM_FILTER",
"USE_GLOBAL_FUNCTIONS",
"USE_DYNAMIC_FILTERS",
"USE_IMU_BNO055",
"USE_SECONDARY_IMU",
"USE_DSHOT",
"FLASH_SIZE 480",
"USE_I2C_IO_EXPANDER",

4
src/main/CMakeLists.txt
View file

@ -92,8 +92,6 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro_mpu6500.h
drivers/accgyro/accgyro_mpu9250.c
drivers/accgyro/accgyro_mpu9250.h
drivers/accgyro/accgyro_bno055_serial.c
drivers/accgyro/accgyro_bno055_serial.h
drivers/adc.c
drivers/adc.h
@ -333,8 +331,6 @@ main_sources(COMMON_SRC
flight/secondary_dynamic_gyro_notch.h
flight/dynamic_lpf.c
flight/dynamic_lpf.h
flight/secondary_imu.c
flight/secondary_imu.h
io/beeper.c
io/beeper.h

1
src/main/build/debug.h
View file

@ -66,7 +66,6 @@ typedef enum {
DEBUG_PCF8574,
DEBUG_DYNAMIC_GYRO_LPF,
DEBUG_AUTOLEVEL,
DEBUG_IMU2,
DEBUG_ALTITUDE,
DEBUG_AUTOTRIM,
DEBUG_AUTOTUNE,

2
src/main/config/parameter_group_ids.h
View file

@ -116,7 +116,7 @@
#define PG_SAFE_HOME_CONFIG 1026
#define PG_DJI_OSD_CONFIG 1027
#define PG_PROGRAMMING_PID 1028
#define PG_SECONDARY_IMU 1029
#define PG_UNUSED_1 1029
#define PG_POWER_LIMITS_CONFIG 1030
#define PG_OSD_COMMON_CONFIG 1031
#define PG_INAV_END 1031

180
src/main/drivers/accgyro/accgyro_bno055.c
View file

@ -1,180 +0,0 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include "drivers/bus.h"
#include "drivers/time.h"
#include "build/debug.h"
#include "common/vector.h"
#ifdef USE_IMU_BNO055
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;
}
static void bno055SetMode(uint8_t mode)
{
busWrite(busDev, BNO055_ADDR_OPR_MODE, 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)
{
return false;
}
if (!deviceDetect(busDev))
{
busDeviceDeInit(busDev);
return false;
}
busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
delay(25);
if (setCalibration) {
bno055SetMode(BNO055_OPR_MODE_CONFIG);
bno055SetCalibrationData(calibrationData);
}
bno055SetMode(BNO055_OPR_MODE_NDOF);
return true;
}
void bno055FetchEulerAngles(int16_t *buffer)
{
uint8_t buf[6];
busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 1.6f;
buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -1.6f; //Pitch has to be reversed to match INAV notation
buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 1.6f;
}
fpVector3_t bno055GetEurlerAngles(void)
{
fpVector3_t eurlerAngles;
uint8_t buf[6];
busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
eurlerAngles.x = ((int16_t)((buf[3] << 8) | buf[2])) / 16;
eurlerAngles.y = ((int16_t)((buf[5] << 8) | buf[4])) / 16;
eurlerAngles.z = ((int16_t)((buf[1] << 8) | buf[0])) / 16;
return eurlerAngles;
}
bno055CalibStat_t bno055GetCalibStat(void)
{
bno055CalibStat_t stats;
uint8_t buf;
busRead(busDev, BNO055_ADDR_CALIB_STAT, &buf);
stats.mag = buf & 0b00000011;
stats.acc = (buf >> 2) & 0b00000011;
stats.gyr = (buf >> 4) & 0b00000011;
stats.sys = (buf >> 6) & 0b00000011;
return stats;
}
bno055CalibrationData_t bno055GetCalibrationData(void)
{
bno055CalibrationData_t data;
uint8_t buf[22];
bno055SetMode(BNO055_OPR_MODE_CONFIG);
busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22);
bno055SetMode(BNO055_OPR_MODE_NDOF);
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)((buf[bufferBit + 1] << 8) | buf[bufferBit]);
bufferBit += 2;
}
}
data.radius[ACC] = (int16_t)((buf[19] << 8) | buf[18]);
data.radius[MAG] = (int16_t)((buf[21] << 8) | buf[20]);
return data;
}
#endif

33
src/main/drivers/accgyro/accgyro_bno055.h
View file

@ -1,33 +0,0 @@
/*
* 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"
bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration);
fpVector3_t bno055GetEurlerAngles(void);
void bno055FetchEulerAngles(int16_t * buffer);
bno055CalibStat_t bno055GetCalibStat(void);
bno055CalibrationData_t bno055GetCalibrationData(void);

270
src/main/drivers/accgyro/accgyro_bno055_serial.c
View file

@ -1,270 +0,0 @@
/*
* 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 "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

91
src/main/drivers/accgyro/accgyro_bno055_serial.h
View file

@ -1,91 +0,0 @@
/*
* 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"
#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 enum {
ACC = 0,
MAG = 1,
GYRO = 2
} bno055Sensor_e;
typedef struct {
uint8_t sys;
uint8_t gyr;
uint8_t acc;
uint8_t mag;
} bno055CalibStat_t;
typedef struct {
int16_t radius[2];
int16_t offset[3][3];
} bno055CalibrationData_t;
bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration);
void bno055SerialFetchEulerAngles(void);
void bno055SerialGetCalibStat(void);
bno055CalibrationData_t bno055SerialGetCalibrationData(void);

62
src/main/fc/cli.c
View file

@ -84,7 +84,6 @@ bool cliMode = false;
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "flight/secondary_imu.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
@ -3086,55 +3085,6 @@ static void cliBatch(char *cmdline)
}
#endif
#ifdef USE_SECONDARY_IMU
static void printImu2Status(void)
{
cliPrintLinef("Secondary IMU active: %d", secondaryImuState.active);
cliPrintLine("Calibration status:");
cliPrintLinef("Sys: %d", secondaryImuState.calibrationStatus.sys);
cliPrintLinef("Gyro: %d", secondaryImuState.calibrationStatus.gyr);
cliPrintLinef("Acc: %d", secondaryImuState.calibrationStatus.acc);
cliPrintLinef("Mag: %d", secondaryImuState.calibrationStatus.mag);
cliPrintLine("Calibration gains:");
cliPrintLinef(
"Gyro: %d %d %d",
secondaryImuConfig()->calibrationOffsetGyro[X],
secondaryImuConfig()->calibrationOffsetGyro[Y],
secondaryImuConfig()->calibrationOffsetGyro[Z]
);
cliPrintLinef(
"Acc: %d %d %d",
secondaryImuConfig()->calibrationOffsetAcc[X],
secondaryImuConfig()->calibrationOffsetAcc[Y],
secondaryImuConfig()->calibrationOffsetAcc[Z]
);
cliPrintLinef(
"Mag: %d %d %d",
secondaryImuConfig()->calibrationOffsetMag[X],
secondaryImuConfig()->calibrationOffsetMag[Y],
secondaryImuConfig()->calibrationOffsetMag[Z]
);
cliPrintLinef(
"Radius: %d %d",
secondaryImuConfig()->calibrationRadiusAcc,
secondaryImuConfig()->calibrationRadiusMag
);
}
static void cliImu2(char *cmdline)
{
if (sl_strcasecmp(cmdline, "fetch") == 0) {
secondaryImuFetchCalibration();
printImu2Status();
} else {
printImu2Status();
}
}
#endif
static void cliSave(char *cmdline)
{
UNUSED(cmdline);
@ -3361,19 +3311,14 @@ static void cliStatus(char *cmdline)
cliPrintLinef(" PCLK2 = %d MHz", clocks.PCLK2_Frequency / 1000000);
#endif
cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s, IMU2=%s",
cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s",
hardwareSensorStatusNames[getHwGyroStatus()],
hardwareSensorStatusNames[getHwAccelerometerStatus()],
hardwareSensorStatusNames[getHwCompassStatus()],
hardwareSensorStatusNames[getHwBarometerStatus()],
hardwareSensorStatusNames[getHwRangefinderStatus()],
hardwareSensorStatusNames[getHwOpticalFlowStatus()],
hardwareSensorStatusNames[getHwGPSStatus()],
#ifdef USE_SECONDARY_IMU
hardwareSensorStatusNames[getHwSecondaryImuStatus()]
#else
hardwareSensorStatusNames[0]
#endif
hardwareSensorStatusNames[getHwGPSStatus()]
);
#ifdef USE_ESC_SENSOR
@ -3918,9 +3863,6 @@ const clicmd_t cmdTable[] = {
"[<index>]", cliBatteryProfile),
CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
#ifdef USE_SECONDARY_IMU
CLI_COMMAND_DEF("imu2", "Secondary IMU", NULL, cliImu2),
#endif
#if defined(USE_SAFE_HOME)
CLI_COMMAND_DEF("safehome", "safe home list", NULL, cliSafeHomes),
#endif

1
src/main/fc/fc_core.c
View file

@ -86,7 +86,6 @@ FILE_COMPILE_FOR_SPEED
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/secondary_imu.h"
#include "flight/rate_dynamics.h"
#include "flight/failsafe.h"

4
src/main/fc/fc_init.c
View file

@ -96,7 +96,6 @@
#include "flight/power_limits.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"
#include "flight/secondary_imu.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
@ -677,9 +676,6 @@ void init(void)
latchActiveFeatures();
motorControlEnable = true;
#ifdef USE_SECONDARY_IMU
secondaryImuInit();
#endif
fcTasksInit();
#ifdef USE_OSD

6
src/main/fc/fc_msp.c
View file

@ -76,7 +76,6 @@
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "flight/secondary_imu.h"
#include "config/config_eeprom.h"
#include "config/feature.h"
@ -411,11 +410,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, getHwRangefinderStatus());
sbufWriteU8(dst, getHwPitotmeterStatus());
sbufWriteU8(dst, getHwOpticalFlowStatus());
#ifdef USE_SECONDARY_IMU
sbufWriteU8(dst, getHwSecondaryImuStatus());
#else
sbufWriteU8(dst, 0);
#endif
break;
case MSP_ACTIVEBOXES:

12
src/main/fc/fc_tasks.c
View file

@ -49,7 +49,6 @@
#include "flight/pid.h"
#include "flight/power_limits.h"
#include "flight/rpm_filter.h"
#include "flight/secondary_imu.h"
#include "flight/servos.h"
#include "flight/wind_estimator.h"
@ -398,9 +397,6 @@ void fcTasksInit(void)
#if defined(USE_SMARTPORT_MASTER)
setTaskEnabled(TASK_SMARTPORT_MASTER, true);
#endif
#ifdef USE_SECONDARY_IMU
setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->hardwareType != SECONDARY_IMU_NONE && secondaryImuState.active);
#endif
}
cfTask_t cfTasks[TASK_COUNT] = {
@ -629,14 +625,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef USE_SECONDARY_IMU
[TASK_SECONDARY_IMU] = {
.taskName = "IMU2",
.taskFunc = taskSecondaryImu,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef USE_RPM_FILTER
[TASK_RPM_FILTER] = {
.taskName = "RPM",

97
src/main/fc/settings.yaml
View file

@ -9,9 +9,6 @@ tables:
- name: rangefinder_hardware
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C"]
enum: rangefinderType_e
- name: secondary_imu_hardware
values: ["NONE", "BNO055_SERIAL"]
enum: secondaryImuType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
enum: magSensor_e
@ -93,7 +90,7 @@ tables:
- name: debug_modes
values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE",
"NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
"AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING"]
- name: aux_operator
values: ["OR", "AND"]
@ -483,98 +480,6 @@ groups:
type: uint8_t
table: alignment
- name: PG_SECONDARY_IMU
type: secondaryImuConfig_t
headers: ["flight/secondary_imu.h"]
condition: USE_SECONDARY_IMU
members:
- name: imu2_hardware
description: "Selection of a Secondary IMU hardware type. NONE disables this functionality"
default_value: "NONE"
field: hardwareType
table: secondary_imu_hardware
- name: imu2_use_for_osd_heading
description: "If set to ON, Secondary IMU data will be used for Analog OSD heading"
default_value: OFF
field: useForOsdHeading
type: bool
- name: imu2_use_for_osd_ahi
description: "If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon"
field: useForOsdAHI
default_value: OFF
type: bool
- name: imu2_use_for_stabilized
description: "If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH)"
field: useForStabilized
default_value: OFF
type: bool
- name: imu2_align_roll
description: "Roll alignment for Secondary IMU. 1/10 of a degree"
field: rollDeciDegrees
default_value: 0
min: -1800
max: 3600
- name: imu2_align_pitch
description: "Pitch alignment for Secondary IMU. 1/10 of a degree"
field: pitchDeciDegrees
default_value: 0
min: -1800
max: 3600
- name: imu2_align_yaw
description: "Yaw alignment for Secondary IMU. 1/10 of a degree"
field: yawDeciDegrees
default_value: 0
min: -1800
max: 3600
- name: imu2_gain_acc_x
description: "Secondary IMU ACC calibration data"
field: calibrationOffsetAcc[X]
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: imu2_gain_acc_y
field: calibrationOffsetAcc[Y]
description: "Secondary IMU ACC calibration data"
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: imu2_gain_acc_z
field: calibrationOffsetAcc[Z]
description: "Secondary IMU ACC calibration data"
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: imu2_gain_mag_x
field: calibrationOffsetMag[X]
description: "Secondary IMU MAG calibration data"
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: imu2_gain_mag_y
field: calibrationOffsetMag[Y]
description: "Secondary IMU MAG calibration data"
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: imu2_gain_mag_z
field: calibrationOffsetMag[Z]
description: "Secondary IMU MAG calibration data"
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: imu2_radius_acc
field: calibrationRadiusAcc
description: "Secondary IMU MAG calibration data"
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: imu2_radius_mag
field: calibrationRadiusMag
description: "Secondary IMU MAG calibration data"
default_value: 0
min: INT16_MIN
max: INT16_MAX
- name: PG_COMPASS_CONFIG
type: compassConfig_t
headers: ["sensors/compass.h"]

17
src/main/flight/pid.c
View file

@ -46,7 +46,6 @@ FILE_COMPILE_FOR_SPEED
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/rpm_filter.h"
#include "flight/secondary_imu.h"
#include "flight/kalman.h"
#include "flight/smith_predictor.h"
@ -624,23 +623,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) {
static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT)
{
#ifdef USE_SECONDARY_IMU
float actual;
if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) {
if (axis == FD_ROLL) {
actual = secondaryImuState.eulerAngles.values.roll;
} else {
actual = secondaryImuState.eulerAngles.values.pitch;
}
} else {
actual = attitude.raw[axis];
}
float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual);
#else
float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
#endif
// Soaring mode deadband inactive if pitch/roll stick not centered to allow RC stick adjustment
if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {

241
src/main/flight/secondary_imu.c
View file

@ -1,241 +0,0 @@
/*
* 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_SECONDARY_IMU
#include "stdint.h"
#include "build/debug.h"
#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_serial.h"
#include "fc/settings.h"
#include "flight/secondary_imu.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 2);
EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
{
instance->hardwareType = SECONDARY_IMU_NONE,
instance->rollDeciDegrees = 0;
instance->pitchDeciDegrees = 0;
instance->yawDeciDegrees = 0;
instance->useForOsdHeading = 0;
instance->useForOsdAHI = 0;
instance->useForStabilized = 0;
for (uint8_t i = 0; i < 3; i++)
{
instance->calibrationOffsetGyro[i] = 0;
instance->calibrationOffsetMag[i] = 0;
instance->calibrationOffsetAcc[i] = 0;
}
instance->calibrationRadiusAcc = 0;
instance->calibrationRadiusMag = 0;
}
void secondaryImuInit(void)
{
secondaryImuState.active = false;
// Create magnetic declination matrix
#ifdef USE_MAG
const int deg = compassConfig()->mag_declination / 100;
const int min = compassConfig()->mag_declination % 100;
#else
const int deg = 0;
const int min = 0;
#endif
secondaryImuSetMagneticDeclination(deg + min / 60.0f);
bno055CalibrationData_t calibrationData;
calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z];
calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X];
calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y];
calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z];
calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X];
calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y];
calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z];
calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
requestedSensors[SENSOR_INDEX_IMU2] = secondaryImuConfig()->hardwareType;
if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
if (secondaryImuState.active) {
detectedSensors[SENSOR_INDEX_IMU2] = SECONDARY_IMU_BNO055_SERIAL;
rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50));
}
}
if (!secondaryImuState.active) {
detectedSensors[SENSOR_INDEX_IMU2] = SECONDARY_IMU_NONE;
}
}
void secondaryImuProcess(void) {
//Apply mag declination
secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
//TODO this way of rotating a vector makes no sense, something simpler have to be developed
const fpVector3_t v = {
.x = secondaryImuState.eulerAngles.raw[0],
.y = secondaryImuState.eulerAngles.raw[1],
.z = secondaryImuState.eulerAngles.raw[2],
};
fpVector3_t rotated;
fp_angles_t imuAngles = {
.angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees),
.angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees),
.angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees),
};
fpMat3_t rotationMatrix;
rotationMatrixFromAngles(&rotationMatrix, &imuAngles);
rotationMatrixRotateVector(&rotated, &v, &rotationMatrix);
rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600;
secondaryImuState.eulerAngles.values.roll = rotated.x;
secondaryImuState.eulerAngles.values.pitch = rotated.y;
secondaryImuState.eulerAngles.values.yaw = rotated.z;
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);
DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag);
DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
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_SERIAL) {
/*
* Every 2 seconds fetch current calibration state
*/
if (tick == 100)
{
bno055SerialGetCalibStat();
tick = 0;
} else {
bno055SerialFetchEulerAngles();
}
}
}
void secondaryImuFetchCalibration(void) {
bno055CalibrationData_t calibrationData;
if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
calibrationData = bno055SerialGetCalibrationData();
} else {
return;
}
secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X];
secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y];
secondaryImuConfigMutable()->calibrationOffsetAcc[Z] = calibrationData.offset[ACC][Z];
secondaryImuConfigMutable()->calibrationOffsetMag[X] = calibrationData.offset[MAG][X];
secondaryImuConfigMutable()->calibrationOffsetMag[Y] = calibrationData.offset[MAG][Y];
secondaryImuConfigMutable()->calibrationOffsetMag[Z] = calibrationData.offset[MAG][Z];
secondaryImuConfigMutable()->calibrationOffsetGyro[X] = calibrationData.offset[GYRO][X];
secondaryImuConfigMutable()->calibrationOffsetGyro[Y] = calibrationData.offset[GYRO][Y];
secondaryImuConfigMutable()->calibrationOffsetGyro[Z] = calibrationData.offset[GYRO][Z];
secondaryImuConfigMutable()->calibrationRadiusAcc = calibrationData.radius[ACC];
secondaryImuConfigMutable()->calibrationRadiusMag = calibrationData.radius[MAG];
}
void secondaryImuSetMagneticDeclination(float declination) { //Incoming units are degrees
secondaryImuState.magDeclination = declination * 10.0f; //Internally declination is stored in decidegrees
}
bool isSecondaryImuHealthy(void) {
return secondaryImuState.active;
}
hardwareSensorStatus_e getHwSecondaryImuStatus(void)
{
#ifdef USE_SECONDARY_IMU
if (detectedSensors[SENSOR_INDEX_IMU2] != SECONDARY_IMU_NONE) {
if (isSecondaryImuHealthy()) {
return HW_SENSOR_OK;
}
else {
return HW_SENSOR_UNHEALTHY;
}
}
else {
if (requestedSensors[SENSOR_INDEX_IMU2] != SECONDARY_IMU_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
}
#else
return HW_SENSOR_NONE;
#endif
}
#endif

70
src/main/flight/secondary_imu.h
View file

@ -1,70 +0,0 @@
/*
* 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 "config/parameter_group.h"
#include "common/time.h"
#include "sensors/sensors.h"
#include "drivers/accgyro/accgyro_bno055_serial.h"
#include "sensors/diagnostics.h"
typedef enum {
SECONDARY_IMU_NONE = 0,
SECONDARY_IMU_BNO055_SERIAL = 1,
} secondaryImuType_e;
typedef struct secondaryImuConfig_s {
uint8_t hardwareType;
int16_t rollDeciDegrees;
int16_t pitchDeciDegrees;
int16_t yawDeciDegrees;
uint8_t useForOsdHeading;
uint8_t useForOsdAHI;
uint8_t useForStabilized;
int16_t calibrationOffsetGyro[3];
int16_t calibrationOffsetMag[3];
int16_t calibrationOffsetAcc[3];
int16_t calibrationRadiusAcc;
int16_t calibrationRadiusMag;
} secondaryImuConfig_t;
typedef struct secondaryImuState_s {
flightDynamicsTrims_t eulerAngles;
bno055CalibStat_t calibrationStatus;
uint8_t active;
float magDeclination;
} secondaryImuState_t;
extern secondaryImuState_t secondaryImuState;
PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
void secondaryImuProcess(void);
void secondaryImuInit(void);
void taskSecondaryImu(timeUs_t currentTimeUs);
void secondaryImuFetchCalibration(void);
void secondaryImuSetMagneticDeclination(float declination);
bool isSecondaryImuHealthy(void);
hardwareSensorStatus_e getHwSecondaryImuStatus(void);

33
src/main/io/osd.c
View file

@ -87,7 +87,6 @@ FILE_COMPILE_FOR_SPEED
#include "flight/pid.h"
#include "flight/power_limits.h"
#include "flight/rth_estimator.h"
#include "flight/secondary_imu.h"
#include "flight/servos.h"
#include "flight/wind_estimator.h"
@ -1117,28 +1116,12 @@ uint16_t osdGetRemainingGlideTime(void) {
static bool osdIsHeadingValid(void)
{
#ifdef USE_SECONDARY_IMU
if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
return true;
} else {
return isImuHeadingValid();
}
#else
return isImuHeadingValid();
#endif
}
int16_t osdGetHeading(void)
{
#ifdef USE_SECONDARY_IMU
if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
return secondaryImuState.eulerAngles.values.yaw;
} else {
return attitude.values.yaw;
}
#else
return attitude.values.yaw;
#endif
}
int16_t osdPanServoHomeDirectionOffset(void)
@ -2265,21 +2248,9 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_ARTIFICIAL_HORIZON:
{
float rollAngle;
float pitchAngle;
float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
#ifdef USE_SECONDARY_IMU
if (secondaryImuState.active && secondaryImuConfig()->useForOsdAHI) {
rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll);
pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch);
} else {
rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
}
#else
rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
#endif
pitchAngle -= osdConfig()->ahi_camera_uptilt_comp ? DEGREES_TO_RADIANS(osdConfig()->camera_uptilt) : 0;
pitchAngle += DEGREES_TO_RADIANS(getFixedWingLevelTrim());
if (osdConfig()->ahi_reverse_roll) {

2
src/main/io/serial.h
View file

@ -55,7 +55,7 @@ typedef enum {
FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152
FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304
FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
FUNCTION_IMU2 = (1 << 24), // 16777216
FUNCTION_UNUSED_2 = (1 << 24), // 16777216
FUNCTION_MSP_OSD = (1 << 25), // 33554432
} serialPortFunction_e;

4
src/main/navigation/navigation_pos_estimator.c
View file

@ -38,7 +38,6 @@
#include "fc/settings.h"
#include "flight/imu.h"
#include "flight/secondary_imu.h"
#include "io/gps.h"
@ -217,9 +216,6 @@ void onNewGPSData(void)
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
const float declination = geoCalculateMagDeclination(&newLLH);
imuSetMagneticDeclination(declination);
#ifdef USE_SECONDARY_IMU
secondaryImuSetMagneticDeclination(declination);
#endif
magDeclinationSet = true;
}
}

3
src/main/scheduler/scheduler.h
View file

@ -118,9 +118,6 @@ typedef enum {
#endif
#ifdef USE_IRLOCK
TASK_IRLOCK,
#endif
#ifdef USE_SECONDARY_IMU
TASK_SECONDARY_IMU,
#endif
/* Count of real tasks */
TASK_COUNT,

9
src/main/sensors/diagnostics.c
View file

@ -24,7 +24,6 @@
#include "sensors/rangefinder.h"
#include "sensors/pitotmeter.h"
#include "sensors/opflow.h"
#include "flight/secondary_imu.h"
extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
@ -232,11 +231,6 @@ bool isHardwareHealthy(void)
const hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus();
const hardwareSensorStatus_e gpsStatus = getHwGPSStatus();
const hardwareSensorStatus_e opflowStatus = getHwOpticalFlowStatus();
#ifdef USE_SECONDARY_IMU
const hardwareSensorStatus_e imu2Status = getHwSecondaryImuStatus();
#else
const hardwareSensorStatus_e imu2Status = HW_SENSOR_NONE;
#endif
// Sensor is considered failing if it's either unavailable (selected but not detected) or unhealthy (returning invalid readings)
if (gyroStatus == HW_SENSOR_UNAVAILABLE || gyroStatus == HW_SENSOR_UNHEALTHY)
@ -263,8 +257,5 @@ bool isHardwareHealthy(void)
if (opflowStatus == HW_SENSOR_UNAVAILABLE || opflowStatus == HW_SENSOR_UNHEALTHY)
return false;
if (imu2Status == HW_SENSOR_UNAVAILABLE || opflowStatus == HW_SENSOR_UNHEALTHY)
return false;
return true;
}

5
src/main/sensors/initialisation.c
View file

@ -39,10 +39,9 @@
#include "sensors/sensors.h"
#include "sensors/temperature.h"
#include "sensors/temperature.h"
#include "flight/secondary_imu.h"
uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE, SECONDARY_IMU_NONE };
uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE, SECONDARY_IMU_NONE };
uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
bool sensorsAutodetect(void)
{

1
src/main/sensors/sensors.h
View file

@ -25,7 +25,6 @@ typedef enum {
SENSOR_INDEX_RANGEFINDER,
SENSOR_INDEX_PITOT,
SENSOR_INDEX_OPFLOW,
SENSOR_INDEX_IMU2,
SENSOR_INDEX_COUNT
} sensorIndex_e;

1
src/main/target/HAKRCF411D/target.h
View file

@ -116,7 +116,6 @@
#define USE_BARO_DPS310
#define TEMPERATURE_I2C_BUS BUS_I2C1
#define BNO055_I2C_BUS BUS_I2C1
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1

2
src/main/target/IFLIGHT_BLITZ_F7_PRO/target.h
View file

@ -148,5 +148,3 @@
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_ESC_SENSOR
#define BNO055_I2C_BUS BUS_I2C2

2
src/main/target/IFLIGHT_JBF7PRO/target.h
View file

@ -159,5 +159,3 @@
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_ESC_SENSOR
#define BNO055_I2C_BUS BUS_I2C1

1
src/main/target/SPEEDYBEEF405V3/target.h
View file

@ -152,7 +152,6 @@
#define USE_OPFLOW_MSP
#define TEMPERATURE_I2C_BUS BUS_I2C2
#define BNO055_I2C_BUS BUS_I2C2
#define PITOT_I2C_BUS BUS_I2C2
#define RANGEFINDER_I2C_BUS BUS_I2C2

3
src/main/target/common.h
View file

@ -145,9 +145,6 @@
#define USE_SERIALRX_GHST
#define USE_TELEMETRY_GHST
#define USE_SECONDARY_IMU
#define USE_IMU_BNO055
#define USE_POWER_LIMITS
#define NAV_FIXED_WING_LANDING