Moved targetLooptime into gyro_t, tidied gyro_sync and gyro
This commit is contained in:
parent
19b02db8dd
commit
4d238b27d5
|
@ -325,9 +325,6 @@ extern uint32_t currentTime;
|
|||
//From rx.c:
|
||||
extern uint16_t rssi;
|
||||
|
||||
//From gyro.c
|
||||
extern uint32_t targetLooptime;
|
||||
|
||||
//From rc_controls.c
|
||||
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("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);
|
||||
|
|
|
@ -22,7 +22,6 @@
|
|||
#include "drivers/accgyro.h"
|
||||
#include "drivers/light_led.h"
|
||||
#include "drivers/sound_beeper.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
|
|
|
@ -38,7 +38,6 @@
|
|||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/max7456.h"
|
||||
|
||||
|
|
|
@ -27,6 +27,7 @@ typedef struct gyro_s {
|
|||
sensorReadFuncPtr temperature; // read temperature if available
|
||||
sensorInterruptFuncPtr intStatus;
|
||||
float scale; // scalefactor
|
||||
uint32_t targetLooptime;
|
||||
} gyro_t;
|
||||
|
||||
typedef struct acc_s {
|
||||
|
|
|
@ -32,7 +32,6 @@
|
|||
#include "gpio.h"
|
||||
#include "exti.h"
|
||||
#include "bus_i2c.h"
|
||||
#include "gyro_sync.h"
|
||||
|
||||
#include "sensor.h"
|
||||
#include "accgyro.h"
|
||||
|
@ -300,11 +299,14 @@ bool mpuGyroRead(int16_t *gyroADC)
|
|||
return true;
|
||||
}
|
||||
|
||||
void checkMPUDataReady(bool *mpuDataReadyPtr) {
|
||||
bool checkMPUDataReady(void)
|
||||
{
|
||||
bool ret;
|
||||
if (mpuDataReady) {
|
||||
*mpuDataReadyPtr = true;
|
||||
ret = true;
|
||||
mpuDataReady= false;
|
||||
} else {
|
||||
*mpuDataReadyPtr = false;
|
||||
ret = false;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
|
|
@ -185,4 +185,4 @@ void mpuIntExtiInit(void);
|
|||
bool mpuAccRead(int16_t *accData);
|
||||
bool mpuGyroRead(int16_t *gyroADC);
|
||||
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
|
||||
void checkMPUDataReady(bool *mpuDataReadyPtr);
|
||||
bool checkMPUDataReady(void);
|
||||
|
|
|
@ -30,7 +30,6 @@
|
|||
#include "accgyro.h"
|
||||
#include "accgyro_mpu.h"
|
||||
#include "accgyro_mpu3050.h"
|
||||
#include "gyro_sync.h"
|
||||
|
||||
// MPU3050, Standard address 0x68
|
||||
#define MPU3050_ADDRESS 0x68
|
||||
|
|
|
@ -1,4 +1,4 @@
|
|||
/*
|
||||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
|
|
|
@ -4,48 +4,40 @@
|
|||
* Created on: 3 aug. 2015
|
||||
* Author: borisb
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "build_config.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.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;
|
||||
|
||||
bool getMpuDataStatus(gyro_t *gyro)
|
||||
bool gyroSyncCheckUpdate(const gyro_t *gyro)
|
||||
{
|
||||
bool mpuDataStatus;
|
||||
if (!gyro->intStatus)
|
||||
return false;
|
||||
gyro->intStatus(&mpuDataStatus);
|
||||
return mpuDataStatus;
|
||||
return false;
|
||||
return gyro->intStatus();
|
||||
}
|
||||
|
||||
bool gyroSyncCheckUpdate(void) {
|
||||
return getMpuDataStatus(&gyro);
|
||||
}
|
||||
#define GYRO_LPF_256HZ 0
|
||||
#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;
|
||||
|
||||
if (!lpf || lpf == 7) {
|
||||
if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
|
||||
gyroSamplePeriod = 125;
|
||||
} else {
|
||||
gyroSamplePeriod = 1000;
|
||||
|
@ -54,9 +46,11 @@ void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
|
|||
|
||||
// calculate gyro divider and targetLooptime (expected cycleTime)
|
||||
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;
|
||||
}
|
||||
|
|
|
@ -5,8 +5,7 @@
|
|||
* Author: borisb
|
||||
*/
|
||||
|
||||
extern uint32_t targetLooptime;
|
||||
|
||||
bool gyroSyncCheckUpdate(void);
|
||||
struct gyro_s;
|
||||
bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
|
||||
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 void (*sensorAccInitFuncPtr)(struct acc_s *acc); // 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/accgyro.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
|
|
@ -29,7 +29,6 @@
|
|||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "drivers/accgyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
@ -74,7 +73,7 @@ pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we
|
|||
|
||||
void setTargetPidLooptime(uint8_t pidProcessDenom)
|
||||
{
|
||||
targetPidLooptime = targetLooptime * pidProcessDenom;
|
||||
targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
|
||||
}
|
||||
|
||||
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
|
||||
|
|
|
@ -52,7 +52,6 @@
|
|||
#include "drivers/inverter.h"
|
||||
#include "drivers/flash_m25p16.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "drivers/usb_io.h"
|
||||
#include "drivers/transponder_ir.h"
|
||||
#include "drivers/sdcard.h"
|
||||
|
|
|
@ -34,7 +34,6 @@
|
|||
#include "drivers/system.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/battery.h"
|
||||
|
|
|
@ -48,7 +48,6 @@
|
|||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "drivers/buf_writer.h"
|
||||
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include "drivers/gpio.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/buf_writer.h"
|
||||
#include "drivers/max7456.h"
|
||||
|
@ -109,8 +108,8 @@ void setGyroSamplingSpeed(uint16_t looptime) {
|
|||
uint16_t gyroSampleRate = 1000;
|
||||
uint8_t maxDivider = 1;
|
||||
|
||||
if (looptime != targetLooptime || looptime == 0) {
|
||||
if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes
|
||||
if (looptime != gyro.targetLooptime || looptime == 0) {
|
||||
if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
|
||||
#ifdef STM32F303xC
|
||||
if (looptime < 1000) {
|
||||
masterConfig.gyro_lpf = 0;
|
||||
|
@ -854,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
break;
|
||||
case MSP_LOOP_TIME:
|
||||
headSerialReply(2);
|
||||
serialize16((uint16_t)targetLooptime);
|
||||
serialize16((uint16_t)gyro.targetLooptime);
|
||||
break;
|
||||
case MSP_RC_TUNING:
|
||||
headSerialReply(11);
|
||||
|
|
|
@ -49,7 +49,6 @@
|
|||
#include "drivers/inverter.h"
|
||||
#include "drivers/flash_m25p16.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/usb_io.h"
|
||||
#include "drivers/transponder_ir.h"
|
||||
|
@ -694,12 +693,12 @@ void main_init(void)
|
|||
|
||||
/* Setup scheduler */
|
||||
schedulerInit();
|
||||
rescheduleTask(TASK_GYROPID, targetLooptime);
|
||||
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
|
||||
if(sensors(SENSOR_ACC)) {
|
||||
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(375):
|
||||
case(250):
|
||||
|
|
|
@ -800,7 +800,7 @@ void taskMainPidLoopCheck(void)
|
|||
|
||||
const uint32_t startTime = micros();
|
||||
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;
|
||||
|
||||
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
|
||||
|
|
|
@ -41,7 +41,6 @@
|
|||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "rx/pwm.h"
|
||||
#include "rx/sbus.h"
|
||||
#include "rx/spektrum.h"
|
||||
|
|
|
@ -28,7 +28,6 @@
|
|||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
|
|
@ -28,7 +28,6 @@
|
|||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "io/beeper.h"
|
||||
#include "io/statusindicator.h"
|
||||
|
@ -36,29 +35,30 @@
|
|||
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
uint16_t calibratingG = 0;
|
||||
gyro_t gyro; // gyro access functions
|
||||
sensor_align_e gyroAlign = 0;
|
||||
|
||||
int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||
float gyroADCf[XYZ_AXIS_COUNT];
|
||||
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
static uint16_t calibratingG = 0;
|
||||
static 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
|
||||
sensor_align_e gyroAlign = 0;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
|
||||
{
|
||||
gyroConfig = gyroConfigToUse;
|
||||
gyroLpfCutFreq = gyro_lpf_hz;
|
||||
}
|
||||
|
||||
void initGyroFilterCoefficients(void) {
|
||||
void initGyroFilterCoefficients(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);
|
||||
if (gyroLpfCutFreq && gyro.targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
|
||||
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], gyro.targetLooptime);
|
||||
gyroFilterStateIsSet = true;
|
||||
}
|
||||
}
|
||||
|
@ -79,7 +79,7 @@ bool isOnFinalGyroCalibrationCycle(void)
|
|||
}
|
||||
|
||||
uint16_t calculateCalibratingCycles(void) {
|
||||
return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
|
||||
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
|
||||
}
|
||||
|
||||
bool isOnFirstGyroCalibrationCycle(void)
|
||||
|
|
|
@ -31,11 +31,9 @@ typedef enum {
|
|||
} gyroSensor_e;
|
||||
|
||||
extern gyro_t gyro;
|
||||
extern sensor_align_e gyroAlign;
|
||||
|
||||
extern int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||
extern float gyroADCf[XYZ_AXIS_COUNT];
|
||||
extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
|
||||
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.
|
||||
|
|
|
@ -79,6 +79,7 @@ extern float magneticDeclination;
|
|||
extern gyro_t gyro;
|
||||
extern baro_t baro;
|
||||
extern acc_t acc;
|
||||
extern sensor_align_e gyroAlign;
|
||||
|
||||
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
|
||||
|
||||
|
@ -632,7 +633,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
|
|||
acc.init(&acc);
|
||||
}
|
||||
// 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);
|
||||
|
||||
detectMag(magHardwareToUse);
|
||||
|
|
Loading…
Reference in New Issue