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

Added possibility to run HIL simulator by using X-Plane as phisical engine

Browse source
This commit is contained in:
Sergey 2022-04-09 20:22:27 +03:00
parent 05d30a0d81
commit 82eba60114
No known key found for this signature in database
GPG key ID: 4A7AC190CD92FE01
20 changed files with 174 additions and 67 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/pwm_output.c
View file

@ -213,7 +213,9 @@ void pwmDisableMotors(void)
void pwmEnableMotors(void)
{
if (! ARMING_FLAG(SIMULATOR_MODE)) {
pwmMotorsEnabled = true;
}
}
bool isMotorBrushed(uint16_t motorPwmRateHz)

14
src/main/fc/cli.c
View file

@ -1703,6 +1703,19 @@ static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const s
}
}
static void cliOutput(char *cmdline)
{
// print out servo output
cliPrintHashLine("servo");
const char *format = "servo %u %d";
for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
cliPrintLinef(format,
i,
getServoValue(i)
);
}
}
static void cliServo(char *cmdline)
{
enum { SERVO_ARGUMENT_COUNT = 5 };
@ -3897,6 +3910,7 @@ const clicmd_t cmdTable[] = {
#ifdef USE_USB_MSC
CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc),
#endif
CLI_COMMAND_DEF("output", "output information", NULL, cliOutput),
#ifdef PLAY_SOUND
CLI_COMMAND_DEF("play_sound", NULL, "[<index>]\r\n", cliPlaySound),
#endif

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

@ -924,11 +924,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
}
//Servos should be filtered or written only when mixer is using servos or special feaures are enabled
if (isServoOutputEnabled()) {
if (isServoOutputEnabled() && ! ARMING_FLAG(SIMULATOR_MODE)) {
writeServos();
}
if (motorControlEnable) {
if (motorControlEnable && ! ARMING_FLAG(SIMULATOR_MODE)) {
writeMotors();
}

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

@ -98,6 +98,7 @@
#include "msp/msp_serial.h"
#include "navigation/navigation.h"
#include "navigation/navigation_pos_estimator_private.h"
#include "rx/rx.h"
#include "rx/msp.h"
@ -603,12 +604,14 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_ALTITUDE:
sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
#if defined(USE_BARO)
sbufWriteU32(dst, baroGetLatestAltitude());
sbufWriteU32(dst, baro.BaroAlt);
sbufWriteU32(dst, baro.BaroMslAlt);
sbufWriteU32(dst, baro.baroPressure);
#else
sbufWriteU32(dst, 0);
sbufWriteU32(dst, 0);
sbufWriteU32(dst, 0);
#endif
break;
@ -2450,7 +2453,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#ifdef USE_GPS
case MSP_SET_RAW_GPS:
if (dataSize == 14) {
if (sbufReadU8(src)) {
gpsSol.fixType = sbufReadU8(src);
if (gpsSol.fixType) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
@ -3165,6 +3169,11 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret)
{
uint8_t tmp_u8;
// uint16_t tmp_u16;
const unsigned int dataSize = sbufBytesRemaining(src);
switch (cmdMSP) {
case MSP_WP:
@ -3230,6 +3239,75 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = mspFcSafeHomeOutCommand(dst, src);
break;
case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src);
if (tmp_u8 == 1) {
if (! ARMING_FLAG(SIMULATOR_MODE)) {
baroStartCalibration(); // just once
}
ENABLE_ARMING_FLAG(SIMULATOR_MODE);
} else {
DISABLE_ARMING_FLAG(SIMULATOR_MODE);
}
if (dataSize >= 14) {
gpsSol.fixType = sbufReadU8(src);
if (gpsSol.fixType) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
gpsSol.flags.hasNewData = true;
gpsSol.flags.validVelNE = 0;
gpsSol.flags.validVelD = 0;
gpsSol.flags.validEPE = 0;
gpsSol.numSat = sbufReadU8(src);
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU32(src);
gpsSol.groundSpeed = sbufReadU16(src);
gpsSol.groundCourse = sbufReadU16(src);
gpsSol.velNED[X] = gpsSol.groundSpeed * cos_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
gpsSol.velNED[Y] = gpsSol.groundSpeed * sin_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
gpsSol.velNED[Z] = 0;
gpsSol.eph = 100;
gpsSol.epv = 100;
// Feed data to navigation
sensorsSet(SENSOR_GPS);
onNewGPSData();
attitude.values.roll = sbufReadU16(src)-1800;
attitude.values.pitch = sbufReadU16(src)-1800;
attitude.values.yaw = sbufReadU16(src);
// posEstimator.est.pos.x = attitude.values.roll;
// posEstimator.est.pos.y = attitude.values.pitch;
// posEstimator.est.pos.z = gpsSol.llh.alt;
acc.accADCf[X] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
acc.accADCf[Y] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
acc.accADCf[Z] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
gyro.gyroADCf[X] = (float)sbufReadU16(src) / 100 - 320;
gyro.gyroADCf[Y] = (float)sbufReadU16(src) / 100 - 320;
gyro.gyroADCf[Z] = (float)sbufReadU16(src) / 100 - 320;
baro.baroPressure = sbufReadU32(src);
} else {
DISABLE_STATE(GPS_FIX);
}
// sbufWriteU16(dst, motor[0]);
// sbufWriteU16(dst, servo[1]);
// sbufWriteU16(dst, servo[2]);
// sbufWriteU16(dst, servo[3]);
// sbufWriteU16(dst, servo[4]);
sbufWriteU16(dst, input[INPUT_STABILIZED_ROLL] + 500);
sbufWriteU16(dst, input[INPUT_STABILIZED_PITCH] + 500);
sbufWriteU16(dst, input[INPUT_STABILIZED_YAW] + 500);
sbufWriteU16(dst, input[INPUT_STABILIZED_THROTTLE] + 500);
*ret = MSP_RESULT_ACK;
break;
default:
// Not handled
return false;

1
src/main/fc/runtime_config.h
View file

@ -21,6 +21,7 @@
typedef enum {
ARMED = (1 << 2),
WAS_EVER_ARMED = (1 << 3),
SIMULATOR_MODE = (1 << 4),
ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7),
ARMING_DISABLED_NOT_LEVEL = (1 << 8),

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

@ -573,6 +573,11 @@ static void imuCalculateEstimatedAttitude(float dT)
const float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeight(dT);
const bool useAcc = (accWeight > 0.001f);
if (ARMING_FLAG(SIMULATOR_MODE)) {
// todo: after imuMahonyAHRSupdate, ground course is divided by 10 =(
return;
}
imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF,
useAcc ? &imuMeasuredAccelBF : NULL,
useMag ? &measuredMagBF : NULL,
@ -583,37 +588,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)
@ -638,23 +618,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;

12
src/main/flight/servos.c
View file

@ -101,6 +101,7 @@ void pgResetFn_servoParams(servoParam_t *instance)
}
}
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
int16_t servo[MAX_SUPPORTED_SERVOS];
static uint8_t servoRuleCount = 0;
@ -245,8 +246,6 @@ void writeServos(void)
void servoMixer(float dT)
{
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
if (FLIGHT_MODE(MANUAL_MODE)) {
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
@ -586,3 +585,12 @@ bool isMixerUsingServos(void)
{
return mixerUsesServos;
}
int16_t getServoValue(uint32_t n)
{
if (n >= MAX_SUPPORTED_SERVOS) {
return;
}
return servo[n];
}

2
src/main/flight/servos.h
View file

@ -150,6 +150,7 @@ typedef struct servoMetadata_s {
} servoMetadata_t;
extern int16_t servo[MAX_SUPPORTED_SERVOS];
extern int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
bool isServoOutputEnabled(void);
void setServoOutputEnabled(bool flag);
@ -160,3 +161,4 @@ void servoMixer(float dT);
void servoComputeScalingFactors(uint8_t servoIndex);
void servosInit(void);
int getServoCount(void);
int16_t getServoValue(uint32_t n);

5
src/main/io/gps.c
View file

@ -362,6 +362,11 @@ bool gpsUpdate(void)
return false;
}
if (ARMING_FLAG(SIMULATOR_MODE)) {
gpsUpdateTime();
gpsSetState(GPS_RUNNING);
return gpsSol.flags.hasNewData;
}
#ifdef USE_FAKE_GPS
return gpsFakeGPSUpdate();
#else

2
src/main/io/osd.c
View file

@ -846,6 +846,8 @@ static const char * osdArmingDisabledReasonMessage(void)
FALLTHROUGH;
case ARMED:
FALLTHROUGH;
case SIMULATOR_MODE:
FALLTHROUGH;
case WAS_EVER_ARMED:
break;
}

2
src/main/io/osd_dji_hd.c
View file

@ -537,6 +537,8 @@ static char * osdArmingDisabledReasonMessage(void)
FALLTHROUGH;
case ARMED:
FALLTHROUGH;
case SIMULATOR_MODE:
FALLTHROUGH;
case WAS_EVER_ARMED:
break;
}

1
src/main/msp/msp_protocol.h
View file

@ -287,6 +287,7 @@
#define MSP_DEBUGMSG 253 //out message debug string buffer
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
#define MSP_V2_FRAME 255 //MSPv2 payload indicator
#define MSP_SIMULATOR 256 //Set simulator state
// Additional commands that are not compatible with MultiWii
#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc

7
src/main/navigation/navigation_pos_estimator.c
View file

@ -526,6 +526,9 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
static void estimationPredict(estimationContext_t * ctx)
{
if (ARMING_FLAG(SIMULATOR_MODE)) {
//return; // posEstimator.est.* was set into fc_msp
}
const float accWeight = navGetAccelerometerWeight();
/* Prediction step: Z-axis */
@ -851,6 +854,10 @@ void initializePositionEstimator(void)
*/
void updatePositionEstimator(void)
{
if (ARMING_FLAG(SIMULATOR_MODE)) {
posEstimator.est.pos.z = baro.BaroAlt;
//return;
}
static bool isInitialized = false;
if (!isInitialized) {

2
src/main/navigation/navigation_pos_estimator_private.h
View file

@ -183,6 +183,8 @@ typedef struct {
fpVector3_t accBiasCorr;
} estimationContext_t;
extern navigationPosEstimator_t posEstimator;
extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
extern void estimationCalculateAGL(estimationContext_t * ctx);
extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);

3
src/main/sensors/acceleration.c
View file

@ -523,6 +523,9 @@ float accGetMeasuredMaxG(void)
void accUpdate(void)
{
if (ARMING_FLAG(SIMULATOR_MODE)) {
return;
}
if (!acc.dev.readFn(&acc.dev)) {
return;
}

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

@ -322,10 +322,13 @@ uint32_t baroUpdate(void)
if (baro.dev.start_ut) {
baro.dev.start_ut(&baro.dev);
}
if (! ARMING_FLAG(SIMULATOR_MODE)) {
baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
if (barometerConfig()->use_median_filtering) {
baro.baroPressure = applyBarometerMedianFilter(baro.baroPressure);
}
}
state = BAROMETER_NEEDS_SAMPLES;
return baro.dev.ut_delay;
break;
@ -365,16 +368,12 @@ int32_t baroCalculateAltitude(void)
}
baro.BaroAlt = 0;
baro.BaroMslAlt = 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;
baro.BaroMslAlt = pressureToAltitude(baro.baroPressure);
baro.BaroAlt = baro.BaroMslAlt - baroGroundAltitude;
}
return baro.BaroAlt;

3
src/main/sensors/barometer.h
View file

@ -41,8 +41,9 @@ typedef enum {
typedef struct baro_s {
baroDev_t dev;
int32_t BaroAlt;
int32_t BaroMslAlt;
int32_t baroTemperature; // Use temperature for telemetry
int32_t baroPressure; // Use pressure for telemetry
uint32_t baroPressure; // Use pressure for telemetry
} baro_t;
extern baro_t baro;

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

@ -449,6 +449,10 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
void FAST_CODE NOINLINE gyroFilter()
{
if (ARMING_FLAG(SIMULATOR_MODE)) {
return;
}
if (!gyro.initialized) {
return;
}
@ -500,6 +504,10 @@ void FAST_CODE NOINLINE gyroFilter()
void FAST_CODE NOINLINE gyroUpdate()
{
if (ARMING_FLAG(SIMULATOR_MODE)) {
return;
}
if (!gyro.initialized) {
return;
}

9
src/main/target/HGLRCF722/target.c
View file

@ -41,8 +41,13 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP1-2 D(1, 5, 5)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP1-2 D(1, 7, 5)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5)
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3)
// DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2
// DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3)
DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2)

14
src/main/target/HGLRCF722/target.h
View file

@ -128,19 +128,19 @@
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define USE_UART3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
//#define USE_UART3
//#define UART3_TX_PIN PB10
//#define UART3_RX_PIN PB11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
//#define USE_UART6
//#define UART6_TX_PIN PC6
//#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 6
#define SERIAL_PORT_COUNT 4
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS