diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index bedfe58a33..eb8990c4ee 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -130,8 +130,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 54bdc3bd14..cdf1499351 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -747,7 +747,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 50c4015644..74074761bf 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 5d6c834983..f2dcaff64e 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 a19d21e560..ee235a0884 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -884,8 +884,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 4e5913136b..f48c16b2de 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();