Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge pull request #8281 from JulioCesarMatias/DropOldHIL

Browse source 
Drop the old HIL
This commit is contained in:
Paweł Spychalski 2022-09-06 08:17:27 +02:00 • committed by GitHub
commit ef1c20fce0
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 1 additions and 184 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/CMakeLists.txt
View file

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

7
src/main/fc/fc_core.c
View file

@ -927,13 +927,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
// Calculate stabilisation
pidController(dT);
#ifdef HIL
if (hilActive) {
hilUpdateControlState();
motorControlEnable = false;
}
#endif
mixTable();
if (isMixerUsingServos()) {

10
src/main/fc/fc_msp.c
View file

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

73
src/main/flight/hil.c
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
// Inertial Measurement Unit (IMU)
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#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

42
src/main/flight/hil.h
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

41
src/main/flight/imu.c
View file

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

8
src/main/sensors/barometer.c
View file

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

2
src/main/target/ANYFC/target.h
View file

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