mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 04:15:44 +03:00
Avoid passing a uint8_t to a gyro method that is only used during gyro
calibration. Conflicts: src/config.c src/flight_imu.c src/mw.h src/sensors.c src/serial_cli.c
This commit is contained in:
parent
9d56b4a00f
commit
11863e5c34
7 changed files with 29 additions and 10 deletions
|
@ -7,11 +7,14 @@
|
|||
#include "common/axis.h"
|
||||
#include "flight_common.h"
|
||||
|
||||
#include "sensors_common.h"
|
||||
|
||||
|
||||
#include "drivers/accgyro_common.h"
|
||||
#include "drivers/system_common.h"
|
||||
|
||||
#include "sensors_common.h"
|
||||
#include "sensors_gyro.h"
|
||||
|
||||
#include "statusindicator.h"
|
||||
#include "sensors_acceleration.h"
|
||||
#include "sensors_barometer.h"
|
||||
|
@ -20,7 +23,6 @@
|
|||
|
||||
#include "drivers/serial_common.h"
|
||||
#include "flight_mixer.h"
|
||||
#include "sensors_common.h"
|
||||
#include "boardalignment.h"
|
||||
#include "battery.h"
|
||||
#include "gimbal.h"
|
||||
|
@ -87,6 +89,7 @@ void activateConfig(void)
|
|||
generatePitchCurve(¤tProfile.controlRateConfig);
|
||||
generateThrottleCurve(¤tProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle);
|
||||
|
||||
useGyroConfig(&masterConfig.gyroConfig);
|
||||
setPIDController(currentProfile.pidController);
|
||||
gpsUseProfile(¤tProfile.gpsProfile);
|
||||
gpsUsePIDs(¤tProfile.pidProfile);
|
||||
|
@ -273,7 +276,7 @@ static void resetConf(void)
|
|||
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
|
||||
masterConfig.max_angle_inclination = 500; // 50 degrees
|
||||
masterConfig.yaw_control_direction = 1;
|
||||
masterConfig.moron_threshold = 32;
|
||||
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
|
||||
masterConfig.batteryConfig.vbatscale = 110;
|
||||
masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
|
||||
masterConfig.batteryConfig.vbatmincellvoltage = 33;
|
||||
|
|
|
@ -34,7 +34,9 @@ typedef struct master_t {
|
|||
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
|
||||
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
|
||||
uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter
|
||||
uint8_t moron_threshold; // 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;
|
||||
|
||||
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
|
||||
int16_flightDynamicsTrims_t accZero;
|
||||
int16_flightDynamicsTrims_t magZero;
|
||||
|
|
|
@ -87,7 +87,7 @@ void computeIMU(void)
|
|||
uint32_t axis;
|
||||
static int16_t gyroYawSmooth = 0;
|
||||
|
||||
gyroGetADC(masterConfig.moron_threshold);
|
||||
gyroGetADC();
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
updateAccelerationReadings(¤tProfile.accelerometerTrims);
|
||||
getEstimatedAttitude();
|
||||
|
|
1
src/mw.h
1
src/mw.h
|
@ -20,6 +20,7 @@ enum {
|
|||
};
|
||||
|
||||
#include "sensors_barometer.h"
|
||||
#include "sensors_gyro.h"
|
||||
#include "serial_common.h"
|
||||
#include "rc_controls.h"
|
||||
#include "rx_common.h"
|
||||
|
|
|
@ -16,9 +16,17 @@
|
|||
|
||||
uint16_t calibratingG = 0;
|
||||
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
|
||||
|
||||
static gyroConfig_t *gyroConfig;
|
||||
|
||||
gyro_t gyro; // gyro access functions
|
||||
sensor_align_e gyroAlign = 0;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse)
|
||||
{
|
||||
gyroConfig = gyroConfigToUse;
|
||||
}
|
||||
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
{
|
||||
calibratingG = calibrationCyclesRequired;
|
||||
|
@ -83,14 +91,14 @@ static void applyGyroZero(void)
|
|||
}
|
||||
}
|
||||
|
||||
void gyroGetADC(uint8_t gyroMovementCalibrationThreshold)
|
||||
void gyroGetADC(void)
|
||||
{
|
||||
// range: +/- 8192; +/- 2000 deg/sec
|
||||
gyro.read(gyroADC);
|
||||
alignSensors(gyroADC, gyroADC, gyroAlign);
|
||||
|
||||
if (!isGyroCalibrationComplete()) {
|
||||
performAcclerationCalibration(gyroMovementCalibrationThreshold);
|
||||
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
|
||||
}
|
||||
|
||||
applyGyroZero();
|
||||
|
|
|
@ -4,6 +4,11 @@ extern uint16_t acc_1G;
|
|||
extern gyro_t gyro;
|
||||
extern sensor_align_e gyroAlign;
|
||||
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||
void gyroGetADC(uint8_t gyroMovementCalibrationThreshold);
|
||||
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.
|
||||
} gyroConfig_t;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||
void gyroGetADC(void);
|
||||
|
||||
|
|
|
@ -163,7 +163,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "yaw_control_direction", VAR_INT8, &masterConfig.yaw_control_direction, -1, 1 },
|
||||
{ "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 },
|
||||
{ "max_angle_inclination", VAR_UINT16, &masterConfig.max_angle_inclination, 100, 900 },
|
||||
{ "moron_threshold", VAR_UINT8, &masterConfig.moron_threshold, 0, 128 },
|
||||
{ "moron_threshold", VAR_UINT8, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 },
|
||||
{ "gyro_lpf", VAR_UINT16, &masterConfig.gyro_lpf, 0, 256 },
|
||||
{ "gyro_cmpf_factor", VAR_UINT16, &masterConfig.gyro_cmpf_factor, 100, 1000 },
|
||||
{ "gyro_cmpfm_factor", VAR_UINT16, &masterConfig.gyro_cmpfm_factor, 100, 1000 },
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue