mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 22:35:23 +03:00
Remove reset facility
This commit is contained in:
parent
9caeceb2dd
commit
58647e678f
10 changed files with 0 additions and 65 deletions
|
@ -96,7 +96,6 @@ typedef struct gyroDev_s {
|
||||||
int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT];
|
int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT];
|
||||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||||
int16_t temperature;
|
int16_t temperature;
|
||||||
mpuConfiguration_t mpuConfiguration;
|
|
||||||
mpuDetectionResult_t mpuDetectionResult;
|
mpuDetectionResult_t mpuDetectionResult;
|
||||||
sensor_align_e gyroAlign;
|
sensor_align_e gyroAlign;
|
||||||
gyroRateKHz_e gyroRateKHz;
|
gyroRateKHz_e gyroRateKHz;
|
||||||
|
|
|
@ -54,8 +54,6 @@
|
||||||
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
|
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu.h"
|
#include "drivers/accgyro/accgyro_mpu.h"
|
||||||
|
|
||||||
mpuResetFnPtr mpuResetFn;
|
|
||||||
|
|
||||||
#ifndef MPU_I2C_INSTANCE
|
#ifndef MPU_I2C_INSTANCE
|
||||||
#define MPU_I2C_INSTANCE I2C_DEVICE
|
#define MPU_I2C_INSTANCE I2C_DEVICE
|
||||||
#endif
|
#endif
|
||||||
|
@ -250,7 +248,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
sensor = mpu9250SpiDetect(&gyro->bus);
|
sensor = mpu9250SpiDetect(&gyro->bus);
|
||||||
if (sensor != MPU_NONE) {
|
if (sensor != MPU_NONE) {
|
||||||
gyro->mpuDetectionResult.sensor = sensor;
|
gyro->mpuDetectionResult.sensor = sensor;
|
||||||
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -140,14 +140,6 @@
|
||||||
// RF = Register Flag
|
// RF = Register Flag
|
||||||
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
||||||
|
|
||||||
typedef void (*mpuResetFnPtr)(void);
|
|
||||||
|
|
||||||
extern mpuResetFnPtr mpuResetFn;
|
|
||||||
|
|
||||||
typedef struct mpuConfiguration_s {
|
|
||||||
mpuResetFnPtr resetFn;
|
|
||||||
} mpuConfiguration_t;
|
|
||||||
|
|
||||||
enum gyro_fsr_e {
|
enum gyro_fsr_e {
|
||||||
INV_FSR_250DPS = 0,
|
INV_FSR_250DPS = 0,
|
||||||
INV_FSR_500DPS,
|
INV_FSR_500DPS,
|
||||||
|
|
|
@ -75,19 +75,6 @@ static bool mpu9250SpiSlowReadRegisterBuffer(const busDevice_t *bus, uint8_t reg
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void mpu9250SpiResetGyro(void)
|
|
||||||
{
|
|
||||||
#if 0
|
|
||||||
// XXX This doesn't work. Need a proper busDevice_t.
|
|
||||||
// Device Reset
|
|
||||||
#ifdef MPU9250_CS_PIN
|
|
||||||
busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } };
|
|
||||||
mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
|
||||||
delay(150);
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void mpu9250SpiGyroInit(gyroDev_t *gyro)
|
void mpu9250SpiGyroInit(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
mpuGyroInit(gyro);
|
mpuGyroInit(gyro);
|
||||||
|
|
|
@ -34,10 +34,6 @@ void SetSysClock(void);
|
||||||
|
|
||||||
void systemReset(void)
|
void systemReset(void)
|
||||||
{
|
{
|
||||||
if (mpuResetFn) {
|
|
||||||
mpuResetFn();
|
|
||||||
}
|
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
NVIC_SystemReset();
|
NVIC_SystemReset();
|
||||||
}
|
}
|
||||||
|
@ -47,10 +43,6 @@ PERSISTENT uint32_t bootloaderRequest = 0;
|
||||||
|
|
||||||
void systemResetToBootloader(void)
|
void systemResetToBootloader(void)
|
||||||
{
|
{
|
||||||
if (mpuResetFn) {
|
|
||||||
mpuResetFn();
|
|
||||||
}
|
|
||||||
|
|
||||||
bootloaderRequest = BOOTLOADER_REQUEST_COOKIE;
|
bootloaderRequest = BOOTLOADER_REQUEST_COOKIE;
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
|
|
|
@ -38,20 +38,12 @@ void SystemClock_Config(void);
|
||||||
|
|
||||||
void systemReset(void)
|
void systemReset(void)
|
||||||
{
|
{
|
||||||
if (mpuResetFn) {
|
|
||||||
mpuResetFn();
|
|
||||||
}
|
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
NVIC_SystemReset();
|
NVIC_SystemReset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void systemResetToBootloader(void)
|
void systemResetToBootloader(void)
|
||||||
{
|
{
|
||||||
if (mpuResetFn) {
|
|
||||||
mpuResetFn();
|
|
||||||
}
|
|
||||||
|
|
||||||
(*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot
|
(*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
|
|
|
@ -147,10 +147,6 @@ void mscWaitForButton(void)
|
||||||
|
|
||||||
void systemResetToMsc(void)
|
void systemResetToMsc(void)
|
||||||
{
|
{
|
||||||
if (mpuResetFn) {
|
|
||||||
mpuResetFn();
|
|
||||||
}
|
|
||||||
|
|
||||||
*((uint32_t *)0x2001FFF0) = MSC_MAGIC;
|
*((uint32_t *)0x2001FFF0) = MSC_MAGIC;
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
|
|
|
@ -153,10 +153,6 @@ void mscWaitForButton(void)
|
||||||
|
|
||||||
void systemResetToMsc(void)
|
void systemResetToMsc(void)
|
||||||
{
|
{
|
||||||
if (mpuResetFn) {
|
|
||||||
mpuResetFn();
|
|
||||||
}
|
|
||||||
|
|
||||||
*((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
|
*((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
|
|
|
@ -241,19 +241,6 @@ const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor)
|
||||||
}
|
}
|
||||||
#endif // USE_GYRO_REGISTER_DUMP
|
#endif // USE_GYRO_REGISTER_DUMP
|
||||||
|
|
||||||
const mpuConfiguration_t *gyroMpuConfiguration(void)
|
|
||||||
{
|
|
||||||
#ifdef USE_DUAL_GYRO
|
|
||||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
|
|
||||||
return &gyroSensor2.gyroDev.mpuConfiguration;
|
|
||||||
} else {
|
|
||||||
return &gyroSensor1.gyroDev.mpuConfiguration;
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
return &gyroSensor1.gyroDev.mpuConfiguration;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
const mpuDetectionResult_t *gyroMpuDetectionResult(void)
|
const mpuDetectionResult_t *gyroMpuDetectionResult(void)
|
||||||
{
|
{
|
||||||
#ifdef USE_DUAL_GYRO
|
#ifdef USE_DUAL_GYRO
|
||||||
|
@ -447,7 +434,6 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
|
||||||
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
|
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
|
||||||
|
|
||||||
mpuDetect(&gyroSensor->gyroDev);
|
mpuDetect(&gyroSensor->gyroDev);
|
||||||
mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
|
const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
|
||||||
|
|
|
@ -105,8 +105,6 @@ void gyroInitFilters(void);
|
||||||
void gyroUpdate(timeUs_t currentTimeUs);
|
void gyroUpdate(timeUs_t currentTimeUs);
|
||||||
bool gyroGetAccumulationAverage(float *accumulation);
|
bool gyroGetAccumulationAverage(float *accumulation);
|
||||||
const busDevice_t *gyroSensorBus(void);
|
const busDevice_t *gyroSensorBus(void);
|
||||||
struct mpuConfiguration_s;
|
|
||||||
const struct mpuConfiguration_s *gyroMpuConfiguration(void);
|
|
||||||
struct mpuDetectionResult_s;
|
struct mpuDetectionResult_s;
|
||||||
const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
|
const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
|
||||||
void gyroStartCalibration(bool isFirstArmingCalibration);
|
void gyroStartCalibration(bool isFirstArmingCalibration);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue