mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 12:25:20 +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 "common/axis.h"
|
||||||
#include "flight_common.h"
|
#include "flight_common.h"
|
||||||
|
|
||||||
#include "sensors_common.h"
|
|
||||||
|
|
||||||
#include "drivers/accgyro_common.h"
|
#include "drivers/accgyro_common.h"
|
||||||
#include "drivers/system_common.h"
|
#include "drivers/system_common.h"
|
||||||
|
|
||||||
|
#include "sensors_common.h"
|
||||||
|
#include "sensors_gyro.h"
|
||||||
|
|
||||||
#include "statusindicator.h"
|
#include "statusindicator.h"
|
||||||
#include "sensors_acceleration.h"
|
#include "sensors_acceleration.h"
|
||||||
#include "sensors_barometer.h"
|
#include "sensors_barometer.h"
|
||||||
|
@ -20,7 +23,6 @@
|
||||||
|
|
||||||
#include "drivers/serial_common.h"
|
#include "drivers/serial_common.h"
|
||||||
#include "flight_mixer.h"
|
#include "flight_mixer.h"
|
||||||
#include "sensors_common.h"
|
|
||||||
#include "boardalignment.h"
|
#include "boardalignment.h"
|
||||||
#include "battery.h"
|
#include "battery.h"
|
||||||
#include "gimbal.h"
|
#include "gimbal.h"
|
||||||
|
@ -87,6 +89,7 @@ void activateConfig(void)
|
||||||
generatePitchCurve(¤tProfile.controlRateConfig);
|
generatePitchCurve(¤tProfile.controlRateConfig);
|
||||||
generateThrottleCurve(¤tProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle);
|
generateThrottleCurve(¤tProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle);
|
||||||
|
|
||||||
|
useGyroConfig(&masterConfig.gyroConfig);
|
||||||
setPIDController(currentProfile.pidController);
|
setPIDController(currentProfile.pidController);
|
||||||
gpsUseProfile(¤tProfile.gpsProfile);
|
gpsUseProfile(¤tProfile.gpsProfile);
|
||||||
gpsUsePIDs(¤tProfile.pidProfile);
|
gpsUsePIDs(¤tProfile.pidProfile);
|
||||||
|
@ -273,7 +276,7 @@ static void resetConf(void)
|
||||||
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
|
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
|
||||||
masterConfig.max_angle_inclination = 500; // 50 degrees
|
masterConfig.max_angle_inclination = 500; // 50 degrees
|
||||||
masterConfig.yaw_control_direction = 1;
|
masterConfig.yaw_control_direction = 1;
|
||||||
masterConfig.moron_threshold = 32;
|
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
|
||||||
masterConfig.batteryConfig.vbatscale = 110;
|
masterConfig.batteryConfig.vbatscale = 110;
|
||||||
masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
|
masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
|
||||||
masterConfig.batteryConfig.vbatmincellvoltage = 33;
|
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_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_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
|
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).
|
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
|
||||||
int16_flightDynamicsTrims_t accZero;
|
int16_flightDynamicsTrims_t accZero;
|
||||||
int16_flightDynamicsTrims_t magZero;
|
int16_flightDynamicsTrims_t magZero;
|
||||||
|
|
|
@ -87,7 +87,7 @@ void computeIMU(void)
|
||||||
uint32_t axis;
|
uint32_t axis;
|
||||||
static int16_t gyroYawSmooth = 0;
|
static int16_t gyroYawSmooth = 0;
|
||||||
|
|
||||||
gyroGetADC(masterConfig.moron_threshold);
|
gyroGetADC();
|
||||||
if (sensors(SENSOR_ACC)) {
|
if (sensors(SENSOR_ACC)) {
|
||||||
updateAccelerationReadings(¤tProfile.accelerometerTrims);
|
updateAccelerationReadings(¤tProfile.accelerometerTrims);
|
||||||
getEstimatedAttitude();
|
getEstimatedAttitude();
|
||||||
|
|
1
src/mw.h
1
src/mw.h
|
@ -20,6 +20,7 @@ enum {
|
||||||
};
|
};
|
||||||
|
|
||||||
#include "sensors_barometer.h"
|
#include "sensors_barometer.h"
|
||||||
|
#include "sensors_gyro.h"
|
||||||
#include "serial_common.h"
|
#include "serial_common.h"
|
||||||
#include "rc_controls.h"
|
#include "rc_controls.h"
|
||||||
#include "rx_common.h"
|
#include "rx_common.h"
|
||||||
|
|
|
@ -16,9 +16,17 @@
|
||||||
|
|
||||||
uint16_t calibratingG = 0;
|
uint16_t calibratingG = 0;
|
||||||
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
|
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
|
||||||
|
|
||||||
|
static gyroConfig_t *gyroConfig;
|
||||||
|
|
||||||
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)
|
||||||
|
{
|
||||||
|
gyroConfig = gyroConfigToUse;
|
||||||
|
}
|
||||||
|
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||||
{
|
{
|
||||||
calibratingG = calibrationCyclesRequired;
|
calibratingG = calibrationCyclesRequired;
|
||||||
|
@ -83,14 +91,14 @@ static void applyGyroZero(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void gyroGetADC(uint8_t gyroMovementCalibrationThreshold)
|
void gyroGetADC(void)
|
||||||
{
|
{
|
||||||
// range: +/- 8192; +/- 2000 deg/sec
|
// range: +/- 8192; +/- 2000 deg/sec
|
||||||
gyro.read(gyroADC);
|
gyro.read(gyroADC);
|
||||||
alignSensors(gyroADC, gyroADC, gyroAlign);
|
alignSensors(gyroADC, gyroADC, gyroAlign);
|
||||||
|
|
||||||
if (!isGyroCalibrationComplete()) {
|
if (!isGyroCalibrationComplete()) {
|
||||||
performAcclerationCalibration(gyroMovementCalibrationThreshold);
|
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
|
||||||
}
|
}
|
||||||
|
|
||||||
applyGyroZero();
|
applyGyroZero();
|
||||||
|
|
|
@ -4,6 +4,11 @@ extern uint16_t acc_1G;
|
||||||
extern gyro_t gyro;
|
extern gyro_t gyro;
|
||||||
extern sensor_align_e gyroAlign;
|
extern sensor_align_e gyroAlign;
|
||||||
|
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
typedef struct gyroConfig_s {
|
||||||
void gyroGetADC(uint8_t gyroMovementCalibrationThreshold);
|
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 },
|
{ "yaw_control_direction", VAR_INT8, &masterConfig.yaw_control_direction, -1, 1 },
|
||||||
{ "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 },
|
{ "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 },
|
||||||
{ "max_angle_inclination", VAR_UINT16, &masterConfig.max_angle_inclination, 100, 900 },
|
{ "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_lpf", VAR_UINT16, &masterConfig.gyro_lpf, 0, 256 },
|
||||||
{ "gyro_cmpf_factor", VAR_UINT16, &masterConfig.gyro_cmpf_factor, 100, 1000 },
|
{ "gyro_cmpf_factor", VAR_UINT16, &masterConfig.gyro_cmpf_factor, 100, 1000 },
|
||||||
{ "gyro_cmpfm_factor", VAR_UINT16, &masterConfig.gyro_cmpfm_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