Merge pull request #602 from martinbudden/bf_gyro_tidy
gyro sync and initialisation cleanup
This commit is contained in:
commit
9843d7ce2f
|
@ -325,9 +325,6 @@ extern uint32_t currentTime;
|
||||||
//From rx.c:
|
//From rx.c:
|
||||||
extern uint16_t rssi;
|
extern uint16_t rssi;
|
||||||
|
|
||||||
//From gyro.c
|
|
||||||
extern uint32_t targetLooptime;
|
|
||||||
|
|
||||||
//From rc_controls.c
|
//From rc_controls.c
|
||||||
extern uint32_t rcModeActivationMask;
|
extern uint32_t rcModeActivationMask;
|
||||||
|
|
||||||
|
@ -1169,7 +1166,7 @@ static bool blackboxWriteSysinfo()
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", targetLooptime);
|
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
|
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
|
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8);
|
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8);
|
||||||
|
|
|
@ -22,7 +22,6 @@
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/light_led.h"
|
#include "drivers/light_led.h"
|
||||||
#include "drivers/sound_beeper.h"
|
#include "drivers/sound_beeper.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "sensors/boardalignment.h"
|
#include "sensors/boardalignment.h"
|
||||||
|
|
|
@ -38,7 +38,6 @@
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/pwm_output.h"
|
#include "drivers/pwm_output.h"
|
||||||
#include "drivers/max7456.h"
|
#include "drivers/max7456.h"
|
||||||
|
|
||||||
|
@ -751,7 +750,7 @@ void activateConfig(void)
|
||||||
¤tProfile->pidProfile
|
¤tProfile->pidProfile
|
||||||
);
|
);
|
||||||
|
|
||||||
useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
|
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
|
||||||
|
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
telemetryUseConfig(&masterConfig.telemetryConfig);
|
telemetryUseConfig(&masterConfig.telemetryConfig);
|
||||||
|
|
|
@ -27,6 +27,7 @@ typedef struct gyro_s {
|
||||||
sensorReadFuncPtr temperature; // read temperature if available
|
sensorReadFuncPtr temperature; // read temperature if available
|
||||||
sensorInterruptFuncPtr intStatus;
|
sensorInterruptFuncPtr intStatus;
|
||||||
float scale; // scalefactor
|
float scale; // scalefactor
|
||||||
|
uint32_t targetLooptime;
|
||||||
} gyro_t;
|
} gyro_t;
|
||||||
|
|
||||||
typedef struct acc_s {
|
typedef struct acc_s {
|
||||||
|
|
|
@ -32,7 +32,6 @@
|
||||||
#include "gpio.h"
|
#include "gpio.h"
|
||||||
#include "exti.h"
|
#include "exti.h"
|
||||||
#include "bus_i2c.h"
|
#include "bus_i2c.h"
|
||||||
#include "gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensor.h"
|
#include "sensor.h"
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
|
@ -300,11 +299,14 @@ bool mpuGyroRead(int16_t *gyroADC)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void checkMPUDataReady(bool *mpuDataReadyPtr) {
|
bool checkMPUDataReady(void)
|
||||||
|
{
|
||||||
|
bool ret;
|
||||||
if (mpuDataReady) {
|
if (mpuDataReady) {
|
||||||
*mpuDataReadyPtr = true;
|
ret = true;
|
||||||
mpuDataReady= false;
|
mpuDataReady= false;
|
||||||
} else {
|
} else {
|
||||||
*mpuDataReadyPtr = false;
|
ret = false;
|
||||||
}
|
}
|
||||||
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
|
@ -185,4 +185,4 @@ void mpuIntExtiInit(void);
|
||||||
bool mpuAccRead(int16_t *accData);
|
bool mpuAccRead(int16_t *accData);
|
||||||
bool mpuGyroRead(int16_t *gyroADC);
|
bool mpuGyroRead(int16_t *gyroADC);
|
||||||
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
|
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
|
||||||
void checkMPUDataReady(bool *mpuDataReadyPtr);
|
bool checkMPUDataReady(void);
|
||||||
|
|
|
@ -30,7 +30,6 @@
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
#include "accgyro_mpu.h"
|
#include "accgyro_mpu.h"
|
||||||
#include "accgyro_mpu3050.h"
|
#include "accgyro_mpu3050.h"
|
||||||
#include "gyro_sync.h"
|
|
||||||
|
|
||||||
// MPU3050, Standard address 0x68
|
// MPU3050, Standard address 0x68
|
||||||
#define MPU3050_ADDRESS 0x68
|
#define MPU3050_ADDRESS 0x68
|
||||||
|
|
|
@ -1,4 +1,4 @@
|
||||||
/*
|
/*
|
||||||
* This file is part of Cleanflight.
|
* This file is part of Cleanflight.
|
||||||
*
|
*
|
||||||
* Cleanflight is free software: you can redistribute it and/or modify
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
|
|
@ -4,48 +4,40 @@
|
||||||
* Created on: 3 aug. 2015
|
* Created on: 3 aug. 2015
|
||||||
* Author: borisb
|
* Author: borisb
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdlib.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "build_config.h"
|
#include "build_config.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
|
||||||
#include "common/maths.h"
|
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/gyro_sync.h"
|
#include "drivers/gyro_sync.h"
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
|
||||||
#include "sensors/acceleration.h"
|
|
||||||
|
|
||||||
#include "config/runtime_config.h"
|
|
||||||
#include "config/config.h"
|
|
||||||
|
|
||||||
extern gyro_t gyro;
|
|
||||||
|
|
||||||
uint32_t targetLooptime;
|
|
||||||
static uint8_t mpuDividerDrops;
|
static uint8_t mpuDividerDrops;
|
||||||
|
|
||||||
bool getMpuDataStatus(gyro_t *gyro)
|
bool gyroSyncCheckUpdate(const gyro_t *gyro)
|
||||||
{
|
{
|
||||||
bool mpuDataStatus;
|
|
||||||
if (!gyro->intStatus)
|
if (!gyro->intStatus)
|
||||||
return false;
|
return false;
|
||||||
gyro->intStatus(&mpuDataStatus);
|
return gyro->intStatus();
|
||||||
return mpuDataStatus;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool gyroSyncCheckUpdate(void) {
|
#define GYRO_LPF_256HZ 0
|
||||||
return getMpuDataStatus(&gyro);
|
#define GYRO_LPF_188HZ 1
|
||||||
}
|
#define GYRO_LPF_98HZ 2
|
||||||
|
#define GYRO_LPF_42HZ 3
|
||||||
|
#define GYRO_LPF_20HZ 4
|
||||||
|
#define GYRO_LPF_10HZ 5
|
||||||
|
#define GYRO_LPF_5HZ 6
|
||||||
|
#define GYRO_LPF_NONE 7
|
||||||
|
|
||||||
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
|
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
|
||||||
|
{
|
||||||
int gyroSamplePeriod;
|
int gyroSamplePeriod;
|
||||||
|
|
||||||
if (!lpf || lpf == 7) {
|
if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
|
||||||
gyroSamplePeriod = 125;
|
gyroSamplePeriod = 125;
|
||||||
} else {
|
} else {
|
||||||
gyroSamplePeriod = 1000;
|
gyroSamplePeriod = 1000;
|
||||||
|
@ -54,9 +46,11 @@ void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
|
||||||
|
|
||||||
// calculate gyro divider and targetLooptime (expected cycleTime)
|
// calculate gyro divider and targetLooptime (expected cycleTime)
|
||||||
mpuDividerDrops = gyroSyncDenominator - 1;
|
mpuDividerDrops = gyroSyncDenominator - 1;
|
||||||
targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
|
const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
|
||||||
|
return targetLooptime;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t gyroMPU6xxxGetDividerDrops(void) {
|
uint8_t gyroMPU6xxxGetDividerDrops(void)
|
||||||
|
{
|
||||||
return mpuDividerDrops;
|
return mpuDividerDrops;
|
||||||
}
|
}
|
||||||
|
|
|
@ -5,8 +5,7 @@
|
||||||
* Author: borisb
|
* Author: borisb
|
||||||
*/
|
*/
|
||||||
|
|
||||||
extern uint32_t targetLooptime;
|
struct gyro_s;
|
||||||
|
bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
|
||||||
bool gyroSyncCheckUpdate(void);
|
|
||||||
uint8_t gyroMPU6xxxGetDividerDrops(void);
|
uint8_t gyroMPU6xxxGetDividerDrops(void);
|
||||||
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
|
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
|
||||||
|
|
|
@ -22,4 +22,4 @@ typedef void (*sensorInitFuncPtr)(void); // sensor init proto
|
||||||
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||||
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
|
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
|
||||||
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
|
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
|
||||||
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready
|
typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready
|
||||||
|
|
|
@ -36,7 +36,6 @@
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
|
|
|
@ -29,7 +29,6 @@
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
|
@ -74,7 +73,7 @@ pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we
|
||||||
|
|
||||||
void setTargetPidLooptime(uint8_t pidProcessDenom)
|
void setTargetPidLooptime(uint8_t pidProcessDenom)
|
||||||
{
|
{
|
||||||
targetPidLooptime = targetLooptime * pidProcessDenom;
|
targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
|
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
|
||||||
|
|
|
@ -52,7 +52,6 @@
|
||||||
#include "drivers/inverter.h"
|
#include "drivers/inverter.h"
|
||||||
#include "drivers/flash_m25p16.h"
|
#include "drivers/flash_m25p16.h"
|
||||||
#include "drivers/sonar_hcsr04.h"
|
#include "drivers/sonar_hcsr04.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/usb_io.h"
|
#include "drivers/usb_io.h"
|
||||||
#include "drivers/transponder_ir.h"
|
#include "drivers/transponder_ir.h"
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
|
|
|
@ -34,7 +34,6 @@
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensors/barometer.h"
|
#include "sensors/barometer.h"
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
|
@ -210,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
|
||||||
|
|
||||||
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
|
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
|
||||||
// GYRO calibration
|
// GYRO calibration
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
|
|
||||||
#ifdef GPS
|
#ifdef GPS
|
||||||
if (feature(FEATURE_GPS)) {
|
if (feature(FEATURE_GPS)) {
|
||||||
|
|
|
@ -48,7 +48,6 @@
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "drivers/buf_writer.h"
|
#include "drivers/buf_writer.h"
|
||||||
|
|
||||||
|
|
|
@ -40,7 +40,6 @@
|
||||||
#include "drivers/gpio.h"
|
#include "drivers/gpio.h"
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
#include "drivers/buf_writer.h"
|
#include "drivers/buf_writer.h"
|
||||||
#include "drivers/max7456.h"
|
#include "drivers/max7456.h"
|
||||||
|
@ -109,8 +108,8 @@ void setGyroSamplingSpeed(uint16_t looptime) {
|
||||||
uint16_t gyroSampleRate = 1000;
|
uint16_t gyroSampleRate = 1000;
|
||||||
uint8_t maxDivider = 1;
|
uint8_t maxDivider = 1;
|
||||||
|
|
||||||
if (looptime != targetLooptime || looptime == 0) {
|
if (looptime != gyro.targetLooptime || looptime == 0) {
|
||||||
if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes
|
if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
|
||||||
#ifdef STM32F303xC
|
#ifdef STM32F303xC
|
||||||
if (looptime < 1000) {
|
if (looptime < 1000) {
|
||||||
masterConfig.gyro_lpf = 0;
|
masterConfig.gyro_lpf = 0;
|
||||||
|
@ -854,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
break;
|
break;
|
||||||
case MSP_LOOP_TIME:
|
case MSP_LOOP_TIME:
|
||||||
headSerialReply(2);
|
headSerialReply(2);
|
||||||
serialize16((uint16_t)targetLooptime);
|
serialize16((uint16_t)gyro.targetLooptime);
|
||||||
break;
|
break;
|
||||||
case MSP_RC_TUNING:
|
case MSP_RC_TUNING:
|
||||||
headSerialReply(11);
|
headSerialReply(11);
|
||||||
|
|
|
@ -49,7 +49,6 @@
|
||||||
#include "drivers/inverter.h"
|
#include "drivers/inverter.h"
|
||||||
#include "drivers/flash_m25p16.h"
|
#include "drivers/flash_m25p16.h"
|
||||||
#include "drivers/sonar_hcsr04.h"
|
#include "drivers/sonar_hcsr04.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
#include "drivers/usb_io.h"
|
#include "drivers/usb_io.h"
|
||||||
#include "drivers/transponder_ir.h"
|
#include "drivers/transponder_ir.h"
|
||||||
|
@ -625,7 +624,7 @@ void init(void)
|
||||||
if (masterConfig.mixerMode == MIXER_GIMBAL) {
|
if (masterConfig.mixerMode == MIXER_GIMBAL) {
|
||||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||||
}
|
}
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
||||||
#endif
|
#endif
|
||||||
|
@ -693,12 +692,12 @@ void main_init(void)
|
||||||
|
|
||||||
/* Setup scheduler */
|
/* Setup scheduler */
|
||||||
schedulerInit();
|
schedulerInit();
|
||||||
rescheduleTask(TASK_GYROPID, targetLooptime);
|
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
|
||||||
setTaskEnabled(TASK_GYROPID, true);
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
|
||||||
if(sensors(SENSOR_ACC)) {
|
if(sensors(SENSOR_ACC)) {
|
||||||
setTaskEnabled(TASK_ACCEL, true);
|
setTaskEnabled(TASK_ACCEL, true);
|
||||||
switch(targetLooptime) { // Switch statement kept in place to change acc rates in the future
|
switch(gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future
|
||||||
case(500):
|
case(500):
|
||||||
case(375):
|
case(375):
|
||||||
case(250):
|
case(250):
|
||||||
|
|
|
@ -377,7 +377,7 @@ void mwArm(void)
|
||||||
static bool firstArmingCalibrationWasCompleted;
|
static bool firstArmingCalibrationWasCompleted;
|
||||||
|
|
||||||
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
|
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
armingCalibrationWasInitialised = true;
|
armingCalibrationWasInitialised = true;
|
||||||
firstArmingCalibrationWasCompleted = true;
|
firstArmingCalibrationWasCompleted = true;
|
||||||
}
|
}
|
||||||
|
@ -802,7 +802,7 @@ void taskMainPidLoopCheck(void)
|
||||||
|
|
||||||
const uint32_t startTime = micros();
|
const uint32_t startTime = micros();
|
||||||
while (true) {
|
while (true) {
|
||||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
|
if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
|
||||||
static uint8_t pidUpdateCountdown;
|
static uint8_t pidUpdateCountdown;
|
||||||
|
|
||||||
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
|
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
|
||||||
|
|
|
@ -41,7 +41,6 @@
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "rx/pwm.h"
|
#include "rx/pwm.h"
|
||||||
#include "rx/sbus.h"
|
#include "rx/sbus.h"
|
||||||
#include "rx/spektrum.h"
|
#include "rx/spektrum.h"
|
||||||
|
|
|
@ -28,7 +28,6 @@
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
|
|
|
@ -28,7 +28,6 @@
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
#include "io/statusindicator.h"
|
#include "io/statusindicator.h"
|
||||||
|
@ -36,36 +35,31 @@
|
||||||
|
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/gyro.h"
|
||||||
|
|
||||||
uint16_t calibratingG = 0;
|
|
||||||
int32_t gyroADC[XYZ_AXIS_COUNT];
|
|
||||||
float gyroADCf[XYZ_AXIS_COUNT];
|
|
||||||
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
|
||||||
|
|
||||||
static gyroConfig_t *gyroConfig;
|
|
||||||
static biquad_t gyroFilterState[3];
|
|
||||||
static bool gyroFilterStateIsSet;
|
|
||||||
static float gyroLpfCutFreq;
|
|
||||||
|
|
||||||
gyro_t gyro; // gyro access functions
|
gyro_t gyro; // gyro access functions
|
||||||
sensor_align_e gyroAlign = 0;
|
sensor_align_e gyroAlign = 0;
|
||||||
|
|
||||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
|
int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||||
|
float gyroADCf[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
|
static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||||
|
static const gyroConfig_t *gyroConfig;
|
||||||
|
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
|
||||||
|
static uint8_t gyroSoftLpfHz;
|
||||||
|
static uint16_t calibratingG = 0;
|
||||||
|
|
||||||
|
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
|
||||||
{
|
{
|
||||||
gyroConfig = gyroConfigToUse;
|
gyroConfig = gyroConfigToUse;
|
||||||
gyroLpfCutFreq = gyro_lpf_hz;
|
gyroSoftLpfHz = gyro_soft_lpf_hz;
|
||||||
}
|
}
|
||||||
|
|
||||||
void initGyroFilterCoefficients(void) {
|
void gyroInit(void)
|
||||||
int axis;
|
|
||||||
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
|
|
||||||
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
|
|
||||||
gyroFilterStateIsSet = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
|
||||||
{
|
{
|
||||||
calibratingG = calibrationCyclesRequired;
|
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
|
||||||
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
|
BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isGyroCalibrationComplete(void)
|
bool isGyroCalibrationComplete(void)
|
||||||
|
@ -73,27 +67,32 @@ bool isGyroCalibrationComplete(void)
|
||||||
return calibratingG == 0;
|
return calibratingG == 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isOnFinalGyroCalibrationCycle(void)
|
static bool isOnFinalGyroCalibrationCycle(void)
|
||||||
{
|
{
|
||||||
return calibratingG == 1;
|
return calibratingG == 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t calculateCalibratingCycles(void) {
|
static uint16_t gyroCalculateCalibratingCycles(void)
|
||||||
return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
|
{
|
||||||
|
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isOnFirstGyroCalibrationCycle(void)
|
static bool isOnFirstGyroCalibrationCycle(void)
|
||||||
{
|
{
|
||||||
return calibratingG == calculateCalibratingCycles();
|
return calibratingG == gyroCalculateCalibratingCycles();
|
||||||
|
}
|
||||||
|
|
||||||
|
void gyroSetCalibrationCycles(void)
|
||||||
|
{
|
||||||
|
calibratingG = gyroCalculateCalibratingCycles();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
|
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
|
||||||
{
|
{
|
||||||
int8_t axis;
|
|
||||||
static int32_t g[3];
|
static int32_t g[3];
|
||||||
static stdev_t var[3];
|
static stdev_t var[3];
|
||||||
|
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
|
|
||||||
// Reset g[axis] at start of calibration
|
// Reset g[axis] at start of calibration
|
||||||
if (isOnFirstGyroCalibrationCycle()) {
|
if (isOnFirstGyroCalibrationCycle()) {
|
||||||
|
@ -113,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
|
||||||
float dev = devStandardDeviation(&var[axis]);
|
float dev = devStandardDeviation(&var[axis]);
|
||||||
// check deviation and startover in case the model was moved
|
// check deviation and startover in case the model was moved
|
||||||
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
|
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles();
|
gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -129,8 +128,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
|
||||||
|
|
||||||
static void applyGyroZero(void)
|
static void applyGyroZero(void)
|
||||||
{
|
{
|
||||||
int8_t axis;
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
for (axis = 0; axis < 3; axis++) {
|
|
||||||
gyroADC[axis] -= gyroZero[axis];
|
gyroADC[axis] -= gyroZero[axis];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -138,14 +136,13 @@ static void applyGyroZero(void)
|
||||||
void gyroUpdate(void)
|
void gyroUpdate(void)
|
||||||
{
|
{
|
||||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||||
int axis;
|
|
||||||
|
|
||||||
// range: +/- 8192; +/- 2000 deg/sec
|
// range: +/- 8192; +/- 2000 deg/sec
|
||||||
if (!gyro.read(gyroADCRaw)) {
|
if (!gyro.read(gyroADCRaw)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
|
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
|
||||||
gyroADC[axis] = gyroADCRaw[axis];
|
gyroADC[axis] = gyroADCRaw[axis];
|
||||||
}
|
}
|
||||||
|
@ -158,16 +155,10 @@ void gyroUpdate(void)
|
||||||
|
|
||||||
applyGyroZero();
|
applyGyroZero();
|
||||||
|
|
||||||
if (gyroLpfCutFreq) {
|
if (gyroSoftLpfHz) {
|
||||||
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
|
gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
|
||||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
gyroADC[axis] = lrintf(gyroADCf[axis]);
|
||||||
if (gyroFilterStateIsSet) {
|
|
||||||
gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
|
|
||||||
gyroADC[axis] = lrintf(gyroADCf[axis]);
|
|
||||||
} else {
|
|
||||||
gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -31,19 +31,17 @@ typedef enum {
|
||||||
} gyroSensor_e;
|
} gyroSensor_e;
|
||||||
|
|
||||||
extern gyro_t gyro;
|
extern gyro_t gyro;
|
||||||
extern sensor_align_e gyroAlign;
|
|
||||||
|
|
||||||
extern int32_t gyroADC[XYZ_AXIS_COUNT];
|
extern int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||||
extern float gyroADCf[XYZ_AXIS_COUNT];
|
extern float gyroADCf[XYZ_AXIS_COUNT];
|
||||||
extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
|
|
||||||
|
|
||||||
typedef struct gyroConfig_s {
|
typedef struct gyroConfig_s {
|
||||||
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||||
} gyroConfig_t;
|
} gyroConfig_t;
|
||||||
|
|
||||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz);
|
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
void gyroSetCalibrationCycles(void);
|
||||||
|
void gyroInit(void);
|
||||||
void gyroUpdate(void);
|
void gyroUpdate(void);
|
||||||
bool isGyroCalibrationComplete(void);
|
bool isGyroCalibrationComplete(void);
|
||||||
uint16_t calculateCalibratingCycles(void);
|
|
||||||
|
|
||||||
|
|
|
@ -79,6 +79,7 @@ extern float magneticDeclination;
|
||||||
extern gyro_t gyro;
|
extern gyro_t gyro;
|
||||||
extern baro_t baro;
|
extern baro_t baro;
|
||||||
extern acc_t acc;
|
extern acc_t acc;
|
||||||
|
extern sensor_align_e gyroAlign;
|
||||||
|
|
||||||
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
|
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
|
||||||
|
|
||||||
|
@ -632,8 +633,9 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
|
||||||
acc.init(&acc);
|
acc.init(&acc);
|
||||||
}
|
}
|
||||||
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
|
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
|
||||||
gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation
|
gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation
|
||||||
gyro.init(gyroLpf);
|
gyro.init(gyroLpf);
|
||||||
|
gyroInit();
|
||||||
|
|
||||||
detectMag(magHardwareToUse);
|
detectMag(magHardwareToUse);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue