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

final commit

Browse source
This commit is contained in:
JuliooCesarMDM 2022-08-25 18:38:11 -03:00
parent 9fdaa646e6
commit c7980b0154
7 changed files with 31 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -48,7 +48,7 @@ void systemBeep(bool onoff)
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
if (SIMULATOR_OPTION_ENABLED(SIMU_MUTE_BEEPER)) {
if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) {
onoff = false;
}
}

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

@ -3401,13 +3401,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
// Check the MSP version of simulator
if (tmp_u8 != SIMULATOR_MSP_VERSION) {
break;
}
simulatorData.flags = sbufReadU8(src);
if (SIMULATOR_OPTION_DISABLED(SIMU_ENABLE)) {
if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) {
if (ARMING_FLAG(SIMULATOR_MODE)) { // Just once
DISABLE_ARMING_FLAG(SIMULATOR_MODE);
@ -3419,7 +3420,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
DISABLE_STATE(COMPASS_CALIBRATED);
compassInit();
#endif
simulatorData.flags = SIMU_RESET_FLAGS;
simulatorData.flags = HITL_RESET_FLAGS;
// Review: Many states were affected. Reboot?
disarm(DISARM_SWITCH); // Disarm to prevent motor output!!!
@ -3445,7 +3446,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (dataSize >= 14) {
if (feature(FEATURE_GPS) && SIMULATOR_OPTION_ENABLED(SIMU_HAS_NEW_GPS_DATA)) {
if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
gpsSol.fixType = sbufReadU8(src);
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.flags.hasNewData = true;
@ -3479,7 +3480,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
}
if (SIMULATOR_OPTION_DISABLED(SIMU_USE_IMU)) {
if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
attitude.values.roll = (int16_t)sbufReadU16(src);
attitude.values.pitch = (int16_t)sbufReadU16(src);
attitude.values.yaw = (int16_t)sbufReadU16(src);
@ -3515,13 +3516,13 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
}
if (SIMULATOR_OPTION_ENABLED(SIMU_EXT_BATTERY_VOLTAGE)) {
if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) {
simulatorData.vbat = sbufReadU8(src);
} else {
simulatorData.vbat = (uint8_t)(SIMULATOR_FULL_BATTERY * 10.0f);
}
if (SIMULATOR_OPTION_ENABLED(SIMU_AIRSPEED)) {
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
simulatorData.airSpeed = sbufReadU16(src);
}
} else {
@ -3529,10 +3530,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
}
}
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_ROLL);
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_PITCH);
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_YAW);
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.INPUT_STABILIZED_THROTTLE : -500));
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8) {

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

@ -170,18 +170,17 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void);
#define SIMULATOR_MSP_VERSION 2 // Simulator MSP version
#define SIMULATOR_BARO_TEMP 25 // °C
#define SIMULATOR_FULL_BATTERY 12.6f // Volts
#define SIMULATOR_OPTION_ENABLED(flag) ((simulatorData.flags & flag) != 0)
#define SIMULATOR_OPTION_DISABLED(flag) ((simulatorData.flags & flag) == 0)
#define SIMULATOR_HAS_OPTION(flag) ((simulatorData.flags & flag) != 0)
typedef enum {
SIMU_RESET_FLAGS = (0 << 0),
SIMU_ENABLE = (1 << 0),
SIMU_SIMULATE_BATTERY = (1 << 1),
SIMU_MUTE_BEEPER = (1 << 2),
SIMU_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from iNav)
SIMU_HAS_NEW_GPS_DATA = (1 << 4),
SIMU_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2
SIMU_AIRSPEED = (1 << 6)
HITL_RESET_FLAGS = (0 << 0),
HITL_ENABLE = (1 << 0),
HITL_SIMULATE_BATTERY = (1 << 1),
HITL_MUTE_BEEPER = (1 << 2),
HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from iNav)
HITL_HAS_NEW_GPS_DATA = (1 << 4),
HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2
HITL_AIRSPEED = (1 << 6)
} simulatorFlags_t;
typedef struct {
@ -189,11 +188,7 @@ typedef struct {
uint8_t debugIndex;
uint8_t vbat; // 126 -> 12.6V
uint16_t airSpeed; // cm/s
int16_t INPUT_STABILIZED_ROLL;
int16_t INPUT_STABILIZED_PITCH;
int16_t INPUT_STABILIZED_YAW;
int16_t INPUT_STABILIZED_THROTTLE;
int16_t input[4];
} simulatorData_t;
extern simulatorData_t simulatorData;

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

@ -462,7 +462,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE) && SIMULATOR_OPTION_DISABLED(SIMU_USE_IMU)) {
if (ARMING_FLAG(SIMULATOR_MODE) && !SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
imuComputeRotationMatrix();
}

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

@ -321,10 +321,10 @@ void servoMixer(float dT)
#undef GET_RX_CHANNEL_INPUT
#ifdef USE_SIMULATOR
simulatorData.INPUT_STABILIZED_ROLL = input[INPUT_STABILIZED_ROLL];
simulatorData.INPUT_STABILIZED_PITCH = input[INPUT_STABILIZED_PITCH];
simulatorData.INPUT_STABILIZED_YAW = input[INPUT_STABILIZED_YAW];
simulatorData.INPUT_STABILIZED_THROTTLE = input[INPUT_STABILIZED_THROTTLE];
simulatorData.input[INPUT_STABILIZED_ROLL] = input[INPUT_STABILIZED_ROLL];
simulatorData.input[INPUT_STABILIZED_PITCH] = input[INPUT_STABILIZED_PITCH];
simulatorData.input[INPUT_STABILIZED_YAW] = input[INPUT_STABILIZED_YAW];
simulatorData.input[INPUT_STABILIZED_THROTTLE] = input[INPUT_STABILIZED_THROTTLE];
#endif
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {

2
src/main/sensors/battery.c
View file

@ -292,7 +292,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
}
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
if (SIMULATOR_OPTION_ENABLED(SIMU_SIMULATE_BATTERY)) {
if (SIMULATOR_HAS_OPTION(HITL_SIMULATE_BATTERY)) {
vbat = ((uint16_t)simulatorData.vbat) * 10;
batteryFullVoltage = 1260;
batteryWarningVoltage = 1020;

4
src/main/sensors/pitotmeter.c
View file

@ -207,7 +207,7 @@ STATIC_PROTOTHREAD(pitotThread)
pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
#ifdef USE_SIMULATOR
if (SIMULATOR_OPTION_ENABLED(SIMU_AIRSPEED)) {
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
}
#endif
@ -235,7 +235,7 @@ STATIC_PROTOTHREAD(pitotThread)
pitot.airSpeed = 0.0f;
}
#ifdef USE_SIMULATOR
if (SIMULATOR_OPTION_ENABLED(SIMU_AIRSPEED)) {
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
pitot.airSpeed = simulatorData.airSpeed;
}
#endif