diff --git a/src/config.c b/src/config.c index c5bb92d60c..647fba00ce 100755 --- a/src/config.c +++ b/src/config.c @@ -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; diff --git a/src/config_master.h b/src/config_master.h index 3c7e120a91..6b82bd3ec6 100644 --- a/src/config_master.h +++ b/src/config_master.h @@ -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; diff --git a/src/flight_imu.c b/src/flight_imu.c index 09f415a972..170f72c46f 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -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(); diff --git a/src/mw.h b/src/mw.h index 261cf15e64..211a752b12 100755 --- a/src/mw.h +++ b/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" diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c index 720b6758ec..de0952313d 100644 --- a/src/sensors_gyro.c +++ b/src/sensors_gyro.c @@ -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(); diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h index ee5683d99e..a2be3f6a88 100644 --- a/src/sensors_gyro.h +++ b/src/sensors_gyro.h @@ -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); diff --git a/src/serial_cli.c b/src/serial_cli.c index 811e8004b3..a1d5d2cef8 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -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 },