Add USE_ACC conditionals
This commit is contained in:
parent
af84f9e99d
commit
cc0e689bb5
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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),
|
||||||
|
|
|
@ -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]),
|
||||||
|
|
|
@ -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))
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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();
|
||||||
|
|
Loading…
Reference in New Issue