1
0
Fork 0
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:
Dominic Clifton 2014-04-23 01:35:49 +01:00
parent 9d56b4a00f
commit 11863e5c34
7 changed files with 29 additions and 10 deletions

View file

@ -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(&currentProfile.controlRateConfig);
generateThrottleCurve(&currentProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle);
useGyroConfig(&masterConfig.gyroConfig);
setPIDController(currentProfile.pidController);
gpsUseProfile(&currentProfile.gpsProfile);
gpsUsePIDs(&currentProfile.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;

View file

@ -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;

View file

@ -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(&currentProfile.accelerometerTrims);
getEstimatedAttitude();

View file

@ -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"

View file

@ -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();

View file

@ -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);

View file

@ -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 },