feat(driver): autodetect IMU, add support for SC7U22 (#7191)

Co-authored-by: raphaelcoeffic <1050031+raphaelcoeffic@users.noreply.github.com>
Co-authored-by: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
3djc
2026-03-25 16:19:03 +10:00
committed by GitHub
co-authored by raphaelcoeffic Claude Opus 4.6
parent 5ea531d391
commit 0ce5c8f3f2
59 changed files with 867 additions and 865 deletions
+1
View File
@@ -47,6 +47,7 @@
#include "sdcard.h" #include "sdcard.h"
#include "debug.h" #include "debug.h"
#include "keys.h" #include "keys.h"
#include "gyro.h"
#include "flysky_gimbal_driver.h" #include "flysky_gimbal_driver.h"
#include "timers_driver.h" #include "timers_driver.h"
+27
View File
@@ -49,12 +49,21 @@
#include "sdcard.h" #include "sdcard.h"
#include "debug.h" #include "debug.h"
#include "keys.h" #include "keys.h"
#include "gyro.h"
#include "flysky_gimbal_driver.h" #include "flysky_gimbal_driver.h"
#include "timers_driver.h" #include "timers_driver.h"
#include "touch_driver.h" #include "touch_driver.h"
#if defined(IMU_ICM4207C)
#include "drivers/icm42607C.h"
#endif
#if defined(IMU_SC7U22)
#include "drivers/sc7u22.h"
#endif
#include <string.h> #include <string.h>
// common ADC driver // common ADC driver
@@ -63,6 +72,22 @@ extern const etx_hal_adc_driver_t _adc_driver;
// RGB LED timer // RGB LED timer
extern const stm32_pulse_timer_t _led_timer; extern const stm32_pulse_timer_t _led_timer;
static const etx_imu_t _imu_candidates[] = {
#if defined(IMU_ICM4207C)
{ &imu_icm42607_driver, IMU_I2C_BUS, ICM426xx_I2C_BASE_ADDR },
{ &imu_icm42607_driver, IMU_I2C_BUS, ICM426xx_I2C_BASE_ADDR + 1 },
#endif
#if defined(IMU_SC7U22)
{ &imu_sc7u22_driver, IMU_I2C_BUS, SC7U22_I2C_BASE_ADDR },
{ &imu_sc7u22_driver, IMU_I2C_BUS, SC7U22_I2C_BASE_ADDR + 1 },
#endif
};
static void gyroInit()
{
gyroStart(imuDetect(_imu_candidates, DIM(_imu_candidates)));
}
static void led_strip_off() static void led_strip_off()
{ {
for (uint8_t i = 0; i < LED_STRIP_LENGTH; i++) { for (uint8_t i = 0; i < LED_STRIP_LENGTH; i++) {
@@ -178,6 +203,8 @@ void boardInit()
// RTC must be initialized before rambackupRestore() is called // RTC must be initialized before rambackupRestore() is called
rtcInit(); rtcInit();
gyroInit();
} }
extern void rtcDisableBackupReg(); extern void rtcDisableBackupReg();
+53 -49
View File
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) EdgeTX * Copyright (C) EdgeTX
* *
* Based on code named * Based on code named
* opentx - https://github.com/opentx/opentx * opentx - https://github.com/opentx/opentx
@@ -24,33 +24,36 @@
with code for generic ICM4267XX. All register have different addresses with code for generic ICM4267XX. All register have different addresses
*/ */
#include "edgetx.h"
#include "hal/i2c_driver.h" #include "hal/i2c_driver.h"
#include "stm32_i2c_driver.h" #include "hal/imu.h"
#include "hal/gpio.h"
#include "delays_driver.h"
#include "icm42607C.h" #include "icm42607C.h"
#include "imu_filter.h"
#include "stm32_gpio.h"
#include "inactivity_timer.h" #include "inactivity_timer.h"
#include "hal.h" #include "debug.h"
constexpr uint32_t I2C_TIMEOUT = 5; //ms static etx_i2c_bus_t s_i2c_bus;
static uint16_t s_i2c_addr;
int16_t getGyroTemperature() static int16_t __attribute__((unused)) get42607Temperature()
{ {
uint8_t reg = TEMP_DATA_X0_REG;
uint8_t buf[2] = {0}; uint8_t buf[2] = {0};
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, buf, 2, I2C_TIMEOUT) < 0) { if (i2c_read(s_i2c_bus, s_i2c_addr, TEMP_DATA_X0_REG, 1, buf, 2) < 0) {
TRACE("ICM426xx ERROR: TEMP_DATA_X0_REG i2c read error"); TRACE("ICM426xx ERROR: TEMP_DATA_X0_REG i2c read error");
return -1; return -1;
} }
return (((buf[0] << 8) | buf[1]) / 128) + 25; int16_t raw = (int16_t)((buf[0] << 8) | buf[1]);
return (int16_t)(raw / 128 + 25);
} }
int write_cmd(uint8_t reg, uint8_t val) static int write_cmd(uint8_t reg, uint8_t val)
{ {
return i2c_write(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, &val, 1); return i2c_write(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1);
} }
#if defined(IMU_INT_GPIO) #if defined(IMU_INT_GPIO)
@@ -60,30 +63,35 @@ static void imu_exti_isr(void)
} }
#endif #endif
#if defined(IMU_INT_GPIO)
static int write_mreg1(uint8_t reg, uint8_t val) { static int write_mreg1(uint8_t reg, uint8_t val) {
if (write_cmd(BLK_SEL_W_REG, 0x00) < 0) return -1; if (write_cmd(BLK_SEL_W_REG, 0x00) < 0) return -1;
if (write_cmd(MADDR_W_REG, reg) < 0) return -1; if (write_cmd(MADDR_W_REG, reg) < 0) return -1;
if (write_cmd(M_W_REG, val) < 0) return -1; if (write_cmd(M_W_REG, val) < 0) return -1;
delay_us(10); //42627-pdf p12“Accessing MREG1, MREG2 And MREG3 Registers delay_us(10); // small delay as recommended by datasheet
return 0; return 0;
} }
#endif
int gyroInit(void) static int gyro42607Init(etx_i2c_bus_t bus, uint16_t addr)
{ {
TRACE("ICM426xx I2C Init"); s_i2c_bus = bus;
s_i2c_addr = addr;
if (i2c_init(IMU_I2C_BUS) < 0) { TRACE("ICM426xx I2C Init at address 0x%x", addr);
if (i2c_init(s_i2c_bus) < 0) {
TRACE("ICM426xx ERROR: i2c_init bus error"); TRACE("ICM426xx ERROR: i2c_init bus error");
return -1; return -1;
} }
if (i2c_dev_ready(IMU_I2C_BUS, ICM426xx_I2C_ADDR) < 0) { if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) {
TRACE("ICM426xx device init error"); TRACE("ICM426xx device init error");
return -1; return -1;
} }
uint8_t data = 0; uint8_t data = 0;
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, 0x75, 1, &data, 1, I2C_TIMEOUT) < 0) { if (i2c_read(s_i2c_bus, s_i2c_addr, WHO_AM_I_REG, 1, &data, 1) < 0) {
TRACE("ICM426xx ERROR: i2c read error"); TRACE("ICM426xx ERROR: i2c read error");
return -1; return -1;
} }
@@ -105,14 +113,14 @@ int gyroInit(void)
} }
delay_ms(10); delay_ms(10);
// Set gyro to 2000 dps, 1kHz ODR // Configure gyro: 2000 dps, 1 kHz ODR
if(write_cmd(GYRO_CONFIG0_REG, GYRO_ODR_1KHZ) < 0) { if(write_cmd(GYRO_CONFIG0_REG, GYRO_ODR_1KHZ) < 0) {
TRACE("ICM426xx ERROR: GYRO_CONFIG0_REG error"); TRACE("ICM426xx ERROR: GYRO_CONFIG0_REG error");
return -1; return -1;
} }
delay_ms(1); delay_ms(1);
// Set accel to ±4g, 1kHz ODR // Configure accelerometer: ±4g, 1 kHz ODR
if (write_cmd(ACCEL_CONFIG0_REG, ACCEL_ODR_1KHZ) < 0) { if (write_cmd(ACCEL_CONFIG0_REG, ACCEL_ODR_1KHZ) < 0) {
TRACE("ICM426xx ERROR: ACCEL_CONFIG0_REG error"); TRACE("ICM426xx ERROR: ACCEL_CONFIG0_REG error");
return -1; return -1;
@@ -127,58 +135,54 @@ int gyroInit(void)
delay_ms(1); delay_ms(1);
#if defined(IMU_INT_GPIO) #if defined(IMU_INT_GPIO)
// CCEL_WOM_X_THR:0-255(0-1g),0x10--16mg // Configure Wake-on-Motion thresholds via MREG1 interface
// Values chosen to approximate activity detection behaviour used elsewhere
if (write_mreg1(ACCEL_WOM_X_THR_REG, 0xFE) < 0) return -1; if (write_mreg1(ACCEL_WOM_X_THR_REG, 0xFE) < 0) return -1;
if (write_mreg1(ACCEL_WOM_Y_THR_REG, 0xFE) < 0) return -1; if (write_mreg1(ACCEL_WOM_Y_THR_REG, 0xFE) < 0) return -1;
if (write_mreg1(ACCEL_WOM_Z_THR_REG, 0xFE) < 0) return -1; if (write_mreg1(ACCEL_WOM_Z_THR_REG, 0xFE) < 0) return -1;
delay_ms(1); delay_ms(1);
// config WoM // Configure WoM mode (OR across axes)
if (write_cmd(WOM_CONFIG_REG, 0x03) < 0) return -1; // OR mode if (write_cmd(WOM_CONFIG_REG, 0x03) < 0) return -1; // OR mode
delay_ms(1); delay_ms(1);
// enable INT1 // Enable INT1 source bits for XYZ
if (write_cmd(INT_SOURCE1_REG, 0x07) < 0) return -1; // XYZ interrupt if (write_cmd(INT_SOURCE1_REG, 0x07) < 0) return -1; // XYZ interrupt
delay_ms(1); delay_ms(1);
// config INT1 pin // Ensure INT1 is configured to default behaviour
if (write_cmd(INT_CONFIG_REG, 0x00) < 0) return -1; if (write_cmd(INT_CONFIG_REG, 0x00) < 0) return -1;
gpio_init_int(IMU_INT_GPIO, GPIO_IN_PU, GPIO_FALLING, imu_exti_isr); gpio_init_int(IMU_INT_GPIO, GPIO_IN_PU, GPIO_FALLING, imu_exti_isr);
#endif #endif
TRACE("ICM426xx succeeded");
return 0; return 0;
} }
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH]) static int gyro42607Read(etx_imu_data_t* data)
{ {
IMU_RawData_t imu_raw;
IMU_FilteredData_t *filtered;
uint8_t buf[6]; uint8_t buf[6];
uint8_t reg = GYRO_DATA_X0_REG;
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, buf, 6, I2C_TIMEOUT) < 0) { if (i2c_read(s_i2c_bus, s_i2c_addr, GYRO_DATA_X0_REG, 1, buf, 6) < 0) {
TRACE("ICM426xx ERROR: i2c read error"); TRACE("ICM426xx ERROR: gyro read error");
return -1; return -1;
} }
imu_raw.gyro_x = -((int16_t)(buf[0] << 8) | buf[1]); // x data->gyro_x = -((int16_t)(buf[0] << 8) | buf[1]);
imu_raw.gyro_y = -((int16_t)(buf[2] << 8) | buf[3]); // y data->gyro_y = -((int16_t)(buf[2] << 8) | buf[3]);
imu_raw.gyro_z = ((int16_t)(buf[4] << 8) | buf[5]); // z data->gyro_z = ((int16_t)(buf[4] << 8) | buf[5]);
reg = ACCEL_DATA_X0_REG; if (i2c_read(s_i2c_bus, s_i2c_addr, ACCEL_DATA_X0_REG, 1, buf, 6) < 0) {
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, buf, 6, I2C_TIMEOUT) < 0) { TRACE("ICM426xx ERROR: accel read error");
TRACE("ICM426xx ERROR: i2c read error");
return -1; return -1;
} }
imu_raw.accel_x = ((int16_t)(buf[0] << 8) | buf[1]); // x data->accel_x = ((int16_t)(buf[0] << 8) | buf[1]);
imu_raw.accel_y = ((int16_t)(buf[2] << 8) | buf[3]); // y data->accel_y = ((int16_t)(buf[2] << 8) | buf[3]);
imu_raw.accel_z = ((int16_t)(buf[4] << 8) | buf[5]); // z data->accel_z = ((int16_t)(buf[4] << 8) | buf[5]);
// Process through filters
process_imu_data(&imu_raw);
filtered = get_filtered_imu_data();
*(int16_t*)&buffer[6] = -(int16_t)filtered->accel_x; // x
*(int16_t*)&buffer[8] = -(int16_t)filtered->accel_y; // y
*(int16_t*)&buffer[10] = (int16_t)filtered->accel_z; // z
return 0; return 0;
} }
const etx_imu_driver_t imu_icm42607_driver = {
gyro42607Init,
gyro42607Read,
"ICM42607",
};
+9 -15
View File
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) EdgeTX * Copyright (C) EdgeTX
* *
* Based on code named * Based on code named
* opentx - https://github.com/opentx/opentx * opentx - https://github.com/opentx/opentx
@@ -20,13 +20,18 @@
*/ */
/* /*
WARNING: while this ICM42607C belong to the ICM426xx familly, it is NOT compatible WARNING: While this ICM42607C belong to the ICM426xx familly, it is NOT
with code for generic ICM4267XX. All register have different addresses compatible with code for generic ICM4267XX. All registers have
different addresses.
*/ */
#pragma once #pragma once
#define ICM426xx_I2C_ADDR 0x68 #include "hal/imu.h"
#define ICM426xx_I2C_BASE_ADDR 0x68
extern const etx_imu_driver_t imu_icm42607_driver;
// Register addresses // Register addresses
#define WHO_AM_I_REG 0x75 #define WHO_AM_I_REG 0x75
@@ -57,14 +62,3 @@
#define INT_SOURCE1_REG 0x2C #define INT_SOURCE1_REG 0x2C
#define INT_CONFIG_REG 0x06 #define INT_CONFIG_REG 0x06
#define INT_STATUS2_REG 0x3B #define INT_STATUS2_REG 0x3B
typedef struct {
float fTemperatureDegC; //°C
float fAccX, fAccY, fAccZ; // m/s^2
float fGyroXradps, fGyroYradps, fGyroZradps; // rad/s
} sIMUoutput;
extern sIMUoutput IMUoutput;
int gyroInit(void);
int gyroRead(unsigned char*);
-180
View File
@@ -1,180 +0,0 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program 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.
*/
#include "edgetx.h"
#include "imu_filter.h"
#include <math.h>
#include <string.h>
// Filter parameters
#define COMPLEMENTARY_ALPHA 0.92f // Complementary filter coefficient
#define LP_FILTER_ALPHA 0.9f // Low-pass filter coefficient
#define SAMPLE_TIME_S 0.005f // Gyro is read every 5ms as part of execMixerFrequentActions()
#define RAD_TO_DEG 57.2958f
#define DEG_TO_RAD 0.0174533f
// Filter state variables
static IMU_FilteredData_t filtered_data = {0};
static float prev_accel[3] = {0};
static float prev_gyro[3] = {0};
static uint8_t filter_initialized = 0;
// Low-pass filter implementation
float low_pass_filter(float new_value, float prev_value, float alpha) {
return alpha * prev_value + (1.0f - alpha) * new_value;
}
// Calculate roll and pitch from accelerometer data
void calculate_accel_angles(float ax, float ay, float az, float *roll, float *pitch) {
// Normalize accelerometer data
float norm = sqrtf(ax*ax + ay*ay + az*az);
if (norm == 0.0f) return;
ax /= norm;
ay /= norm;
az /= norm;
// Calculate angles
*roll = atan2f(ay, sqrtf(ax*ax + az*az));
*pitch = atan2f(-ax, sqrtf(ay*ay + az*az));
}
// Complementary filter for attitude estimation
void complementary_filter(IMU_RawData_t *raw_data, float dt) {
float accel_roll, accel_pitch;
// Calculate angles from accelerometer
calculate_accel_angles(raw_data->accel_x, raw_data->accel_y, raw_data->accel_z,
&accel_roll, &accel_pitch);
if (!filter_initialized) {
// Initialize filter with accelerometer angles
filtered_data.roll = accel_roll;
filtered_data.pitch = accel_pitch;
filtered_data.yaw = 0.0f;
filter_initialized = 1;
} else {
// Integrate gyroscope data
float gyro_roll = filtered_data.roll + raw_data->gyro_x * dt;
float gyro_pitch = filtered_data.pitch + raw_data->gyro_y * dt;
float gyro_yaw = filtered_data.yaw + raw_data->gyro_z * dt;
// Apply complementary filter
filtered_data.roll = COMPLEMENTARY_ALPHA * gyro_roll +
(1.0f - COMPLEMENTARY_ALPHA) * accel_roll;
filtered_data.pitch = COMPLEMENTARY_ALPHA * gyro_pitch +
(1.0f - COMPLEMENTARY_ALPHA) * accel_pitch;
filtered_data.yaw = gyro_yaw; // No magnetometer correction in this example
// Keep yaw within -π to π range
while (filtered_data.yaw > M_PI) filtered_data.yaw -= 2.0f * M_PI;
while (filtered_data.yaw < -M_PI) filtered_data.yaw += 2.0f * M_PI;
}
}
// Apply low-pass filter to raw sensor data
void apply_lowpass_filter(IMU_RawData_t *raw_data) {
// Filter accelerometer data
filtered_data.accel_x = low_pass_filter(raw_data->accel_x, prev_accel[0], LP_FILTER_ALPHA);
filtered_data.accel_y = low_pass_filter(raw_data->accel_y, prev_accel[1], LP_FILTER_ALPHA);
filtered_data.accel_z = low_pass_filter(raw_data->accel_z, prev_accel[2], LP_FILTER_ALPHA);
// Filter gyroscope data
filtered_data.gyro_x = low_pass_filter(raw_data->gyro_x, prev_gyro[0], LP_FILTER_ALPHA);
filtered_data.gyro_y = low_pass_filter(raw_data->gyro_y, prev_gyro[1], LP_FILTER_ALPHA);
filtered_data.gyro_z = low_pass_filter(raw_data->gyro_z, prev_gyro[2], LP_FILTER_ALPHA);
// Update previous values
prev_accel[0] = filtered_data.accel_x;
prev_accel[1] = filtered_data.accel_y;
prev_accel[2] = filtered_data.accel_z;
prev_gyro[0] = filtered_data.gyro_x;
prev_gyro[1] = filtered_data.gyro_y;
prev_gyro[2] = filtered_data.gyro_z;
}
// Main IMU filter processing function
void process_imu_data(IMU_RawData_t *raw_data) {
// Apply low-pass filtering to raw data
apply_lowpass_filter(raw_data);
// Apply complementary filter for attitude estimation
complementary_filter(raw_data, SAMPLE_TIME_S);
}
// Get filtered IMU data
IMU_FilteredData_t* get_filtered_imu_data(void) {
return &filtered_data;
}
// Reset filter state
void reset_imu_filter(void) {
memset(&filtered_data, 0, sizeof(filtered_data));
memset(prev_accel, 0, sizeof(prev_accel));
memset(prev_gyro, 0, sizeof(prev_gyro));
filter_initialized = 0;
}
// Advanced Kalman filter structure (optional implementation)
typedef struct {
float Q_angle; // Process noise variance for accelerometer
float Q_bias; // Process noise variance for gyro bias
float R_measure; // Measurement noise variance
float angle; // Estimated angle
float bias; // Estimated bias
float rate; // Unbiased rate
float P[2][2]; // Error covariance matrix
} kalman_state_t;
// Kalman filter implementation (more advanced option)
float kalman_filter(kalman_state_t *state, float new_angle, float new_rate, float dt) {
// Prediction step
state->rate = new_rate - state->bias;
state->angle += dt * state->rate;
// Update error covariance
state->P[0][0] += dt * (dt * state->P[1][1] - state->P[0][1] - state->P[1][0] + state->Q_angle);
state->P[0][1] -= dt * state->P[1][1];
state->P[1][0] -= dt * state->P[1][1];
state->P[1][1] += state->Q_bias * dt;
// Innovation
float S = state->P[0][0] + state->R_measure;
float K[2] = {state->P[0][0] / S, state->P[1][0] / S};
float y = new_angle - state->angle;
// Update step
state->angle += K[0] * y;
state->bias += K[1] * y;
// Update error covariance
float P00_temp = state->P[0][0];
float P01_temp = state->P[0][1];
state->P[0][0] -= K[0] * P00_temp;
state->P[0][1] -= K[0] * P01_temp;
state->P[1][0] -= K[1] * P00_temp;
state->P[1][1] -= K[1] * P01_temp;
return state->angle;
}
@@ -19,18 +19,18 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "gyro.h" #include "lsm6ds.h"
#include "hal.h"
#if defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS)
#include "hal/i2c_driver.h" #include "hal/i2c_driver.h"
#include "stm32_i2c_driver.h"
#include "delays_driver.h" #include "delays_driver.h"
#include <string.h> #include "definitions.h"
#include <stdint.h> #include <stdint.h>
static etx_i2c_bus_t s_i2c_bus;
static uint16_t s_i2c_addr;
/* COMMON VALUES FOR ACCEL-GYRO SENSORS */ /* COMMON VALUES FOR ACCEL-GYRO SENSORS */
#define LSM6DS_WHO_AM_I 0x0f #define LSM6DS_WHO_AM_I 0x0f
#define LSM6DS_WHO_AM_I_DEF 0x69 #define LSM6DS_WHO_AM_I_DEF 0x69
@@ -204,72 +204,64 @@ static const char configure[][2] = {
{LSM6DS_BDU_ADDR, 0x04} {LSM6DS_BDU_ADDR, 0x04}
}; };
#define I2C_TIMEOUT_MAX 10000 static int write_reg(uint8_t reg, uint8_t val)
#define I2C_LSM6DS_MAX_WRITE 32
static int I2C_LSM6DS_WriteRegister(uint8_t reg, uint8_t* data, uint8_t len)
{ {
uint8_t buffer[I2C_LSM6DS_MAX_WRITE]; return i2c_write(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1);
if (len >= I2C_LSM6DS_MAX_WRITE - 1) return -1;
buffer[0] = reg;
memcpy(buffer + 1, data, len);
return stm32_i2c_master_tx(IMU_I2C_BUS, IMU_I2C_ADDRESS, buffer, len + 1, 100);
} }
static int I2C_LSM6DS_WriteRegister(uint8_t reg, uint8_t value) static int read_reg(uint8_t reg)
{ {
return I2C_LSM6DS_WriteRegister(reg, &value, 1); uint8_t val = 0;
if (i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1) < 0)
return -1;
return val;
} }
static int I2C_LSM6DS_ReadRegister(uint8_t reg, uint8_t* data, uint8_t len) static int read_regs(uint8_t reg, uint8_t* data, uint8_t len)
{ {
if (stm32_i2c_master_tx(IMU_I2C_BUS, IMU_I2C_ADDRESS, &reg, 1, 10) < 0) return i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, data, len);
}
static int lsm6dsRead(etx_imu_data_t* data)
{
// LSM6DS registers are contiguous: gyro X/Y/Z then accel X/Y/Z (little-endian)
uint8_t buf[12];
if (read_regs(LSM6DS_GYRO_OUT_X_L_ADDR, buf, sizeof(buf)) < 0)
return -1; return -1;
uint8_t value = 0; int16_t* raw = (int16_t*)buf;
if (stm32_i2c_master_rx(IMU_I2C_BUS, IMU_I2C_ADDRESS, data, len, 100) < 0) data->gyro_x = raw[0];
return -1; data->gyro_y = raw[1];
data->gyro_z = raw[2];
return value; data->accel_x = raw[3];
data->accel_y = raw[4];
data->accel_z = raw[5];
return 0;
} }
static int I2C_LSM6DS_ReadRegister(uint8_t reg) static int lsm6dsInit(etx_i2c_bus_t bus, uint16_t addr)
{ {
uint8_t value = 0; s_i2c_bus = bus;
if (I2C_LSM6DS_ReadRegister(reg, &value, 1) < 0) s_i2c_addr = addr;
return -1;
return value; if (i2c_init(s_i2c_bus) < 0)
}
int gyroInit()
{
if (i2c_init(IMU_I2C_BUS) < 0)
return -1; return -1;
// LSM6DS33TR works with LSM6DSLTR code for our use // LSM6DS33TR works with LSM6DSLTR code for our use
uint8_t id = I2C_LSM6DS_ReadRegister(LSM6DS_WHO_AM_I); int id = read_reg(LSM6DS_WHO_AM_I);
if (id != LSM6DSLTR_ID && id != LSM6DS33TR_ID) if (id != LSM6DSLTR_ID && id != LSM6DS33TR_ID)
return -1; return -1;
for (uint8_t i = 0; i < DIM(configure); i++) { for (uint8_t i = 0; i < DIM(configure); i++) {
I2C_LSM6DS_WriteRegister(configure[i][0], configure[i][1]); write_reg(configure[i][0], configure[i][1]);
} }
return 0; return 0;
} }
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH]) const etx_imu_driver_t imu_lsm6ds_driver = {
{ lsm6dsInit,
return I2C_LSM6DS_ReadRegister(LSM6DS_GYRO_OUT_X_L_ADDR, buffer, IMU_BUFFER_LENGTH); lsm6dsRead,
} "LSM6DS",
};
#else
int gyroInit() { return -1; }
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH]) { return -1; }
#endif
@@ -19,7 +19,8 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "gyro.h" #pragma once
int gyroInit() { return -1; } #include "hal/imu.h"
int gyroRead(uint8_t*) { return -1; }
extern const etx_imu_driver_t imu_lsm6ds_driver;
+241
View File
@@ -0,0 +1,241 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program 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.
*/
#include "hal/i2c_driver.h"
#include "hal/imu.h"
#include "hal/gpio.h"
#include "delays_driver.h"
#include "sc7u22.h"
#include "inactivity_timer.h"
#include "debug.h"
static etx_i2c_bus_t s_i2c_bus;
static uint16_t s_i2c_addr;
static int write_reg(uint8_t reg, uint8_t val)
{
return i2c_write(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1);
}
static int read_regs(uint8_t reg, uint8_t *buf, uint8_t len)
{
return i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, buf, len);
}
static int soft_reset(void)
{
// Ensure Addr_Auto is enabled (default 0x50 has it set, but be explicit)
if (write_reg(SC7U22_COM_CONF_REG, SC7U22_COM_CONF_DEFAULT) < 0)
return -1;
delay_ms(1);
// Write 0xA5 twice to SOFT_RST
if (write_reg(SC7U22_SOFT_RST_REG, 0xA5) < 0) return -1;
if (write_reg(SC7U22_SOFT_RST_REG, 0xA5) < 0) return -1;
// Datasheet recommends ~100 ms after reset for chip to re-initialise
delay_ms(100);
return 0;
}
// ---------------------------------------------------------------------------
// Optional: WoM ISR (mirror of ICM42607C pattern)
// ---------------------------------------------------------------------------
#if defined(IMU_INT_GPIO)
static void imu_exti_isr(void)
{
inactivityTimerReset(ActivitySource::MainControls);
}
#endif
static int16_t __attribute__((unused)) getSC7U22Temperature(void)
{
uint8_t buf[2] = {0};
if (read_regs(SC7U22_TEMP_H_REG, buf, 2) < 0) {
TRACE("SC7U22 ERROR: TEMP read error");
return -1;
}
int16_t raw = (int16_t)((buf[0] << 8) | buf[1]);
return (int16_t)(raw / 512 + 23);
}
static int gyroSC7U22Init(etx_i2c_bus_t bus, uint16_t addr)
{
s_i2c_bus = bus;
s_i2c_addr = addr;
TRACE("SC7U22 I2C Init at address 0x%x", addr);
if (i2c_init(s_i2c_bus) < 0) {
TRACE("SC7U22 ERROR: i2c_init bus error");
return -1;
}
if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) {
TRACE("SC7U22 ERROR: device not ready");
return -1;
}
uint8_t who_am_i = 0;
if (read_regs(SC7U22_WHO_AM_I_REG, &who_am_i, 1) < 0) {
TRACE("SC7U22 ERROR: WHO_AM_I read error");
return -1;
}
if (who_am_i != 0x6A) {
TRACE("SC7U22 ERROR: unexpected WHO_AM_I 0x%02X (expected 0x6A)", who_am_i);
return -1;
}
if (soft_reset() < 0) {
TRACE("SC7U22 ERROR: soft reset failed");
return -1;
}
// -------------------------------------------------------------------------
// 4. Configure COM_CONF (0x04)
// BDU=1 (block data update – output not updated until both bytes read)
// Addr_Auto=1 (auto-increment for burst reads)
// All other bits at default (push-pull, active-high INT, 4-wire SPI)
// -------------------------------------------------------------------------
if (write_reg(SC7U22_COM_CONF_REG, SC7U22_COM_CONF_DEFAULT) < 0) {
TRACE("SC7U22 ERROR: COM_CONF write error");
return -1;
}
delay_ms(1);
// -------------------------------------------------------------------------
// 5. Enable accelerometer, gyroscope and temperature sensor via PWR_CTRL
// Datasheet §5.47: ACC_EN | GYR_EN | TEMP_EN
// Delay ≥1 ms after configuring before reading.
// -------------------------------------------------------------------------
if (write_reg(SC7U22_PWR_CTRL_REG, SC7U22_PWR_CTRL_ENABLE) < 0) {
TRACE("SC7U22 ERROR: PWR_CTRL write error");
return -1;
}
delay_ms(10); // Allow sensors to start up
// -------------------------------------------------------------------------
// 6. Configure accelerometer
// ACC_CONF (0x40): high-performance mode, NORM_AVG4 filter, 1600 Hz ODR
// ACC_RANGE (0x41): ±4g
// -------------------------------------------------------------------------
if (write_reg(SC7U22_ACC_CONF_REG, SC7U22_ACC_CONF_HP_1KHZ) < 0) {
TRACE("SC7U22 ERROR: ACC_CONF write error");
return -1;
}
delay_ms(1);
if (write_reg(SC7U22_ACC_RANGE_REG, SC7U22_ACC_RANGE_4G) < 0) {
TRACE("SC7U22 ERROR: ACC_RANGE write error");
return -1;
}
delay_ms(1);
// -------------------------------------------------------------------------
// 7. Configure gyroscope
// GYR_CONF (0x42): high-performance mode, NORM_AVG4 filter, 1600 Hz ODR
// GYR_RANGE (0x43): ±2000 dps
// -------------------------------------------------------------------------
if (write_reg(SC7U22_GYR_CONF_REG, SC7U22_GYR_CONF_HP_1KHZ) < 0) {
TRACE("SC7U22 ERROR: GYR_CONF write error");
return -1;
}
delay_ms(1);
if (write_reg(SC7U22_GYR_RANGE_REG, SC7U22_GYR_RANGE_2000DPS) < 0) {
TRACE("SC7U22 ERROR: GYR_RANGE write error");
return -1;
}
delay_ms(1);
// -------------------------------------------------------------------------
// 8. Optional: Wake-on-Motion via AOI1
// Uses the AOI1 OR-interrupt with all three axes enabled.
// Threshold: 0x7F LSB. At ±4g, 1 LSB = 32 mg → 0x7F ≈ 4g (max, safe).
// For activity detection similar to ICM42607C (0xFE ≈ 1g), set 0x1F ≈ 1g.
// -------------------------------------------------------------------------
#if defined(IMU_INT_GPIO)
// AOI1_CFG: AOI=0 (OR mode), 6D=0, enable ZHIE|YHIE|XHIE
if (write_reg(SC7U22_AOI1_CFG_REG, 0x15) < 0) return -1; // 0b00010101
delay_ms(1);
// AOI1_THS: ~0.5g threshold for ±4g range (1 LSB = 32 mg → 16 LSB ≈ 512 mg)
if (write_reg(SC7U22_AOI1_THS_REG, 0x10) < 0) return -1;
delay_ms(1);
// AOI1_DURATION: minimal (1 sample)
if (write_reg(SC7U22_AOI1_DURATION_REG, 0x00) < 0) return -1;
delay_ms(1);
// AOI1&AOI2_CFG (0x3F): enable AOI, high-pass filter on AOI1
// AOI_EN=1 (bit6), HPIS1=1 (bit4) for motion detection with HP data
if (write_reg(SC7U22_AOI1_AOI2_CFG_REG, 0x50) < 0) return -1;
delay_ms(1);
// Route AOI1 interrupt to INT1 pin (INT1_OUT_SEL2, 0x06, bit0 = INT1_AOI1)
if (write_reg(SC7U22_INT1_OUT_SEL2_REG, 0x01) < 0) return -1;
delay_ms(1);
// Configure INT1 GPIO as falling-edge interrupt input
gpio_init_int(IMU_INT_GPIO, GPIO_IN_PU, GPIO_FALLING, imu_exti_isr);
#endif
TRACE("SC7U22 succeeded");
return 0;
}
static int gyroSC7U22Read(etx_imu_data_t* data)
{
uint8_t buf[6];
// Read gyroscope: GYR_XH ... GYR_ZL (0x12 ... 0x17, 6 bytes)
if (read_regs(SC7U22_GYR_XH_REG, buf, 6) < 0) {
TRACE("SC7U22 ERROR: gyro read error");
return -1;
}
data->gyro_x = -((int16_t)(buf[0] << 8) | buf[1]);
data->gyro_y = -((int16_t)(buf[2] << 8) | buf[3]);
data->gyro_z = ((int16_t)(buf[4] << 8) | buf[5]);
// Read accelerometer: ACC_XH ... ACC_ZL (0x0C ... 0x11, 6 bytes)
if (read_regs(SC7U22_ACC_XH_REG, buf, 6) < 0) {
TRACE("SC7U22 ERROR: accel read error");
return -1;
}
data->accel_x = ((int16_t)(buf[0] << 8) | buf[1]);
data->accel_y = ((int16_t)(buf[2] << 8) | buf[3]);
data->accel_z = ((int16_t)(buf[4] << 8) | buf[5]);
return 0;
}
const etx_imu_driver_t imu_sc7u22_driver = {
gyroSC7U22Init,
gyroSC7U22Read,
"SC7U22",
};
+177
View File
@@ -0,0 +1,177 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program 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.
*/
/*
* Driver for Silan SC7U22 six-axis IMU (3-axis accelerometer + 3-axis gyroscope).
*
* I2C slave address: 0x19 (SDO/SA0 floating or high) / 0x18 (SDO/SA0 grounded)
* WHO_AM_I register (0x01) expected value: 0x6A
*/
#pragma once
#include "hal/imu.h"
extern const etx_imu_driver_t imu_sc7u22_driver;
// ---------------------------------------------------------------------------
// I2C address
// ---------------------------------------------------------------------------
// SDO/SA0 tied high or floating → 7-bit address 0x19
// SDO/SA0 grounded → 7-bit address 0x18
#define SC7U22_I2C_BASE_ADDR 0x18
// ---------------------------------------------------------------------------
// General-purpose registers (directly accessible with 8-bit address)
// ---------------------------------------------------------------------------
#define SC7U22_WHO_AM_I_REG 0x01 // Expected value: 0x6A
#define SC7U22_COM_CONF_REG 0x04 // Communication configuration
#define SC7U22_INT1_OUT_SEL1_REG 0x05
#define SC7U22_INT1_OUT_SEL2_REG 0x06
#define SC7U22_INT2_OUT_SEL1_REG 0x07
#define SC7U22_INT2_OUT_SEL2_REG 0x08
#define SC7U22_INT1_STAT_REG 0x09
#define SC7U22_INT2_STAT_REG 0x0A
#define SC7U22_DATA_STAT_REG 0x0B // Data ready status
// Accelerometer output registers (16-bit, 2's complement, MSB first)
#define SC7U22_ACC_XH_REG 0x0C
#define SC7U22_ACC_XL_REG 0x0D
#define SC7U22_ACC_YH_REG 0x0E
#define SC7U22_ACC_YL_REG 0x0F
#define SC7U22_ACC_ZH_REG 0x10
#define SC7U22_ACC_ZL_REG 0x11
// Gyroscope output registers (16-bit, 2's complement, MSB first)
#define SC7U22_GYR_XH_REG 0x12
#define SC7U22_GYR_XL_REG 0x13
#define SC7U22_GYR_YH_REG 0x14
#define SC7U22_GYR_YL_REG 0x15
#define SC7U22_GYR_ZH_REG 0x16
#define SC7U22_GYR_ZL_REG 0x17
// Temperature registers: T = (int16_t)(TEMP_H<<8 | TEMP_L) / 512 + 23 °C
#define SC7U22_TEMP_H_REG 0x22
#define SC7U22_TEMP_L_REG 0x23
// FIFO
#define SC7U22_FIFO_CFG0_REG 0x1C
#define SC7U22_FIFO_CFG1_REG 0x1D
#define SC7U22_FIFO_CFG2_REG 0x1E
#define SC7U22_FIFO_STAT0_REG 0x1F
#define SC7U22_FIFO_STAT1_REG 0x20
#define SC7U22_FIFO_DATA_REG 0x21
// Saturation / self-test status
#define SC7U22_DATA_SAT_REG 0x24
// Power control
#define SC7U22_PWR_CTRL_REG 0x7D
// Accelerometer configuration
#define SC7U22_ACC_CONF_REG 0x40 // ACC_FILTER_PERF | ACC_BWP[2:0] | ACC_ODR[3:0]
#define SC7U22_ACC_RANGE_REG 0x41 // FS[1:0]: 00=±2g 01=±4g 10=±8g 11=±16g
// Gyroscope configuration
#define SC7U22_GYR_CONF_REG 0x42 // GYR_FILTER_PERF | GYR_NOISE_PERF | GYR_BWP[1:0] | GYR_ODR[3:0]
#define SC7U22_GYR_RANGE_REG 0x43 // OIS bits | FS[2:0]: 000=±2000dps … 100=±125dps
// Soft reset (write 0xA5 twice after enabling Addr_Auto)
#define SC7U22_SOFT_RST_REG 0x4A
// Segment selector (for special registers)
#define SC7U22_SEG_SEL_REG 0x7F
// ---------------------------------------------------------------------------
// COM_CONF (0x04) bit definitions
// ---------------------------------------------------------------------------
#define SC7U22_COM_CONF_BOOT (1 << 7) // Reboot memory content
#define SC7U22_COM_CONF_BDU (1 << 6) // Block data update
#define SC7U22_COM_CONF_OIS_EN (1 << 5) // OIS enable
#define SC7U22_COM_CONF_ADDR_AUTO (1 << 4) // Address auto-increment
#define SC7U22_COM_CONF_PP_OD (1 << 3) // INT push-pull / open-drain
#define SC7U22_COM_CONF_INT_H (1 << 2) // INT active level
#define SC7U22_COM_CONF_OISM (1 << 1) // OIS SPI mode
#define SC7U22_COM_CONF_SIM (1 << 0) // SPI 4-wire / 3-wire
// Default after reset: BDU=1, Addr_Auto=1 → 0x50
#define SC7U22_COM_CONF_DEFAULT 0x50
// ---------------------------------------------------------------------------
// DATA_STAT (0x0B) bit definitions
// ---------------------------------------------------------------------------
#define SC7U22_DATA_STAT_DRDY_GYR (1 << 1)
#define SC7U22_DATA_STAT_DRDY_ACC (1 << 0)
#define SC7U22_DATA_STAT_DRDY_TMP (1 << 2)
// ---------------------------------------------------------------------------
// PWR_CTRL (0x7D) bit definitions
// ---------------------------------------------------------------------------
#define SC7U22_PWR_CTRL_TEMP_EN (1 << 3)
#define SC7U22_PWR_CTRL_ACC_EN (1 << 2)
#define SC7U22_PWR_CTRL_GYR_EN (1 << 1)
// Enable accelerometer + gyroscope + temperature sensor
#define SC7U22_PWR_CTRL_ENABLE (SC7U22_PWR_CTRL_TEMP_EN | \
SC7U22_PWR_CTRL_ACC_EN | \
SC7U22_PWR_CTRL_GYR_EN)
// ---------------------------------------------------------------------------
// ACC_CONF (0x40) preset values
// ACC_FILTER_PERF=1 (high-perf), ACC_BWP=010 (NORM_AVG4), ACC_ODR=1000 (100 Hz)
// → bits[7:0] = 1_010_1000 = 0xA8 (matches chip default)
// For 1 kHz high-performance: ACC_ODR=1100 → 0xAC
// ---------------------------------------------------------------------------
#define SC7U22_ACC_CONF_HP_1KHZ 0xAC // High-perf, NORM_AVG4, 1600 Hz ODR
#define SC7U22_ACC_CONF_HP_100HZ 0xA8 // High-perf, NORM_AVG4, 100 Hz ODR (default)
// ACC_RANGE: ±4g = 0x01
#define SC7U22_ACC_RANGE_4G 0x01
// ---------------------------------------------------------------------------
// GYR_CONF (0x42) preset values
// GYR_FILTER_PERF=1 (high-perf/normal), GYR_NOISE_PERF=0, GYR_BWP=10 (NORM_AVG4),
// GYR_ODR=1000 (100 Hz) → 0xA8 … for 1 kHz: GYR_ODR=1100 → 0xAC
// Chip default is 0xA9 (BWP=10, ODR=1001 = 200 Hz)
// ---------------------------------------------------------------------------
#define SC7U22_GYR_CONF_HP_1KHZ 0xAC // High-perf, NORM_AVG4, 1600 Hz ODR
#define SC7U22_GYR_CONF_HP_100HZ 0xA8 // High-perf, NORM_AVG4, 100 Hz ODR
// GYR_RANGE: ±2000 dps = FS[2:0]=000
#define SC7U22_GYR_RANGE_2000DPS 0x00
// ---------------------------------------------------------------------------
// WoM / interrupt registers (for optional wake-on-motion, same principle as
// ICM42607C but using SC7U22 AOI mechanism via AOI1_CFG / AOI2_CFG)
// ---------------------------------------------------------------------------
#define SC7U22_AOI1_CFG_REG 0x30
#define SC7U22_AOI1_STAT_REG 0x31
#define SC7U22_AOI1_THS_REG 0x32
#define SC7U22_AOI1_DURATION_REG 0x33
#define SC7U22_AOI2_CFG_REG 0x34
#define SC7U22_AOI2_STAT_REG 0x35
#define SC7U22_AOI2_THS_REG 0x36
#define SC7U22_AOI2_DURATION_REG 0x37
#define SC7U22_AOI1_AOI2_CFG_REG 0x3F
// INT1_OUT_SEL1 bits (0x05)
#define SC7U22_INT1_ACC (1 << 0)
#define SC7U22_INT1_GYR (1 << 2)
+4 -4
View File
@@ -125,13 +125,13 @@ void menuRadioDiagAnalogs(event_t event)
y += FH; y += FH;
uint8_t x = INDENT_WIDTH; uint8_t x = INDENT_WIDTH;
lcdDrawText(x, y, "X:"); lcdDrawText(x, y, "X:");
lcdDrawNumber(x+3*FW-1, y, gyro.outputs[0] * 180 / 1024); lcdDrawNumber(x+3*FW-1, y, gyroOutputs[0] * 180 / 1024);
lcdDrawChar(lcdNextPos, y, CHAR_BW_DEGREE); lcdDrawChar(lcdNextPos, y, CHAR_BW_DEGREE);
lcdDrawNumber(x+10*FW-1, y, gyro.scaledX(), RIGHT); lcdDrawNumber(x+10*FW-1, y, gyroScaledX(), RIGHT);
x = LCD_W/2 + INDENT_WIDTH; x = LCD_W/2 + INDENT_WIDTH;
lcdDrawText(x, y, "Y:"); lcdDrawText(x, y, "Y:");
lcdDrawNumber(x+3*FW-1, y, gyro.outputs[1] * 180 / 1024); lcdDrawNumber(x+3*FW-1, y, gyroOutputs[1] * 180 / 1024);
lcdDrawChar(lcdNextPos, y, CHAR_BW_DEGREE); lcdDrawChar(lcdNextPos, y, CHAR_BW_DEGREE);
lcdDrawNumber(x+10*FW-1, y, gyro.scaledY(), RIGHT); lcdDrawNumber(x+10*FW-1, y, gyroScaledY(), RIGHT);
#endif #endif
} }
+2 -2
View File
@@ -484,7 +484,7 @@ void menuRadioSetup(event_t event)
if (attr) { if (attr) {
CHECK_INCDEC_GENVAR(event, g_eeGeneral.imuMax, IMU_MAX_DEFAULT - IMU_MAX_RANGE, IMU_MAX_DEFAULT + IMU_MAX_RANGE); CHECK_INCDEC_GENVAR(event, g_eeGeneral.imuMax, IMU_MAX_DEFAULT - IMU_MAX_RANGE, IMU_MAX_DEFAULT + IMU_MAX_RANGE);
lcdDrawText(lp, y, ")", RIGHT); lcdDrawText(lp, y, ")", RIGHT);
lcdDrawNumber(lcdLastLeftPos, y, max(abs(gyro.outputs[0]), abs(gyro.outputs[1])) * 180 / 1024, RIGHT); lcdDrawNumber(lcdLastLeftPos, y, max(abs(gyroOutputs[0]), abs(gyroOutputs[1])) * 180 / 1024, RIGHT);
lcdDrawText(lcdLastLeftPos, y, "(", RIGHT); lcdDrawText(lcdLastLeftPos, y, "(", RIGHT);
} }
} }
@@ -499,7 +499,7 @@ void menuRadioSetup(event_t event)
if (attr) { if (attr) {
CHECK_INCDEC_GENVAR(event, g_eeGeneral.imuOffset, IMU_OFFSET_MIN, IMU_OFFSET_MAX); CHECK_INCDEC_GENVAR(event, g_eeGeneral.imuOffset, IMU_OFFSET_MIN, IMU_OFFSET_MAX);
lcdDrawText(lp, y, ")", RIGHT); lcdDrawText(lp, y, ")", RIGHT);
lcdDrawNumber(lcdLastLeftPos, y, gyro.outputs[0] * 180 / 1024, RIGHT); lcdDrawNumber(lcdLastLeftPos, y, gyroOutputs[0] * 180 / 1024, RIGHT);
lcdDrawText(lcdLastLeftPos, y, "(", RIGHT); lcdDrawText(lcdLastLeftPos, y, "(", RIGHT);
} }
} }
+27 -16
View File
@@ -124,26 +124,37 @@ class AnaViewWindow : public Window
} }
} }
#if defined(IMU_ICM4207C) && LANDSCAPE #if defined(IMU) && LANDSCAPE
line = newLine(grid); if (imuGetName()) {
lv_obj_set_style_pad_column(line->getLvObj(), PAD_SMALL, LV_PART_MAIN); line = newLine(grid);
lv_obj_set_style_pad_column(line->getLvObj(), PAD_SMALL, LV_PART_MAIN);
grid.setColSpan(2); grid.setColSpan(2);
new StaticText(line, rect_t{}, "Tilt X"); new StaticText(line, rect_t{}, STR_GYRO);
grid.setColSpan(1); grid.setColSpan(3);
new DynamicNumber<int16_t>(line, rect_t{}, new StaticText(line, rect_t{}, imuGetName());
[=]() { return gyro.scaledX(); }, COLOR_THEME_PRIMARY1_INDEX, RIGHT); grid.setColSpan(1);
for (int i = 0; i < 3; i++) {grid.nextCell();} line = newLine(grid);
lv_obj_set_style_pad_column(line->getLvObj(), PAD_SMALL, LV_PART_MAIN);
line = newLine(grid); grid.setColSpan(2);
lv_obj_set_style_pad_column(line->getLvObj(), PAD_SMALL, LV_PART_MAIN); new StaticText(line, rect_t{}, "Tilt X");
grid.setColSpan(1);
new DynamicNumber<int16_t>(line, rect_t{},
[=]() { return gyroScaledX(); }, COLOR_THEME_PRIMARY1_INDEX, RIGHT);
grid.setColSpan(2); for (int i = 0; i < 3; i++) {grid.nextCell();}
new StaticText(line, rect_t{}, "Tilt Y");
grid.setColSpan(1); line = newLine(grid);
new DynamicNumber<int16_t>(line, rect_t{}, lv_obj_set_style_pad_column(line->getLvObj(), PAD_SMALL, LV_PART_MAIN);
[=]() { return gyro.scaledY(); }, COLOR_THEME_PRIMARY1_INDEX, RIGHT);
grid.setColSpan(2);
new StaticText(line, rect_t{}, "Tilt Y");
grid.setColSpan(1);
new DynamicNumber<int16_t>(line, rect_t{},
[=]() { return gyroScaledY(); }, COLOR_THEME_PRIMARY1_INDEX, RIGHT);
}
#endif #endif
#if defined(LUMINOSITY_SENSOR) #if defined(LUMINOSITY_SENSOR)
+96 -68
View File
@@ -21,113 +21,141 @@
#include "edgetx.h" #include "edgetx.h"
#include "hal.h" #include "hal.h"
#include "debug.h"
#define _USE_MATH_DEFINES
#include <math.h>
#include "hal/usb_driver.h" #include "hal/usb_driver.h"
#define ACC_LSB_VALUE 0.000488 // 0.488 mg/LSB #define _USE_MATH_DEFINES
#include <math.h>
#define ALPHA 0.98 #define COMPLEMENTARY_ALPHA 0.92f
#define DT 0.01 #define LP_FILTER_ALPHA 0.9f
#define SCALE_FACT_ACC 0.005 #define SAMPLE_TIME_S 0.005f
#define SCALE_FACT_GYRO 0.0078 // 250deg/s / 32000
Gyro gyro; int16_t gyroOutputs[2];
#if !defined(IMU_ICM4207C) static imu_read_fn readFn;
static float deg2RESX(float deg) static uint8_t errors;
static int16_t offset_x, offset_y;
static int16_t range_x = 8192, range_y = 8192;
static int16_t raw_ax, raw_ay;
// filter state
static bool filterInitialized;
static float roll, pitch, yaw;
static float filteredAccel[3];
static float prevAccel[3];
static float filteredGyro[3];
static float prevGyro[3];
static float lowPass(float val, float prev, float alpha)
{ {
// [-90 : 90] -> [-RESX : RESX] return alpha * prev + (1.0f - alpha) * val;
return (deg * float(RESX)) / 90.0;
} }
#endif
void Gyro::wakeup() static void gyroFilter(etx_imu_data_t* raw)
{
// low-pass
filteredAccel[0] = lowPass(raw->accel_x, prevAccel[0], LP_FILTER_ALPHA);
filteredAccel[1] = lowPass(raw->accel_y, prevAccel[1], LP_FILTER_ALPHA);
filteredAccel[2] = lowPass(raw->accel_z, prevAccel[2], LP_FILTER_ALPHA);
filteredGyro[0] = lowPass(raw->gyro_x, prevGyro[0], LP_FILTER_ALPHA);
filteredGyro[1] = lowPass(raw->gyro_y, prevGyro[1], LP_FILTER_ALPHA);
filteredGyro[2] = lowPass(raw->gyro_z, prevGyro[2], LP_FILTER_ALPHA);
prevAccel[0] = filteredAccel[0];
prevAccel[1] = filteredAccel[1];
prevAccel[2] = filteredAccel[2];
prevGyro[0] = filteredGyro[0];
prevGyro[1] = filteredGyro[1];
prevGyro[2] = filteredGyro[2];
// complementary filter for attitude
float ax = raw->accel_x;
float ay = raw->accel_y;
float az = raw->accel_z;
float norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return;
ax /= norm;
ay /= norm;
az /= norm;
float accelRoll = atan2f(ay, sqrtf(ax * ax + az * az));
float accelPitch = atan2f(-ax, sqrtf(ay * ay + az * az));
if (!filterInitialized) {
roll = accelRoll;
pitch = accelPitch;
yaw = 0.0f;
filterInitialized = true;
} else {
float dt = SAMPLE_TIME_S;
roll = COMPLEMENTARY_ALPHA * (roll + raw->gyro_x * dt) +
(1.0f - COMPLEMENTARY_ALPHA) * accelRoll;
pitch = COMPLEMENTARY_ALPHA * (pitch + raw->gyro_y * dt) +
(1.0f - COMPLEMENTARY_ALPHA) * accelPitch;
yaw += raw->gyro_z * dt;
if (yaw > (float)M_PI) yaw -= 2.0f * (float)M_PI;
if (yaw < -(float)M_PI) yaw += 2.0f * (float)M_PI;
}
}
void gyroStart(imu_read_fn fn)
{
readFn = fn;
}
void gyroWakeup()
{ {
static tmr10ms_t gyroWakeupTime = 0; static tmr10ms_t gyroWakeupTime = 0;
tmr10ms_t now = get_tmr10ms(); tmr10ms_t now = get_tmr10ms();
if (errors >= 100 || now < gyroWakeupTime || usbPluggedInStorageMode()) if (!readFn || errors >= 100 || now < gyroWakeupTime ||
usbPluggedInStorageMode())
return; return;
gyroWakeupTime = now + 1; /* 10ms default */ gyroWakeupTime = now + 1; /* 10ms default */
int16_t values[IMU_VALUES_COUNT]; etx_imu_data_t raw;
if (gyroRead((uint8_t*)values) < 0) { if (readFn(&raw) < 0) {
++errors; ++errors;
return; return;
} }
// reset error count on each
// successful query to avoid
// stopping the sensor forever
errors = 0; errors = 0;
#if defined(IMU_ICM4207C) gyroFilter(&raw);
raw_ax = values[3];
raw_ay = values[4]; raw_ax = (int16_t)filteredAccel[0];
raw_ay = (int16_t)filteredAccel[1];
int16_t ax = raw_ax - offset_x; int16_t ax = raw_ax - offset_x;
int16_t ay = raw_ay - offset_y; int16_t ay = raw_ay - offset_y;
// Use only ACC value, they are really reliable gyroOutputs[0] = (ax * float(RESX)) / range_x;
outputs[0] = (ax * float(RESX)) / range_x; gyroOutputs[1] = (ay * float(RESX)) / range_y;
outputs[1] = (ay * float(RESX)) / range_y;
#else
int16_t ax = values[3];
int16_t ay = values[4];
int16_t az = values[5];
int16_t gx = values[0];
int16_t gy = values[1];
// int16_t gz = values[2];
// integrate gyro
roll -= gx * SCALE_FACT_GYRO * DT;
pitch += gy * SCALE_FACT_GYRO * DT;
int32_t magn = abs(ax) + abs(ay) + abs(az);
if (magn > 8192 && magn < 32768) {
if (az < 0) az = -az;
float rollAcc = atan2f((float)ay,(float)az) * 57.3 /* 180/PI */;
float pitchAcc = atan2f((float)ax,(float)az) * 57.3 /* 180/PI */;
roll = roll * ALPHA + rollAcc * (1.0-ALPHA);
pitch = pitch * ALPHA + pitchAcc * (1.0-ALPHA);
}
outputs[0] = deg2RESX(roll);
outputs[1] = deg2RESX(pitch);
#endif
} }
int16_t Gyro::scaledX() int16_t gyroScaledX()
{ {
// return limit(-RESX, return limit<int16_t>(-RESX, gyroOutputs[0], RESX);
// (int)(outputs[0] - g_eeGeneral.imuOffset * RESX / 180) *
// (180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
// RESX);
return limit<int16_t>(-RESX, outputs[0], RESX);
} }
int16_t Gyro::scaledY() int16_t gyroScaledY()
{ {
// return limit(-RESX, return limit<int16_t>(-RESX, gyroOutputs[1], RESX);
// outputs[1] * (180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
// RESX);
return limit<int16_t>(-RESX, outputs[1], RESX);
} }
void Gyro::setIMU_X(int16_t offset, int16_t range) void gyroSetIMU_X(int16_t offset, int16_t range)
{ {
if (offset == -1) offset_x = raw_ax; if (offset == -1) offset_x = raw_ax;
else offset_x = offset; else offset_x = offset;
range_x = range * 8192 / 180; range_x = range * 8192 / 180;
} }
void Gyro::setIMU_Y(int16_t offset, int16_t range) void gyroSetIMU_Y(int16_t offset, int16_t range)
{ {
if (offset == -1) offset_y = raw_ay; if (offset == -1) offset_y = raw_ay;
else offset_y = offset; else offset_y = offset;
+10 -35
View File
@@ -19,46 +19,21 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include <inttypes.h> #pragma once
#include "myeeprom.h"
#define IMU_VALUES_COUNT 6 #include <inttypes.h>
#define IMU_BUFFER_LENGTH (IMU_VALUES_COUNT * sizeof(int16_t)) #include "hal/imu.h"
#define IMU_MAX_DEFAULT 30 #define IMU_MAX_DEFAULT 30
#define IMU_MAX_RANGE 60 #define IMU_MAX_RANGE 60
#define IMU_OFFSET_MIN -30 #define IMU_OFFSET_MIN -30
#define IMU_OFFSET_MAX 10 #define IMU_OFFSET_MAX 10
#define IMU_SAMPLES_EXPONENT 3 void gyroStart(imu_read_fn fn);
#define IMU_SAMPLES_COUNT (2 ^ IMU_SAMPLES_EXPONENT) void gyroWakeup();
void gyroSetIMU_X(int16_t offset, int16_t range);
void gyroSetIMU_Y(int16_t offset, int16_t range);
int16_t gyroScaledX();
int16_t gyroScaledY();
class Gyro extern int16_t gyroOutputs[2];
{
protected:
uint8_t errors = 0;
float roll = 0.0;
float pitch = 0.0;
int16_t offset_x = 0;
int16_t offset_y = 0;
int16_t range_x = 8192;
int16_t range_y = 8192;
int16_t raw_ax = 0;
int16_t raw_ay = 0;
public:
int16_t outputs[2];
void wakeup();
void setIMU_X(int16_t, int16_t);
void setIMU_Y(int16_t, int16_t);
int16_t scaledX();
int16_t scaledY();
};
extern Gyro gyro;
// Gyro driver
int gyroInit();
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH]);
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) EdgeTX * Copyright (C) EdgeTX
* *
* Based on code named * Based on code named
* opentx - https://github.com/opentx/opentx * opentx - https://github.com/opentx/opentx
@@ -19,20 +19,22 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#pragma once #include "hal/imu.h"
// IMU data structure static const char* s_imu_name = nullptr;
typedef struct {
float accel_x, accel_y, accel_z; // Accelerometer data (m/s²)
float gyro_x, gyro_y, gyro_z; // Gyroscope data (rad/s)
float mag_x, mag_y, mag_z; // Magnetometer data (optional)
} IMU_RawData_t;
typedef struct { imu_read_fn imuDetect(const etx_imu_t* candidates, uint8_t count)
float roll, pitch, yaw; // Filtered attitude (radians) {
float accel_x, accel_y, accel_z; // Filtered acceleration for (uint8_t i = 0; i < count; i++) {
float gyro_x, gyro_y, gyro_z; // Filtered gyroscope if (candidates[i].driver->init(candidates[i].bus, candidates[i].addr) == 0) {
} IMU_FilteredData_t; s_imu_name = candidates[i].driver->name;
return candidates[i].driver->read;
}
}
return nullptr;
}
void process_imu_data(IMU_RawData_t *raw_data); const char* imuGetName()
IMU_FilteredData_t* get_filtered_imu_data(void); {
return s_imu_name;
}
+51
View File
@@ -0,0 +1,51 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program 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.
*/
#pragma once
#include <stdint.h>
#include "hal/i2c_driver.h"
struct etx_imu_data_t {
float accel_x, accel_y, accel_z;
float gyro_x, gyro_y, gyro_z;
};
typedef int (*imu_init_fn)(etx_i2c_bus_t bus, uint16_t addr);
typedef int (*imu_read_fn)(etx_imu_data_t* data);
struct etx_imu_driver_t {
imu_init_fn init;
imu_read_fn read;
const char* name;
};
struct etx_imu_t {
const etx_imu_driver_t* driver;
etx_i2c_bus_t bus;
uint16_t addr;
};
// Generic detection: iterates candidates, returns read fn on success
imu_read_fn imuDetect(const etx_imu_t* candidates, uint8_t count);
// Returns the name of the detected IMU, or nullptr if none
const char* imuGetName();
+2 -2
View File
@@ -3065,7 +3065,7 @@ static int luaSetIMU_X(lua_State* const L)
return 1; return 1;
} }
gyro.setIMU_X(offset, range); gyroSetIMU_X(offset, range);
#endif #endif
lua_pushboolean(L, true); lua_pushboolean(L, true);
return 1; return 1;
@@ -3096,7 +3096,7 @@ static int luaSetIMU_Y(lua_State* const L)
return 1; return 1;
} }
gyro.setIMU_Y(offset, range); gyroSetIMU_Y(offset, range);
#endif #endif
lua_pushboolean(L, true); lua_pushboolean(L, true);
return 1; return 1;
+2 -2
View File
@@ -386,10 +386,10 @@ getvalue_t _getValue(mixsrc_t i, bool* valid)
#if defined(IMU) #if defined(IMU)
else if (i == MIXSRC_TILT_X) { else if (i == MIXSRC_TILT_X) {
return gyro.scaledX(); return gyroScaledX();
} }
else if (i == MIXSRC_TILT_Y) { else if (i == MIXSRC_TILT_Y) {
return gyro.scaledY(); return gyroScaledY();
} }
#endif #endif
@@ -137,7 +137,7 @@ endif()
if(IMU) if(IMU)
add_definitions(-DIMU) add_definitions(-DIMU)
set(SRC ${SRC} gyro.cpp) set(SRC ${SRC} gyro.cpp hal/imu.cpp)
endif() endif()
if(AUX_SERIAL OR AUX2_SERIAL) if(AUX_SERIAL OR AUX2_SERIAL)
+1 -1
View File
@@ -291,7 +291,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_COMMON_SRC} ${BOARD_COMMON_SRC}
targets/common/arm/stm32/delays_driver.cpp targets/common/arm/stm32/delays_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp drivers/lsm6ds.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/sticks_pwm_driver.cpp targets/common/arm/stm32/sticks_pwm_driver.cpp
+21
View File
@@ -53,6 +53,10 @@
#include "csd203_sensor.h" #include "csd203_sensor.h"
#endif #endif
#if defined(IMU) && defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS)
#define HAS_IMU
#endif
HardwareOptions hardwareOptions; HardwareOptions hardwareOptions;
bool boardBacklightOn = false; bool boardBacklightOn = false;
@@ -150,6 +154,19 @@ uint16_t getSixPosAnalogValue(uint16_t adcValue)
} }
#endif #endif
#if defined(HAS_IMU)
#include "drivers/lsm6ds.h"
#include "stm32_i2c_driver.h"
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
#endif
void boardInit() void boardInit()
{ {
delaysInit(); delaysInit();
@@ -246,6 +263,10 @@ void boardInit()
ledGreen(); ledGreen();
#endif #endif
#endif #endif
#if defined(HAS_IMU)
gyroInit();
#endif
} }
#endif #endif
-263
View File
@@ -1,263 +0,0 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program 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.
*/
#include "edgetx.h"
#include "i2c_driver.h"
#include "imu_lsm6ds33.h"
I2C_HandleTypeDef hi2c2;
sIMUsettings IMUsettings = {-1, -1.0, -1, -1, -1.0};
sIMUoutput IMUoutput = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float fIMUtemp = 0.0;
bool I2C_LSM6DS33_ReadRegister(uint8_t reg, uint8_t * buf, uint8_t len)
{
if (HAL_I2C_Master_Transmit(&hi2c2, LSM6DS33_I2C_ADDR << 1, &reg, 1, 10000) != HAL_OK)
{
TRACE("I2C B2 ERROR: ReadRegister write reg. address failed");
return false;
}
if (HAL_I2C_Master_Receive(&hi2c2, LSM6DS33_I2C_ADDR << 1, buf, len, 10000) != HAL_OK)
{
TRACE("I2C B2 ERROR: ReadRegister read register failed");
return false;
}
return true;
}
bool I2C_LSM6DS33_WriteRegister(uint8_t reg, uint8_t * buf, uint8_t len)
{
uint8_t uAddrAndBuf[15];
uAddrAndBuf[0] = reg;
if (len > 0)
{
for (int i = 0;i < len;i++)
{
uAddrAndBuf[i + 1] = buf[i];
}
}
if (HAL_I2C_Master_Transmit(&hi2c2, LSM6DS33_I2C_ADDR << 1, uAddrAndBuf, len + 1, 10000) != HAL_OK)
{
TRACE("I2C B2 ERROR: WriteRegister failed");
return false;
}
return true;
}
bool IMUgetConf(void)
{
uint8_t ui8_regs[2] = {0};
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_CTRL1_XL_ADDR, ui8_regs, 2))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
switch ((ui8_regs[0] >> 4) & 0x0F) // CTRL1_XL
{
case LSM6DS33_ODR_0Hz: IMUsettings.linacc_odr = 0; TRACE("LSM6DS33 lin. acc. output turned off"); break;
case LSM6DS33_ODR_12_5Hz: IMUsettings.linacc_odr = 13; break;
case LSM6DS33_ODR_26Hz: IMUsettings.linacc_odr = 26; break;
case LSM6DS33_ODR_52Hz: IMUsettings.linacc_odr = 52; break;
case LSM6DS33_ODR_104Hz: IMUsettings.linacc_odr = 104; break;
case LSM6DS33_ODR_208Hz: IMUsettings.linacc_odr = 208; break;
case LSM6DS33_ODR_416Hz: IMUsettings.linacc_odr = 417; break;
case LSM6DS33_ODR_833Hz: IMUsettings.linacc_odr = 833; break;
case LSM6DS33_ODR_1_66kHz: IMUsettings.linacc_odr = 1667; break;
case LSM6DS33_ODR_3_33kHz: IMUsettings.linacc_odr = 3333; break;
case LSM6DS33_ODR_6_66kHz: IMUsettings.linacc_odr = 6667; break;
default: TRACE("ERROR: LSM6DS33 lin.acc. output data rate at illegal setting"); return false; break;
}
if (IMUsettings.linacc_odr > 0) TRACE("LSM6DS33 lin. acc. output at %i Hz", IMUsettings.linacc_odr);
switch ((ui8_regs[0] >> 2) & 0x03)
{
case LSM6DS33_LA_FS_2G: IMUsettings.linacc_mG = 0.061; break;
case LSM6DS33_LA_FS_4G: IMUsettings.linacc_mG = 0.122; break;
case LSM6DS33_LA_FS_8G: IMUsettings.linacc_mG = 0.244; break;
case LSM6DS33_LA_FS_16G: IMUsettings.linacc_mG = 0.488; break;
default: TRACE("ERROR: LSM6DS33 lin. acc full scale value at illegal setting"); return false; break;
}
TRACE("LSM6DS33 lin. acc. resolution %.2f G", IMUsettings.linacc_mG);
switch ((ui8_regs[0] >> 2) & 0x03)
{
case LSM6DS33_LA_AABW_400Hz: IMUsettings.linacc_aabw = 400; break;
case LSM6DS33_LA_AABW_200Hz: IMUsettings.linacc_aabw = 200; break;
case LSM6DS33_LA_AABW_100Hz: IMUsettings.linacc_aabw = 100; break;
case LSM6DS33_LA_AABW_50Hz: IMUsettings.linacc_aabw = 50; break;
default: TRACE("ERROR: LSM6DS33 lin. acc anti aliasing bandwidth at illegal setting"); return false; break;
}
TRACE("LSM6DS33 lin. acc. anti aliasing bandwidth at %i Hz", IMUsettings.linacc_aabw);
switch ((ui8_regs[1] >> 4) & 0x0F) // CTRL2_G
{
case LSM6DS33_ODR_0Hz: IMUsettings.gyro_odr = 0; TRACE("LSM6DS33 gyro output turned off"); break;
case LSM6DS33_ODR_12_5Hz: IMUsettings.gyro_odr = 13; break;
case LSM6DS33_ODR_26Hz: IMUsettings.gyro_odr = 26; break;
case LSM6DS33_ODR_52Hz: IMUsettings.gyro_odr = 52; break;
case LSM6DS33_ODR_104Hz: IMUsettings.gyro_odr = 104; break;
case LSM6DS33_ODR_208Hz: IMUsettings.gyro_odr = 208; break;
case LSM6DS33_ODR_416Hz: IMUsettings.gyro_odr = 417; break;
case LSM6DS33_ODR_833Hz: IMUsettings.gyro_odr = 833; break;
case LSM6DS33_ODR_1_66kHz: IMUsettings.gyro_odr = 1667; break;
default: TRACE("ERROR: LSM6DS33 gyro output data rate at illegal setting"); return false; break;
}
if (IMUsettings.gyro_odr > 0) TRACE("LSM6DS33 gyro output at %i Hz", IMUsettings.gyro_odr);
switch ((ui8_regs[1] >> 1) & 0x07)
{
case LSM6DS33_GY_FS_125dps: IMUsettings.gyro_mDPS = 4.375; break;
case LSM6DS33_GY_FS_250dps: IMUsettings.gyro_mDPS = 8.75; break;
case LSM6DS33_GY_FS_500dps: IMUsettings.gyro_mDPS = 17.5; break;
case LSM6DS33_GY_FS_1000dps: IMUsettings.gyro_mDPS = 35.0; break;
case LSM6DS33_GY_FS_2000dps: IMUsettings.gyro_mDPS = 70.0; break;
default: TRACE("ERROR: LSM6DS33 gyro full scale value at illegal setting"); return false; break;
}
TRACE("LSM6DS33 raw gyro resolution: %.2f milli-degrees/sec.", IMUsettings.gyro_mDPS);
return true;
}
void init_imu_i2c(void)
{
TRACE("I2C IMU Init");
hi2c2.Instance = I2C_B2;
hi2c2.Init.ClockSpeed = I2C_B2_CLK_RATE;
hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c2.Init.OwnAddress1 = 0;
hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c2.Init.OwnAddress2 = 0;
hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c2) != HAL_OK)
{
TRACE("I2C B2 ERROR: HAL_I2C_Init() failed");
}
// Configure Analogue filter
if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
{
TRACE("I2C B2 ERROR: HAL_I2CEx_ConfigAnalogFilter() failed");
}
// Configure Digital filter
if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK)
{
TRACE("I2C B2 ERROR: HAL_I2CEx_ConfigDigitalFilter() failed");
}
}
bool imu_lsm6ds33_init(void)
{
uint8_t ui8_reg[2] = {0};
GPIO_SetBits(AUX_I2C_B2_PWR_GPIO, AUX_I2C_B2_PWR_GPIO_PIN); // Turn on AUX1 power
init_imu_i2c();
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_WHO_AM_I_ADDR, ui8_reg, 1))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
if (ui8_reg[0] != LSM6DS33_WHOAMI)
{
TRACE("LSM6DS33 ERROR: Product ID read failes");
return false;
}
TRACE("LSM6DS33 detected");
// Set IMU configuration
ui8_reg[0] = (LSM6DS33_ODR_104Hz << 4) | (LSM6DS33_LA_FS_4G << 2) | LSM6DS33_LA_AABW_100Hz; // Linear acceleration configuration
ui8_reg[1] = (LSM6DS33_ODR_104Hz << 4) | (LSM6DS33_GY_FS_125dps < 1); // Gyro configuration
if (!I2C_LSM6DS33_WriteRegister(LSM6DS33_CTRL1_XL_ADDR, ui8_reg, 2))
{
TRACE("ERROR: Failed to write LSM6DS33 configuration");
return false;
}
if (!IMUgetConf())
{
TRACE("ERROR: Failed ro read LSM6DS33 configuration");
return false;
}
return true;
}
bool imu_lsm6ds33_read()
{
uint8_t ui8_regs[14] = {0};
int16_t i16_rawTemperature = 0;
int16_t i16_rawGyroX = 0;
int16_t i16_rawGyroY = 0;
int16_t i16_rawGyroZ = 0;
int16_t i16_rawAccX = 0;
int16_t i16_rawAccY = 0;
int16_t i16_rawAccZ = 0;
// Check first if new data is available
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_STATUS_REG_ADDR, ui8_regs, 1))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
if ((ui8_regs[0] & 0x07) != 0x07)
{
TRACE("ERROR: LSM6DS33 has no new data available yet");
return false;
}
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_OUT_TEMP_L_ADDR, ui8_regs, 14))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
i16_rawTemperature = (ui8_regs[1] << 8) | ui8_regs[0];
i16_rawGyroX = (ui8_regs[3] << 8) | ui8_regs[2];
i16_rawGyroY = (ui8_regs[5] << 8) | ui8_regs[4];
i16_rawGyroZ = (ui8_regs[7] << 8) | ui8_regs[6];
i16_rawAccX = (ui8_regs[9] << 8) | ui8_regs[8];
i16_rawAccY = (ui8_regs[11] << 8) | ui8_regs[10];
i16_rawAccZ = (ui8_regs[13] << 8) | ui8_regs[12];
IMUoutput.fGyroXradps = i16_rawGyroX * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0;
IMUoutput.fGyroYradps = i16_rawGyroY * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0;
IMUoutput.fGyroZradps = i16_rawGyroZ * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0;
IMUoutput.fAccX = i16_rawAccX * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0;
IMUoutput.fAccY = i16_rawAccY * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0;
IMUoutput.fAccZ = i16_rawAccZ * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0;
IMUoutput.fTemperatureDegC = (float)i16_rawTemperature / 16.0 + 25.0;
/*TRACE("LSM6DS33: %.2f deg.C, GyroXYZ [rad/s]: %.2f, %.2f, %.2f, AccXYZ [m/s^2]: %.2f %.2f %.2f",
IMUoutput.fTemperatureDegC,
IMUoutput.fGyroXradps, IMUoutput.fGyroYradps, IMUoutput.fGyroZradps,
IMUoutput.fAccX, IMUoutput.fAccY, IMUoutput.fAccZ); */
return true;
}
-139
View File
@@ -1,139 +0,0 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program 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.
*/
#pragma once
#include "i2c_driver.h"
#define GRAVITY_EARTH 9.80665F // m/s^2
#define LSM6DS33_I2C_ADDR 0x6A
#define LSM6DS33_WHOAMI 0x69
#define LSM6DS33_TIMEOUT 3 // 3ms
// LSM6DS33 register map
#define LSM6DS33_FUNC_CFG_ACCESS_ADDR 0x01
#define LSM6DS33_FIFO_CTRL1_ADDR 0x06
#define LSM6DS33_FIFO_CTRL2_ADDR 0x07
#define LSM6DS33_FIFO_CTRL3_ADDR 0x08
#define LSM6DS33_FIFO_CTRL4_ADDR 0x09
#define LSM6DS33_FIFO_CTRL5_ADDR 0x0A
#define LSM6DS33_ORIENT_CFG_G_ADDR 0x0B
#define LSM6DS33_INT1_CTRL_ADDR 0x0D
#define LSM6DS33_INT2_CTRL_ADDR 0x0E
#define LSM6DS33_WHO_AM_I_ADDR 0x0F
#define LSM6DS33_CTRL1_XL_ADDR 0x10
#define LSM6DS33_CTRL2_G_ADDR 0x11
#define LSM6DS33_CTRL3_C_ADDR 0x12
#define LSM6DS33_CTRL4_C_ADDR 0x13
#define LSM6DS33_CTRL5_C_ADDR 0x14
#define LSM6DS33_CTRL6_C_ADDR 0x15
#define LSM6DS33_CTRL7_G_ADDR 0x16
#define LSM6DS33_CTRL8_XL_ADDR 0x17
#define LSM6DS33_CTRL9_XL_ADDR 0x18
#define LSM6DS33_CTRL10_C_ADDR 0x19
#define LSM6DS33_WAKE_UP_SRC_ADDR 0x1B
#define LSM6DS33_TAP_SRC_ADDR 0x1C
#define LSM6DS33_D6D_SRC_ADDR 0x1D
#define LSM6DS33_STATUS_REG_ADDR 0x1E
#define LSM6DS33_OUT_TEMP_L_ADDR 0x20
#define LSM6DS33_OUT_TEMP_H_ADDR 0x21
#define LSM6DS33_OUTX_L_G_ADDR 0x22
#define LSM6DS33_OUTX_H_G_ADDR 0x23
#define LSM6DS33_OUTY_L_G_ADDR 0x24
#define LSM6DS33_OUTY_H_G_ADDR 0x25
#define LSM6DS33_OUTZ_L_G_ADDR 0x26
#define LSM6DS33_OUTZ_H_G_ADDR 0x27
#define LSM6DS33_OUTX_L_XL_ADDR 0x28
#define LSM6DS33_OUTX_H_XL_ADDR 0x29
#define LSM6DS33_OUTY_L_XL_ADDR 0x2A
#define LSM6DS33_OUTY_H_XL_ADDR 0x2B
#define LSM6DS33_OUTZ_L_XL_ADDR 0x2C
#define LSM6DS33_OUTZ_H_XL_ADDR 0x2D
#define LSM6DS33_FIFO_STATUS1_ADDR 0x3A
#define LSM6DS33_FIFO_STATUS2_ADDR 0x3B
#define LSM6DS33_FIFO_STATUS3_ADDR 0x3C
#define LSM6DS33_FIFO_STATUS4_ADDR 0x3D
#define LSM6DS33_FIFO_DATA_OUT_L_ADDR 0x3E
#define LSM6DS33_FIFO_DATA_OUT_H_ADDR 0x3F
#define LSM6DS33_TIMESTAMP0_REG_ADDR 0x40
#define LSM6DS33_TIMESTAMP1_REG_ADDR 0x41
#define LSM6DS33_TIMESTAMP2_REG_ADDR 0x42
#define LSM6DS33_STEP_TIMESTAMP_L_ADDR 0x49
#define LSM6DS33_STEP_TIMESTAMP_H_ADDR 0x4A
#define LSM6DS33_STEP_COUNTER_L_ADDR 0x4B
#define LSM6DS33_STEP_COUNTER_H_ADDR 0x4C
#define LSM6DS33_FUNC_SRC_ADDR 0x53
#define LSM6DS33_TAP_CFG_ADDR 0x58
#define LSM6DS33_TAP_THS_6D_ADDR 0x59
#define LSM6DS33_INT_DUR2_ADDR 0x5A
#define LSM6DS33_WAKE_UP_THS_ADDR 0x5B
#define LSM6DS33_WAKE_UP_DUR_ADDR 0x5C
#define LSM6DS33_FREE_FALL_ADDR 0x5D
#define LSM6DS33_MD1_CFG_ADDR 0x5E
#define LSM6DS33_MD2_CFG_ADDR 0x5F
#define LSM6DS33_ODR_0Hz 0x00
#define LSM6DS33_ODR_12_5Hz 0x01
#define LSM6DS33_ODR_26Hz 0x02
#define LSM6DS33_ODR_52Hz 0x03
#define LSM6DS33_ODR_104Hz 0x04
#define LSM6DS33_ODR_208Hz 0x05
#define LSM6DS33_ODR_416Hz 0x06
#define LSM6DS33_ODR_833Hz 0x07
#define LSM6DS33_ODR_1_66kHz 0x08
#define LSM6DS33_ODR_3_33kHz 0x09
#define LSM6DS33_ODR_6_66kHz 0x0A
#define LSM6DS33_LA_FS_2G 0x00
#define LSM6DS33_LA_FS_16G 0x01
#define LSM6DS33_LA_FS_4G 0x02
#define LSM6DS33_LA_FS_8G 0x03
#define LSM6DS33_LA_AABW_400Hz 0x00
#define LSM6DS33_LA_AABW_200Hz 0x01
#define LSM6DS33_LA_AABW_100Hz 0x02
#define LSM6DS33_LA_AABW_50Hz 0x03
#define LSM6DS33_GY_FS_125dps 0x01
#define LSM6DS33_GY_FS_250dps 0x00
#define LSM6DS33_GY_FS_500dps 0x02
#define LSM6DS33_GY_FS_1000dps 0x04
#define LSM6DS33_GY_FS_2000dps 0x05
typedef struct {
int16_t linacc_odr;
float linacc_mG; // milli G per Bit
int16_t linacc_aabw;
int16_t gyro_odr;
float gyro_mDPS; // milli degrees per second per Bit
} sIMUsettings;
typedef struct {
float fTemperatureDegC; //°C
float fAccX, fAccY, fAccZ; // m/s^2
float fGyroXradps, fGyroYradps, fGyroZradps; // rad/s
} sIMUoutput;
extern sIMUoutput IMUoutput;
bool imu_lsm6ds33_init(void);
bool imu_lsm6ds33_read();
+1 -1
View File
@@ -146,7 +146,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${TARGET_SRC_DIR}/battery_driver.cpp ${TARGET_SRC_DIR}/battery_driver.cpp
targets/common/arm/stm32/audio_dac_driver.cpp targets/common/arm/stm32/audio_dac_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp drivers/lsm6ds.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp targets/common/arm/stm32/stm32_pulse_driver.cpp
+12
View File
@@ -47,11 +47,13 @@
#include "sdcard.h" #include "sdcard.h"
#include "debug.h" #include "debug.h"
#include "keys.h" #include "keys.h"
#include "gyro.h"
#include "flysky_gimbal_driver.h" #include "flysky_gimbal_driver.h"
#include "timers_driver.h" #include "timers_driver.h"
#include "battery_driver.h" #include "battery_driver.h"
#include "drivers/lsm6ds.h"
#include <string.h> #include <string.h>
@@ -126,6 +128,14 @@ void boardBLInit()
flashRegisterDriver(QSPI_BASE, 8 * 1024 * 1024, &extflash_driver); flashRegisterDriver(QSPI_BASE, 8 * 1024 * 1024, &extflash_driver);
} }
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
bool pwrPressedDebounced() bool pwrPressedDebounced()
{ {
static bool debouncedState = 0; static bool debouncedState = 0;
@@ -377,6 +387,8 @@ void boardInit()
#if defined(RTCLOCK) #if defined(RTCLOCK)
rtcInit(); // RTC must be initialized before rambackupRestore() is called rtcInit(); // RTC must be initialized before rambackupRestore() is called
#endif #endif
gyroInit();
} }
extern void rtcDisableBackupReg(); extern void rtcDisableBackupReg();
-1
View File
@@ -35,7 +35,6 @@ set(SIMU_DRIVERS
module_drivers.cpp module_drivers.cpp
led_driver.cpp led_driver.cpp
backlight_driver.cpp backlight_driver.cpp
gyro_driver.cpp
bt_driver.cpp bt_driver.cpp
timers_driver.cpp timers_driver.cpp
abnormal_reboot.cpp abnormal_reboot.cpp
+1 -1
View File
@@ -149,7 +149,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${TARGET_SRC_DIR}/touch_driver.cpp ${TARGET_SRC_DIR}/touch_driver.cpp
${TARGET_SRC_DIR}/battery_driver.cpp ${TARGET_SRC_DIR}/battery_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp drivers/lsm6ds.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp targets/common/arm/stm32/stm32_pulse_driver.cpp
+12
View File
@@ -48,11 +48,13 @@
#include "sdcard.h" #include "sdcard.h"
#include "debug.h" #include "debug.h"
#include "keys.h" #include "keys.h"
#include "gyro.h"
#include "flysky_gimbal_driver.h" #include "flysky_gimbal_driver.h"
#include "timers_driver.h" #include "timers_driver.h"
#include "battery_driver.h" #include "battery_driver.h"
#include "drivers/lsm6ds.h"
#include "bitmapbuffer.h" #include "bitmapbuffer.h"
#include "colors.h" #include "colors.h"
@@ -173,6 +175,14 @@ void boardBLInit()
flashRegisterDriver(QSPI_BASE, 8 * 1024 * 1024, &extflash_driver); flashRegisterDriver(QSPI_BASE, 8 * 1024 * 1024, &extflash_driver);
} }
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
void boardInit() void boardInit()
{ {
// enable interrupts // enable interrupts
@@ -263,6 +273,8 @@ void boardInit()
#if defined(RTCLOCK) #if defined(RTCLOCK)
rtcInit(); // RTC must be initialized before rambackupRestore() is called rtcInit(); // RTC must be initialized before rambackupRestore() is called
#endif #endif
gyroInit();
} }
extern void rtcDisableBackupReg(); extern void rtcDisableBackupReg();
@@ -99,7 +99,6 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${TARGET_SRC_DIR}/tp_gt911.cpp ${TARGET_SRC_DIR}/tp_gt911.cpp
${TARGET_SRC_DIR}/audio_driver.cpp ${TARGET_SRC_DIR}/audio_driver.cpp
drivers/wm8904.cpp drivers/wm8904.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/stm32_switch_driver.cpp targets/common/arm/stm32/stm32_switch_driver.cpp
targets/common/arm/stm32/stm32_i2s.cpp targets/common/arm/stm32/stm32_i2s.cpp
@@ -40,6 +40,7 @@
#include "sdcard.h" #include "sdcard.h"
#include "touch.h" #include "touch.h"
#include "debug.h" #include "debug.h"
#include "gyro.h"
#include "timers_driver.h" #include "timers_driver.h"
#include "tp_gt911.h" #include "tp_gt911.h"
-1
View File
@@ -149,7 +149,6 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_SRC_DIR}/touch_driver.cpp ${BOARD_SRC_DIR}/touch_driver.cpp
targets/common/arm/stm32/audio_dac_driver.cpp targets/common/arm/stm32/audio_dac_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp targets/common/arm/stm32/stm32_pulse_driver.cpp
+1 -1
View File
@@ -627,7 +627,7 @@ if(USB_CHARGER)
endif() endif()
if(IMU) if(IMU)
target_sources(board PRIVATE targets/common/arm/stm32/lsm6ds_driver.cpp) target_sources(board PRIVATE drivers/lsm6ds.cpp)
endif() endif()
if(AUTOUPDATE) if(AUTOUPDATE)
+19
View File
@@ -125,6 +125,21 @@ void SDLEDpwrInit()
} }
#endif #endif
#if defined(IMU) && defined(IMU_I2C_BUS) && defined(IMU_I2C_BUS)
#include "drivers/lsm6ds.h"
#include "stm32_i2c_driver.h"
#define HAS_IMU
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
#endif
void boardInit() void boardInit()
{ {
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG);
@@ -277,6 +292,10 @@ void boardInit()
#if defined(RADIO_GX12) #if defined(RADIO_GX12)
gpio_init(HALL_SYNC, GPIO_OUT, GPIO_PIN_SPEED_LOW); gpio_init(HALL_SYNC, GPIO_OUT, GPIO_PIN_SPEED_LOW);
#endif #endif
#if defined(HAS_IMU)
gyroInit();
#endif
} }
#endif #endif
+4 -3
View File
@@ -45,6 +45,7 @@ endif()
# IMU support # IMU support
set(IMU ON) set(IMU ON)
add_definitions(-DIMU_ICM4207C) add_definitions(-DIMU_ICM4207C)
add_definitions(-DIMU_SC7U22)
# for size report script # for size report script
set(CPU_TYPE_FULL STM32H750xB) set(CPU_TYPE_FULL STM32H750xB)
@@ -144,6 +145,9 @@ set(BOARD_COMMON_SRC
${BOARD_SRC_DIR}/extflash_driver.cpp ${BOARD_SRC_DIR}/extflash_driver.cpp
${BOARD_SRC_DIR}/usb_charger_driver.cpp ${BOARD_SRC_DIR}/usb_charger_driver.cpp
${TARGET_SRC_DIR}/key_driver.cpp ${TARGET_SRC_DIR}/key_driver.cpp
drivers/pca95xx.cpp
drivers/icm42607C.cpp
drivers/sc7u22.cpp
targets/common/arm/stm32/abnormal_reboot.cpp targets/common/arm/stm32/abnormal_reboot.cpp
targets/common/arm/stm32/delays_driver.cpp targets/common/arm/stm32/delays_driver.cpp
targets/common/arm/stm32/dma2d.cpp targets/common/arm/stm32/dma2d.cpp
@@ -152,7 +156,6 @@ set(BOARD_COMMON_SRC
targets/common/arm/stm32/rotary_encoder_driver.cpp targets/common/arm/stm32/rotary_encoder_driver.cpp
targets/common/arm/stm32/rtc_driver.cpp targets/common/arm/stm32/rtc_driver.cpp
targets/common/arm/stm32/watchdog_driver.cpp targets/common/arm/stm32/watchdog_driver.cpp
drivers/pca95xx.cpp
) )
if(BLUETOOTH) if(BLUETOOTH)
@@ -186,8 +189,6 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_COMMON_SRC} ${BOARD_COMMON_SRC}
${BOARD_SRC_DIR}/tp_gt911.cpp ${BOARD_SRC_DIR}/tp_gt911.cpp
${BOARD_SRC_DIR}/audio_driver.cpp ${BOARD_SRC_DIR}/audio_driver.cpp
drivers/icm42607C.cpp
drivers/imu_filter.cpp
drivers/tas2505.cpp drivers/tas2505.cpp
targets/common/arm/stm32/heartbeat_driver.cpp targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp
+1 -1
View File
@@ -382,8 +382,8 @@ TIM17: ROTARY_ENCODER_TIMER
// IMU // IMU
#define IMU_I2C_BUS I2C_Bus_1 #define IMU_I2C_BUS I2C_Bus_1
#define IMU_I2C_ADDRESS 0x6A
#define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13 #define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13
// IMU_INT_EXTI IRQ // IMU_INT_EXTI IRQ
#if !defined(USE_EXTI15_10_IRQ) #if !defined(USE_EXTI15_10_IRQ)
#define USE_EXTI15_10_IRQ #define USE_EXTI15_10_IRQ
+3 -2
View File
@@ -36,6 +36,7 @@ set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS")
# IMU support # IMU support
set(IMU ON) set(IMU ON)
add_definitions(-DIMU_ICM4207C) add_definitions(-DIMU_ICM4207C)
add_definitions(-DIMU_SC7U22)
# for size report script # for size report script
set(CPU_TYPE_FULL STM32H750xB) set(CPU_TYPE_FULL STM32H750xB)
@@ -172,9 +173,9 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_SRC_DIR}/tp_gt911.cpp ${BOARD_SRC_DIR}/tp_gt911.cpp
${BOARD_SRC_DIR}/audio_driver.cpp ${BOARD_SRC_DIR}/audio_driver.cpp
${BOARD_SRC_DIR}/luminosity_sensor.cpp ${BOARD_SRC_DIR}/luminosity_sensor.cpp
drivers/icm42607C.cpp
drivers/imu_filter.cpp
drivers/tas2505.cpp drivers/tas2505.cpp
drivers/icm42607C.cpp
drivers/sc7u22.cpp
targets/common/arm/stm32/heartbeat_driver.cpp targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp targets/common/arm/stm32/module_timer_driver.cpp
+1 -2
View File
@@ -572,8 +572,7 @@ USART6: INTMODULE_USART
#endif #endif
#define IMU_I2C_BUS I2C_Bus_2 #define IMU_I2C_BUS I2C_Bus_2
#define IMU_I2C_ADDRESS 0x6A #define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13
#define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13
#if !defined(USE_EXTI15_10_IRQ) #if !defined(USE_EXTI15_10_IRQ)
#define USE_EXTI15_10_IRQ #define USE_EXTI15_10_IRQ
#define EXTI15_10_IRQ_Priority 6 #define EXTI15_10_IRQ_Priority 6
+1 -5
View File
@@ -128,7 +128,7 @@ constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/;
void execMixerFrequentActions() void execMixerFrequentActions()
{ {
#if defined(IMU) #if defined(IMU)
gyro.wakeup(); gyroWakeup();
#endif #endif
#if defined(BLUETOOTH) #if defined(BLUETOOTH)
@@ -138,10 +138,6 @@ void execMixerFrequentActions()
void mixerTask() void mixerTask()
{ {
#if defined(IMU)
gyroInit();
#endif
while (task_running()) { while (task_running()) {
int timeout = 0; int timeout = 0;
+1
View File
@@ -1202,6 +1202,7 @@
#define TR_ALTSENSOR "Alt 传感器" #define TR_ALTSENSOR "Alt 传感器"
#define TR_CELLSENSOR "Cell 传感器" #define TR_CELLSENSOR "Cell 传感器"
#define TR_GPSSENSOR "GPS 传感器" #define TR_GPSSENSOR "GPS 传感器"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "传感器" #define TR_CURRENTSENSOR "传感器"
#define TR_AUTOOFFSET "自动偏移值" #define TR_AUTOOFFSET "自动偏移值"
#define TR_ONLYPOSITIVE "正向" #define TR_ONLYPOSITIVE "正向"
+1
View File
@@ -1205,6 +1205,7 @@
#define TR_ALTSENSOR "Senzor výšky" #define TR_ALTSENSOR "Senzor výšky"
#define TR_CELLSENSOR "Senzor článků" #define TR_CELLSENSOR "Senzor článků"
#define TR_GPSSENSOR "GPS senzor" #define TR_GPSSENSOR "GPS senzor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Senzor" #define TR_CURRENTSENSOR "Senzor"
#define TR_AUTOOFFSET "Auto offset" #define TR_AUTOOFFSET "Auto offset"
#define TR_ONLYPOSITIVE "Jen kladné" #define TR_ONLYPOSITIVE "Jen kladné"
+1
View File
@@ -1207,6 +1207,7 @@
#define TR_ALTSENSOR "Højde sensor" #define TR_ALTSENSOR "Højde sensor"
#define TR_CELLSENSOR "Cell sensor" #define TR_CELLSENSOR "Cell sensor"
#define TR_GPSSENSOR "GPS sensor" #define TR_GPSSENSOR "GPS sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto offset" #define TR_AUTOOFFSET "Auto offset"
#define TR_ONLYPOSITIVE "Positive" #define TR_ONLYPOSITIVE "Positive"
+1
View File
@@ -1201,6 +1201,7 @@
#define TR_ALTSENSOR "Höhen Sensor" #define TR_ALTSENSOR "Höhen Sensor"
#define TR_CELLSENSOR "Zellen Sensor" #define TR_CELLSENSOR "Zellen Sensor"
#define TR_GPSSENSOR "GPS Sensor" #define TR_GPSSENSOR "GPS Sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto Offset" #define TR_AUTOOFFSET "Auto Offset"
#define TR_ONLYPOSITIVE "Nur Positiv" #define TR_ONLYPOSITIVE "Nur Positiv"
+1
View File
@@ -1203,6 +1203,7 @@
#define TR_ALTSENSOR "Alt sensor" #define TR_ALTSENSOR "Alt sensor"
#define TR_CELLSENSOR "Cell sensor" #define TR_CELLSENSOR "Cell sensor"
#define TR_GPSSENSOR "GPS sensor" #define TR_GPSSENSOR "GPS sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto Offset" #define TR_AUTOOFFSET "Auto Offset"
#define TR_ONLYPOSITIVE "Positive" #define TR_ONLYPOSITIVE "Positive"
+1
View File
@@ -1203,6 +1203,7 @@
#define TR_ALTSENSOR "Alt sensor" #define TR_ALTSENSOR "Alt sensor"
#define TR_CELLSENSOR "Cell sensor" #define TR_CELLSENSOR "Cell sensor"
#define TR_GPSSENSOR "GPS sensor" #define TR_GPSSENSOR "GPS sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto offset" #define TR_AUTOOFFSET "Auto offset"
#define TR_ONLYPOSITIVE "Positivo" #define TR_ONLYPOSITIVE "Positivo"
+1
View File
@@ -1202,6 +1202,7 @@
#define TR_ALTSENSOR "Alt sensor" #define TR_ALTSENSOR "Alt sensor"
#define TR_CELLSENSOR "Cell sensor" #define TR_CELLSENSOR "Cell sensor"
#define TR_GPSSENSOR "GPS sensor" #define TR_GPSSENSOR "GPS sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto Offset" #define TR_AUTOOFFSET "Auto Offset"
#define TR_ONLYPOSITIVE "Positive" #define TR_ONLYPOSITIVE "Positive"
+1
View File
@@ -1205,6 +1205,7 @@
#define TR_ALTSENSOR "Capteur Alt." #define TR_ALTSENSOR "Capteur Alt."
#define TR_CELLSENSOR "Capteur Elém." #define TR_CELLSENSOR "Capteur Elém."
#define TR_GPSSENSOR "Capteur GPS" #define TR_GPSSENSOR "Capteur GPS"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Capteur" #define TR_CURRENTSENSOR "Capteur"
#define TR_AUTOOFFSET "Offset auto" #define TR_AUTOOFFSET "Offset auto"
#define TR_ONLYPOSITIVE "Positif" #define TR_ONLYPOSITIVE "Positif"
+1
View File
@@ -1205,6 +1205,7 @@
#define TR_ALTSENSOR "Alt sensor" #define TR_ALTSENSOR "Alt sensor"
#define TR_CELLSENSOR "Cell sensor" #define TR_CELLSENSOR "Cell sensor"
#define TR_GPSSENSOR "GPS sensor" #define TR_GPSSENSOR "GPS sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "חיישן" #define TR_CURRENTSENSOR "חיישן"
#define TR_AUTOOFFSET "Auto Offset" #define TR_AUTOOFFSET "Auto Offset"
#define TR_ONLYPOSITIVE "חיובי" #define TR_ONLYPOSITIVE "חיובי"
+1
View File
@@ -1202,6 +1202,7 @@
#define TR_ALTSENSOR "Sensore Alt" #define TR_ALTSENSOR "Sensore Alt"
#define TR_CELLSENSOR "Sensore Cell" #define TR_CELLSENSOR "Sensore Cell"
#define TR_GPSSENSOR "Sensore GPS" #define TR_GPSSENSOR "Sensore GPS"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensore" #define TR_CURRENTSENSOR "Sensore"
#define TR_AUTOOFFSET "Offset Auto" #define TR_AUTOOFFSET "Offset Auto"
#define TR_ONLYPOSITIVE "Positivo" #define TR_ONLYPOSITIVE "Positivo"
+1
View File
@@ -1201,6 +1201,7 @@
#define TR_ALTSENSOR "Altセンサー" #define TR_ALTSENSOR "Altセンサー"
#define TR_CELLSENSOR "セルセンサー" #define TR_CELLSENSOR "セルセンサー"
#define TR_GPSSENSOR "GPSセンサー" #define TR_GPSSENSOR "GPSセンサー"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "センサー" #define TR_CURRENTSENSOR "センサー"
#define TR_AUTOOFFSET "自動オフセット" #define TR_AUTOOFFSET "自動オフセット"
#define TR_ONLYPOSITIVE "ポジティブ" #define TR_ONLYPOSITIVE "ポジティブ"
+1
View File
@@ -1248,6 +1248,7 @@
#define TR_ALTSENSOR "고도 센서" #define TR_ALTSENSOR "고도 센서"
#define TR_CELLSENSOR "셀 전압 센서" #define TR_CELLSENSOR "셀 전압 센서"
#define TR_GPSSENSOR "GPS 센서" #define TR_GPSSENSOR "GPS 센서"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "전류 센서" #define TR_CURRENTSENSOR "전류 센서"
#define TR_AUTOOFFSET "자동 오프셋" #define TR_AUTOOFFSET "자동 오프셋"
#define TR_ONLYPOSITIVE "양수만" #define TR_ONLYPOSITIVE "양수만"
+1
View File
@@ -1203,6 +1203,7 @@
#define TR_ALTSENSOR "Hoogte Sensor" #define TR_ALTSENSOR "Hoogte Sensor"
#define TR_CELLSENSOR "Cellen Sensor" #define TR_CELLSENSOR "Cellen Sensor"
#define TR_GPSSENSOR "GPS Sensor" #define TR_GPSSENSOR "GPS Sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Stroomsensor" #define TR_CURRENTSENSOR "Stroomsensor"
#define TR_AUTOOFFSET "Auto Offset" #define TR_AUTOOFFSET "Auto Offset"
#define TR_ONLYPOSITIVE "Enkel positief" #define TR_ONLYPOSITIVE "Enkel positief"
+1
View File
@@ -1200,6 +1200,7 @@
#define TR_ALTSENSOR "Sensor wys." #define TR_ALTSENSOR "Sensor wys."
#define TR_CELLSENSOR "Sensor Cell" #define TR_CELLSENSOR "Sensor Cell"
#define TR_GPSSENSOR "Sensor GPS" #define TR_GPSSENSOR "Sensor GPS"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto Ofset" #define TR_AUTOOFFSET "Auto Ofset"
#define TR_ONLYPOSITIVE "Dodatni" #define TR_ONLYPOSITIVE "Dodatni"
+1
View File
@@ -1203,6 +1203,7 @@
#define TR_ALTSENSOR "Alt sensor" #define TR_ALTSENSOR "Alt sensor"
#define TR_CELLSENSOR "Cell sensor" #define TR_CELLSENSOR "Cell sensor"
#define TR_GPSSENSOR "GPS sensor" #define TR_GPSSENSOR "GPS sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto Offset" #define TR_AUTOOFFSET "Auto Offset"
#define TR_ONLYPOSITIVE "Positive" #define TR_ONLYPOSITIVE "Positive"
+1
View File
@@ -1204,6 +1204,7 @@
#define TR_ALTSENSOR "Датчик высот" #define TR_ALTSENSOR "Датчик высот"
#define TR_CELLSENSOR "Датчик АКБ" #define TR_CELLSENSOR "Датчик АКБ"
#define TR_GPSSENSOR "Датчик GPS" #define TR_GPSSENSOR "Датчик GPS"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Датчик тока" #define TR_CURRENTSENSOR "Датчик тока"
#define TR_AUTOOFFSET "Авто смещ" #define TR_AUTOOFFSET "Авто смещ"
#define TR_ONLYPOSITIVE "Положител" #define TR_ONLYPOSITIVE "Положител"
+1
View File
@@ -1213,6 +1213,7 @@
#define TR_ALTSENSOR "Höjdsensor" #define TR_ALTSENSOR "Höjdsensor"
#define TR_CELLSENSOR "Cellsensor" #define TR_CELLSENSOR "Cellsensor"
#define TR_GPSSENSOR "GPS-sensor" #define TR_GPSSENSOR "GPS-sensor"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Sensor" #define TR_CURRENTSENSOR "Sensor"
#define TR_AUTOOFFSET "Auto offset" #define TR_AUTOOFFSET "Auto offset"
#define TR_ONLYPOSITIVE "Positiv" #define TR_ONLYPOSITIVE "Positiv"
+1
View File
@@ -1200,6 +1200,7 @@
#define TR_ALTSENSOR "Alt 傳感器" #define TR_ALTSENSOR "Alt 傳感器"
#define TR_CELLSENSOR "Cell 傳感器" #define TR_CELLSENSOR "Cell 傳感器"
#define TR_GPSSENSOR "GPS 傳感器" #define TR_GPSSENSOR "GPS 傳感器"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "傳感器" #define TR_CURRENTSENSOR "傳感器"
#define TR_AUTOOFFSET "自動偏移值" #define TR_AUTOOFFSET "自動偏移值"
#define TR_ONLYPOSITIVE "正向" #define TR_ONLYPOSITIVE "正向"
+1
View File
@@ -1203,6 +1203,7 @@
#define TR_ALTSENSOR "Сенсор висоти" #define TR_ALTSENSOR "Сенсор висоти"
#define TR_CELLSENSOR "Сенсор комірки" #define TR_CELLSENSOR "Сенсор комірки"
#define TR_GPSSENSOR "GPS сенсор" #define TR_GPSSENSOR "GPS сенсор"
#define TR_GYRO "Gyro"
#define TR_CURRENTSENSOR "Сенсор" #define TR_CURRENTSENSOR "Сенсор"
#define TR_AUTOOFFSET "Авто зсув" #define TR_AUTOOFFSET "Авто зсув"
#define TR_ONLYPOSITIVE "Позитив" #define TR_ONLYPOSITIVE "Позитив"
+1
View File
@@ -586,6 +586,7 @@
#define STR_GPS_COORDS_FORMAT currentLangStrings->STR_GPS_COORDS_FORMAT #define STR_GPS_COORDS_FORMAT currentLangStrings->STR_GPS_COORDS_FORMAT
#define STR_GPS currentLangStrings->STR_GPS #define STR_GPS currentLangStrings->STR_GPS
#define STR_GPSSENSOR currentLangStrings->STR_GPSSENSOR #define STR_GPSSENSOR currentLangStrings->STR_GPSSENSOR
#define STR_GYRO currentLangStrings->STR_GYRO
#define STR_GV currentLangStrings->STR_GV #define STR_GV currentLangStrings->STR_GV
#define STR_GVARS currentLangStrings->STR_GVARS #define STR_GVARS currentLangStrings->STR_GVARS
#define STR_HARDWARE currentLangStrings->STR_HARDWARE #define STR_HARDWARE currentLangStrings->STR_HARDWARE
+1
View File
@@ -585,6 +585,7 @@ STR(GLOBALVAR)
STR(GPS_COORDS_FORMAT) STR(GPS_COORDS_FORMAT)
STR(GPS) STR(GPS)
STR(GPSSENSOR) STR(GPSSENSOR)
STR(GYRO)
STR(GV) STR(GV)
STR(GVARS) STR(GVARS)
STR(HARDWARE) STR(HARDWARE)