diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e45dfdd885..cad36e5762 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -140,8 +140,10 @@ static void activateConfig(void) rcControlsInit(); failsafeReset(); +#ifdef USE_ACC setAccelerationTrims(&accelerometerConfigMutable()->accZero); accInitFilters(); +#endif imuConfigure(throttleCorrectionConfig()->throttle_correction_angle, throttleCorrectionConfig()->throttle_correction_value); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 66b53bb030..6d1e2bc5d3 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -184,7 +184,13 @@ static bool isCalibrating(void) // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG - return (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); + return ( +#ifdef USE_ACC + !accIsCalibrationComplete() +#else + false +#endif + && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } #ifdef USE_LAUNCH_CONTROL diff --git a/src/main/fc/init.c b/src/main/fc/init.c index b0a66e6707..180d8d26ce 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -538,7 +538,9 @@ void init(void) // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter() validateAndFixGyroConfig(); pidInit(currentPidProfile); +#ifdef USE_ACC accInitFilters(); +#endif #ifdef USE_PID_AUDIO pidAudioInit(); @@ -700,9 +702,11 @@ void init(void) blackboxInit(); #endif +#ifdef USE_ACC if (mixerConfig()->mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } +#endif gyroStartCalibration(false); #ifdef USE_BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index daaa41b24f..213b3c83e4 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -278,12 +278,13 @@ void processRcStickPositions() saveConfigAndNotify(); } +#ifdef USE_ACC if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); return; } - +#endif if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { // Calibrating Mag diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 3aab22fa8e..1beb523038 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -149,10 +149,12 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs) batteryUpdateAlarms(); } +#ifdef USE_ACC static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims); } +#endif static void taskUpdateRxMain(timeUs_t currentTimeUs) { @@ -389,7 +391,9 @@ cfTask_t cfTasks[TASK_COUNT] = { #endif [TASK_GYROPID] = DEFINE_TASK("PID", "GYRO", NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME), +#ifdef USE_ACC [TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM), +#endif [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM), [TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling [TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH), diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 45ef0b6984..3801437846 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -351,6 +351,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) } } +#ifdef USE_ACC static bool imuIsAccelerometerHealthy(float *accAverage) { float accMagnitudeSq = 0; @@ -364,6 +365,7 @@ static bool imuIsAccelerometerHealthy(float *accAverage) // Accept accel readings only in range 0.9g - 1.1g return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f); } +#endif // Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling. // When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence. @@ -489,9 +491,12 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) #endif float gyroAverage[XYZ_AXIS_COUNT]; gyroGetAccumulationAverage(gyroAverage); + +#ifdef USE_ACC if (accGetAccumulationAverage(accAverage)) { useAcc = imuIsAccelerometerHealthy(accAverage); } +#endif imuMahonyAHRSupdate(deltaT * 1e-6f, DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index da943af006..f0c5d99836 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -2066,10 +2066,12 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; +#ifdef USE_ACC case MSP_ACC_CALIBRATION: if (!ARMING_FLAG(ARMED)) accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); break; +#endif case MSP_MAG_CALIBRATION: if (!ARMING_FLAG(ARMED)) diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index b83367816b..4fd049d869 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -138,6 +138,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) resetFlightDynamicsTrims(&instance->accZero); } +#ifdef USE_ACC + bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware = ACC_NONE; @@ -536,3 +538,4 @@ void accInitFilters(void) } } } +#endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 8874633dfa..2f8a4a596b 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -66,9 +66,11 @@ bool sensorsAutodetect(void) bool gyroDetected = gyroInit(); +#ifdef USE_ACC if (gyroDetected) { accInit(gyro.targetLooptime); } +#endif #ifdef USE_MAG compassInit();