1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00

Fix eeprom writing. Update some clocks. Add fake implementation of acc

and gyro to allow the main loop to run.
This commit is contained in:
Dominic Clifton 2014-04-25 03:50:33 +01:00
parent f9ceb0c40f
commit cde26ac1a2
5 changed files with 47 additions and 7 deletions

View file

@ -144,7 +144,6 @@ void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex)
void writeEEPROM(void) void writeEEPROM(void)
{ {
#ifndef STM32F3DISCOVERY
FLASH_Status status = 0; FLASH_Status status = 0;
uint32_t wordOffset; uint32_t wordOffset;
int8_t attemptsRemaining = 3; int8_t attemptsRemaining = 3;
@ -160,8 +159,12 @@ void writeEEPROM(void)
// write it // write it
FLASH_Unlock(); FLASH_Unlock();
while (attemptsRemaining--) { while (attemptsRemaining--) {
#ifdef STM32F3DISCOVERY
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
#endif
#ifdef STM32F10X_MD
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
#endif
status = FLASH_ErasePage(FLASH_WRITE_ADDR); status = FLASH_ErasePage(FLASH_WRITE_ADDR);
for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) { for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&masterConfig + wordOffset)); status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&masterConfig + wordOffset));
@ -176,7 +179,6 @@ void writeEEPROM(void)
if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
failureMode(10); failureMode(10);
} }
#endif
} }
void ensureEEPROMContainsValidData(void) void ensureEEPROMContainsValidData(void)

View file

@ -100,13 +100,13 @@ void systemInit(bool overclock)
#ifdef STM32F303xC #ifdef STM32F303xC
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 | RCC_APB2Periph_USART1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 | RCC_APB2Periph_USART1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | RCC_AHBPeriph_GPIOC | RCC_AHBPeriph_ADC12, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | RCC_AHBPeriph_GPIOC | RCC_AHBPeriph_ADC12, ENABLE);
#endif #endif
#ifdef STM32F10X_MD #ifdef STM32F10X_MD
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1, ENABLE);
#endif
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
#endif
RCC_ClearFlag(); RCC_ClearFlag();
// Make all GPIO in by default to save power and reduce noise // Make all GPIO in by default to save power and reduce noise

View file

@ -7,7 +7,8 @@ typedef enum AccelSensors {
ACC_MPU6050 = 2, ACC_MPU6050 = 2,
ACC_MMA8452 = 3, ACC_MMA8452 = 3,
ACC_BMA280 = 4, ACC_BMA280 = 4,
ACC_NONE = 5 ACC_FAKE = 5,
ACC_NONE = 6
} AccelSensors; } AccelSensors;
extern uint8_t accHardware; extern uint8_t accHardware;

View file

@ -43,6 +43,30 @@ extern gyro_t gyro;
extern baro_t baro; extern baro_t baro;
extern acc_t acc; extern acc_t acc;
static void fakeGyroInit(void) {}
static void fakeGyroRead(int16_t *gyroData) {}
static void fakeGyroReadTemp(int16_t *tempData) {}
bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
{
gyro->init = fakeGyroInit;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
return true;
}
static void fakeAccInit(void) {}
static void fakeAccRead(int16_t *gyroData) {}
bool fakeAccDetect(acc_t *acc)
{
acc->init = fakeAccInit;
acc->read = fakeAccRead;
acc->revisionCode = 0;
return true;
}
#ifdef FY90Q #ifdef FY90Q
// FY90Q analog gyro/acc // FY90Q analog gyro/acc
void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig)
@ -69,6 +93,10 @@ void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
gyroAlign = CW0_DEG; gyroAlign = CW0_DEG;
} else if (mpu3050Detect(&gyro, gyroLpf)) { } else if (mpu3050Detect(&gyro, gyroLpf)) {
gyroAlign = CW0_DEG; gyroAlign = CW0_DEG;
#ifdef STM32F3DISCOVERY
} else if (fakeGyroDetect(&gyro, gyroLpf)) {
gyroAlign = ALIGN_DEFAULT;
#endif
} else { } else {
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
failureMode(3); failureMode(3);
@ -116,6 +144,15 @@ retry:
if (accHardwareToUse == ACC_BMA280) if (accHardwareToUse == ACC_BMA280)
break; break;
} }
#endif
#ifdef STM32F3DISCOVERY
default:
if (fakeAccDetect(&acc)) {
accHardware = ACC_BMA280;
accAlign = CW0_DEG; //
if (accHardwareToUse == ACC_FAKE)
break;
}
#endif #endif
} }

View file

@ -92,7 +92,7 @@ static const char * const sensorNames[] = {
}; };
static const char * const accNames[] = { static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "None", NULL "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "FAKE", "None", NULL
}; };
typedef struct { typedef struct {