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();
failsafeReset();
#ifdef USE_ACC
setAccelerationTrims(&accelerometerConfigMutable()->accZero);
accInitFilters();
#endif
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
return (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
return (
#ifdef USE_ACC
!accIsCalibrationComplete()
#else
false
#endif
&& sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
#ifdef USE_LAUNCH_CONTROL

View File

@ -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);

View File

@ -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

View File

@ -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),

View File

@ -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]),

View File

@ -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))

View File

@ -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

View File

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