From ee8a1676c4104f8ed194530ce7b75257e004b4fe Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 7 Dec 2016 11:48:29 +0000 Subject: [PATCH] Moved accelerometerTrims into accelerometerConfig() --- src/main/config/config_master.h | 2 -- src/main/fc/config.c | 2 +- src/main/fc/fc_msp.c | 8 ++++---- src/main/fc/fc_tasks.c | 2 +- src/main/fc/mw.c | 6 +++--- src/main/flight/imu.c | 11 +---------- src/main/flight/imu.h | 3 --- src/main/io/serial_cli.c | 4 ++-- src/main/sensors/acceleration.c | 3 ++- src/main/sensors/acceleration.h | 4 +++- src/main/target/COLIBRI_RACE/i2c_bst.c | 8 ++++---- 11 files changed, 21 insertions(+), 32 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4a03ff0ec2..6b20b64e3b 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -128,8 +128,6 @@ typedef struct master_s { imuConfig_t imuConfig; - rollAndPitchTrims_t accelerometerTrims; // accelerometer trim - pidConfig_t pidConfig; uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate diff --git a/src/main/fc/config.c b/src/main/fc/config.c index eaa40a7ed6..15b3113175 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -746,7 +746,7 @@ void createDefaultConfig(master_t *config) resetProfile(&config->profile[0]); - resetRollAndPitchTrims(&config->accelerometerTrims); + resetRollAndPitchTrims(&config->accelerometerConfig.accelerometerTrims); config->compassConfig.mag_declination = 0; config->accelerometerConfig.acc_lpf_hz = 10.0f; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 79c01a0476..accbd9a551 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -908,8 +908,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn // Additional commands that are not compatible with MultiWii case MSP_ACC_TRIM: - sbufWriteU16(dst, masterConfig.accelerometerTrims.values.pitch); - sbufWriteU16(dst, masterConfig.accelerometerTrims.values.roll); + sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch); + sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll); break; case MSP_UID: @@ -1289,8 +1289,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif break; case MSP_SET_ACC_TRIM: - masterConfig.accelerometerTrims.values.pitch = sbufReadU16(src); - masterConfig.accelerometerTrims.values.roll = sbufReadU16(src); + accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src); + accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src); break; case MSP_SET_ARMING_CONFIG: armingConfig()->auto_disarm_delay = sbufReadU8(src); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index cb04a8f23f..528780e96c 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -91,7 +91,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - imuUpdateAccelerometer(&masterConfig.accelerometerTrims); + accUpdate(&accelerometerConfig()->accelerometerTrims); } static void taskHandleSerial(timeUs_t currentTimeUs) diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index cfcf30fd2f..d49c9a69ef 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -103,8 +103,8 @@ float rcInput[3]; void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { - masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; - masterConfig.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; + accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; + accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; saveConfigAndNotify(); } @@ -679,7 +679,7 @@ void subTaskPidController(void) pidController( ¤tProfile->pidProfile, pidConfig()->max_angle_inclination, - &masterConfig.accelerometerTrims, + &accelerometerConfig()->accelerometerTrims, rxConfig()->midrc ); if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;} diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 0ba4467071..a25d992285 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -65,7 +65,6 @@ float fc_acc; float smallAngleCosZ = 0; float magneticDeclination = 0.0f; // calculated at startup from config -static bool isAccelUpdatedAtLeastOnce = false; static imuRuntimeConfig_t imuRuntimeConfig; static pidProfile_t *pidProfile; @@ -406,17 +405,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame } -void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims) -{ - if (sensors(SENSOR_ACC)) { - updateAccelerationReadings(accelerometerTrims); - isAccelUpdatedAtLeastOnce = true; - } -} - void imuUpdateAttitude(timeUs_t currentTimeUs) { - if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { + if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) { imuCalculateEstimatedAttitude(currentTimeUs); } else { acc.accSmooth[X] = 0; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 36439ea6df..9525c251ea 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -98,8 +98,6 @@ void imuConfigure( float getCosTiltAngle(void); void calculateEstimatedAltitude(timeUs_t currentTimeUs); -union rollAndPitchTrims_u; -void imuUpdateAccelerometer(union rollAndPitchTrims_u *accelerometerTrims); void imuUpdateAttitude(timeUs_t currentTimeUs); float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); @@ -109,5 +107,4 @@ union u_fp_vector; int16_t imuCalculateHeading(union u_fp_vector *vec); void imuResetAccelerationSum(void); -void imuUpdateAcc(union rollAndPitchTrims_u *accelerometerTrims); void imuInit(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 43c45c1864..36d750b3ee 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -883,8 +883,8 @@ const clivalue_t valueTable[] = { { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.xy, .config.minmax = { 0, 100 } }, { "accz_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.z, .config.minmax = { 0, 100 } }, { "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &imuConfig()->acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, - { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, - { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } }, + { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &accelerometerConfig()->accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, + { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &accelerometerConfig()->accelerometerTrims.values.roll, .config.minmax = { -300, 300 } }, #ifdef BARO { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, &barometerConfig()->baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } }, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 0b2017d3db..d2eb3eb606 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -377,11 +377,12 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim acc.accSmooth[Z] -= accelerationTrims->raw[Z]; } -void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) +void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims) { if (!acc.dev.read(&acc.dev)) { return; } + acc.isAccelUpdatedAtLeastOnce = true; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[axis]); diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index e936ca2035..0cc1020ae9 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -39,6 +39,7 @@ typedef struct acc_s { accDev_t dev; uint32_t accSamplingInterval; int32_t accSmooth[XYZ_AXIS_COUNT]; + bool isAccelUpdatedAtLeastOnce; } acc_t; extern acc_t acc; @@ -59,13 +60,14 @@ typedef struct accelerometerConfig_s { sensor_align_e acc_align; // acc alignment uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device flightDynamicsTrims_t accZero; + rollAndPitchTrims_t accelerometerTrims; } accelerometerConfig_t; bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); -void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims); +void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims); union flightDynamicsTrims_u; void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse); void setAccelerationFilter(uint16_t initialAccLpfCutHz); diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index bb9f92918f..21012f8710 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -837,8 +837,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) // Additional commands that are not compatible with MultiWii case BST_ACC_TRIM: - bstWrite16(masterConfig.accelerometerTrims.values.pitch); - bstWrite16(masterConfig.accelerometerTrims.values.roll); + bstWrite16(accelerometerConfig()->accelerometerTrims.values.pitch); + bstWrite16(accelerometerConfig()->accelerometerTrims.values.roll); break; case BST_UID: @@ -1033,8 +1033,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) } } case BST_SET_ACC_TRIM: - masterConfig.accelerometerTrims.values.pitch = bstRead16(); - masterConfig.accelerometerTrims.values.roll = bstRead16(); + accelerometerConfig()->accelerometerTrims.values.pitch = bstRead16(); + accelerometerConfig()->accelerometerTrims.values.roll = bstRead16(); break; case BST_SET_ARMING_CONFIG: armingConfig()->auto_disarm_delay = bstRead8();