diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 595ac4cbb1..297f0d4f7f 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -302,8 +302,6 @@ main_sources(COMMON_SRC flight/failsafe.c flight/failsafe.h - flight/hil.c - flight/hil.h flight/imu.c flight/imu.h flight/kalman.c diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 157af76c83..1ba541b628 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -927,13 +927,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) // Calculate stabilisation pidController(dT); -#ifdef HIL - if (hilActive) { - hilUpdateControlState(); - motorControlEnable = false; - } -#endif - mixTable(); if (isMixerUsingServos()) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ac28c1ca13..11aa762d3c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -73,7 +73,6 @@ #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/hil.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" @@ -402,15 +401,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH); break; -#ifdef HIL - case MSP_HIL_STATE: - sbufWriteU16(dst, hilToSIM.pidCommand[ROLL]); - sbufWriteU16(dst, hilToSIM.pidCommand[PITCH]); - sbufWriteU16(dst, hilToSIM.pidCommand[YAW]); - sbufWriteU16(dst, hilToSIM.pidCommand[THROTTLE]); - break; -#endif - case MSP_SENSOR_STATUS: sbufWriteU8(dst, isHardwareHealthy() ? 1 : 0); sbufWriteU8(dst, getHwGyroStatus()); diff --git a/src/main/flight/hil.c b/src/main/flight/hil.c deleted file mode 100644 index bb3b4a2d5d..0000000000 --- a/src/main/flight/hil.c +++ /dev/null @@ -1,73 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -// Inertial Measurement Unit (IMU) - -#include -#include -#include - -#include "platform.h" - -#ifdef HIL - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/axis.h" -#include "common/maths.h" -#include "common/filter.h" - -#include "drivers/time.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" -#include "sensors/boardalignment.h" - -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/hil.h" - -#include "fc/config.h" -#include "fc/runtime_config.h" - -#include "navigation/navigation.h" -#include "navigation/navigation_private.h" - - -bool hilActive = false; -hilIncomingStateData_t hilToFC; -hilOutgoingStateData_t hilToSIM; - -void hilUpdateControlState(void) -{ - // FIXME: There must be a cleaner way to to this - // If HIL active, store PID outout into hilState and disable motor control - if (FLIGHT_MODE(MANUAL_MODE) || !STATE(AIRPLANE)) { - hilToSIM.pidCommand[ROLL] = rcCommand[ROLL]; - hilToSIM.pidCommand[PITCH] = rcCommand[PITCH]; - hilToSIM.pidCommand[YAW] = rcCommand[YAW]; - } else { - hilToSIM.pidCommand[ROLL] = axisPID[ROLL]; - hilToSIM.pidCommand[PITCH] = axisPID[PITCH]; - hilToSIM.pidCommand[YAW] = axisPID[YAW]; - } - - hilToSIM.pidCommand[THROTTLE] = rcCommand[THROTTLE]; -} - -#endif diff --git a/src/main/flight/hil.h b/src/main/flight/hil.h deleted file mode 100644 index df51c1441d..0000000000 --- a/src/main/flight/hil.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#ifdef HIL - -typedef struct { - int16_t rollAngle; // deg * 10 - int16_t pitchAngle; // deg * 10 - int16_t yawAngle; // deg * 10 - - int32_t baroAlt; // cm above launch - - int16_t bodyAccel[3]; // cm/s/s // forward, right, up -} hilIncomingStateData_t; - -typedef struct { - int16_t pidCommand[4]; -} hilOutgoingStateData_t; - -extern bool hilActive; -extern hilIncomingStateData_t hilToFC; -extern hilOutgoingStateData_t hilToSIM; - -void hilUpdateControlState(void); - -#endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3cc883f024..f85a3d6eb3 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -47,7 +47,6 @@ FILE_COMPILE_FOR_SPEED #include "fc/runtime_config.h" #include "fc/settings.h" -#include "flight/hil.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -194,7 +193,7 @@ void imuTransformVectorEarthToBody(fpVector3_t * v) quaternionRotateVector(v, v, &orientation); } -#if defined(USE_GPS) || defined(HIL) +#if defined(USE_GPS) STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw) { if (initialRoll > 1800) initialRoll -= 3600; @@ -600,37 +599,12 @@ static void imuCalculateEstimatedAttitude(float dT) imuUpdateEulerAngles(); } -#ifdef HIL -void imuHILUpdate(void) -{ - /* Set attitude */ - attitude.values.roll = hilToFC.rollAngle; - attitude.values.pitch = hilToFC.pitchAngle; - attitude.values.yaw = hilToFC.yawAngle; - - /* Compute rotation quaternion for future use */ - imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw); - - /* Fake accADC readings */ - accADCf[X] = hilToFC.bodyAccel[X] / GRAVITY_CMSS; - accADCf[Y] = hilToFC.bodyAccel[Y] / GRAVITY_CMSS; - accADCf[Z] = hilToFC.bodyAccel[Z] / GRAVITY_CMSS; -} -#endif - void imuUpdateAccelerometer(void) { -#ifdef HIL - if (sensors(SENSOR_ACC) && !hilActive) { - accUpdate(); - isAccelUpdatedAtLeastOnce = true; - } -#else if (sensors(SENSOR_ACC)) { accUpdate(); isAccelUpdatedAtLeastOnce = true; } -#endif } void imuCheckVibrationLevels(void) @@ -655,23 +629,10 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) previousIMUUpdateTimeUs = currentTimeUs; if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { -#ifdef HIL - if (!hilActive) { - gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s - accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s - imuCheckVibrationLevels(); - imuCalculateEstimatedAttitude(dT); // Update attitude estimate - } - else { - imuHILUpdate(); - imuUpdateMeasuredAcceleration(); - } -#else gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s imuCheckVibrationLevels(); imuCalculateEstimatedAttitude(dT); // Update attitude estimate -#endif } else { acc.accADCf[X] = 0.0f; acc.accADCf[Y] = 0.0f; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index dc7240f426..710b2c67c8 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -50,8 +50,6 @@ #include "sensors/barometer.h" #include "sensors/sensors.h" -#include "flight/hil.h" - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -325,12 +323,6 @@ int32_t baroCalculateAltitude(void) baro.BaroAlt = 0; } else { -#ifdef HIL - if (hilActive) { - baro.BaroAlt = hilToFC.baroAlt; - return baro.BaroAlt; - } -#endif // calculates height from ground via baro readings baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude; } diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index 68208000df..ccdc2648ef 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -100,8 +100,6 @@ #define I2C_DEVICE_2_SHARES_UART3 //#define USE_I2C_PULLUP -//#define HIL - #define USE_ADC #define ADC_CHANNEL_1_PIN PC0 #define ADC_CHANNEL_2_PIN PC1