Add USE_ACC conditionals

This commit is contained in:
jflyper 2018-10-09 00:06:05 +09:00
parent af84f9e99d
commit cc0e689bb5
9 changed files with 31 additions and 2 deletions

View File

@ -140,8 +140,10 @@ static void activateConfig(void)
rcControlsInit(); rcControlsInit();
failsafeReset(); failsafeReset();
#ifdef USE_ACC
setAccelerationTrims(&accelerometerConfigMutable()->accZero); setAccelerationTrims(&accelerometerConfigMutable()->accZero);
accInitFilters(); accInitFilters();
#endif
imuConfigure(throttleCorrectionConfig()->throttle_correction_angle, throttleCorrectionConfig()->throttle_correction_value); imuConfigure(throttleCorrectionConfig()->throttle_correction_angle, throttleCorrectionConfig()->throttle_correction_value);

View File

@ -184,7 +184,13 @@ static bool isCalibrating(void)
// Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG // 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 #ifdef USE_LAUNCH_CONTROL

View File

@ -538,7 +538,9 @@ void init(void)
// so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter() // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
validateAndFixGyroConfig(); validateAndFixGyroConfig();
pidInit(currentPidProfile); pidInit(currentPidProfile);
#ifdef USE_ACC
accInitFilters(); accInitFilters();
#endif
#ifdef USE_PID_AUDIO #ifdef USE_PID_AUDIO
pidAudioInit(); pidAudioInit();
@ -700,9 +702,11 @@ void init(void)
blackboxInit(); blackboxInit();
#endif #endif
#ifdef USE_ACC
if (mixerConfig()->mixerMode == MIXER_GIMBAL) { if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
} }
#endif
gyroStartCalibration(false); gyroStartCalibration(false);
#ifdef USE_BARO #ifdef USE_BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);

View File

@ -278,12 +278,13 @@ void processRcStickPositions()
saveConfigAndNotify(); saveConfigAndNotify();
} }
#ifdef USE_ACC
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
// Calibrating Acc // Calibrating Acc
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
return; return;
} }
#endif
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
// Calibrating Mag // Calibrating Mag

View File

@ -149,10 +149,12 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
batteryUpdateAlarms(); batteryUpdateAlarms();
} }
#ifdef USE_ACC
static void taskUpdateAccelerometer(timeUs_t currentTimeUs) static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{ {
accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims); accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims);
} }
#endif
static void taskUpdateRxMain(timeUs_t currentTimeUs) static void taskUpdateRxMain(timeUs_t currentTimeUs)
{ {
@ -389,7 +391,9 @@ cfTask_t cfTasks[TASK_COUNT] = {
#endif #endif
[TASK_GYROPID] = DEFINE_TASK("PID", "GYRO", NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME), [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), [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_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_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), [TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH),

View File

@ -351,6 +351,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
} }
} }
#ifdef USE_ACC
static bool imuIsAccelerometerHealthy(float *accAverage) static bool imuIsAccelerometerHealthy(float *accAverage)
{ {
float accMagnitudeSq = 0; float accMagnitudeSq = 0;
@ -364,6 +365,7 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
// Accept accel readings only in range 0.9g - 1.1g // Accept accel readings only in range 0.9g - 1.1g
return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f); return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
} }
#endif
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling. // 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. // 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 #endif
float gyroAverage[XYZ_AXIS_COUNT]; float gyroAverage[XYZ_AXIS_COUNT];
gyroGetAccumulationAverage(gyroAverage); gyroGetAccumulationAverage(gyroAverage);
#ifdef USE_ACC
if (accGetAccumulationAverage(accAverage)) { if (accGetAccumulationAverage(accAverage)) {
useAcc = imuIsAccelerometerHealthy(accAverage); useAcc = imuIsAccelerometerHealthy(accAverage);
} }
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f, imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),

View File

@ -2066,10 +2066,12 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} }
break; break;
#ifdef USE_ACC
case MSP_ACC_CALIBRATION: case MSP_ACC_CALIBRATION:
if (!ARMING_FLAG(ARMED)) if (!ARMING_FLAG(ARMED))
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
break; break;
#endif
case MSP_MAG_CALIBRATION: case MSP_MAG_CALIBRATION:
if (!ARMING_FLAG(ARMED)) if (!ARMING_FLAG(ARMED))

View File

@ -138,6 +138,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
resetFlightDynamicsTrims(&instance->accZero); resetFlightDynamicsTrims(&instance->accZero);
} }
#ifdef USE_ACC
bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{ {
accelerationSensor_e accHardware = ACC_NONE; accelerationSensor_e accHardware = ACC_NONE;
@ -536,3 +538,4 @@ void accInitFilters(void)
} }
} }
} }
#endif

View File

@ -66,9 +66,11 @@ bool sensorsAutodetect(void)
bool gyroDetected = gyroInit(); bool gyroDetected = gyroInit();
#ifdef USE_ACC
if (gyroDetected) { if (gyroDetected) {
accInit(gyro.targetLooptime); accInit(gyro.targetLooptime);
} }
#endif
#ifdef USE_MAG #ifdef USE_MAG
compassInit(); compassInit();