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 #ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) { if (ARMING_FLAG(SIMULATOR_MODE)) {
if (SIMULATOR_OPTION_ENABLED(SIMU_MUTE_BEEPER)) { if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) {
onoff = false; onoff = false;
} }
} }

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

@ -3400,14 +3400,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
case MSP_SIMULATOR: case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
// Check the MSP version of simulator
if (tmp_u8 != SIMULATOR_MSP_VERSION) { if (tmp_u8 != SIMULATOR_MSP_VERSION) {
break; break;
} }
simulatorData.flags = sbufReadU8(src); simulatorData.flags = sbufReadU8(src);
if (SIMULATOR_OPTION_DISABLED(SIMU_ENABLE)) { if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) {
if (ARMING_FLAG(SIMULATOR_MODE)) { // Just once if (ARMING_FLAG(SIMULATOR_MODE)) { // Just once
DISABLE_ARMING_FLAG(SIMULATOR_MODE); 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); DISABLE_STATE(COMPASS_CALIBRATED);
compassInit(); compassInit();
#endif #endif
simulatorData.flags = SIMU_RESET_FLAGS; simulatorData.flags = HITL_RESET_FLAGS;
// Review: Many states were affected. Reboot? // Review: Many states were affected. Reboot?
disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! 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 (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.fixType = sbufReadU8(src);
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.flags.hasNewData = true; 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); 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.roll = (int16_t)sbufReadU16(src);
attitude.values.pitch = (int16_t)sbufReadU16(src); attitude.values.pitch = (int16_t)sbufReadU16(src);
attitude.values.yaw = (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); 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); simulatorData.vbat = sbufReadU8(src);
} else { } else {
simulatorData.vbat = (uint8_t)(SIMULATOR_FULL_BATTERY * 10.0f); 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); simulatorData.airSpeed = sbufReadU16(src);
} }
} else { } 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[INPUT_STABILIZED_ROLL]);
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_PITCH); sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_YAW); sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.INPUT_STABILIZED_THROTTLE : -500)); sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
simulatorData.debugIndex++; simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8) { 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_MSP_VERSION 2 // Simulator MSP version
#define SIMULATOR_BARO_TEMP 25 // °C #define SIMULATOR_BARO_TEMP 25 // °C
#define SIMULATOR_FULL_BATTERY 12.6f // Volts #define SIMULATOR_FULL_BATTERY 12.6f // Volts
#define SIMULATOR_OPTION_ENABLED(flag) ((simulatorData.flags & flag) != 0) #define SIMULATOR_HAS_OPTION(flag) ((simulatorData.flags & flag) != 0)
#define SIMULATOR_OPTION_DISABLED(flag) ((simulatorData.flags & flag) == 0)
typedef enum { typedef enum {
SIMU_RESET_FLAGS = (0 << 0), HITL_RESET_FLAGS = (0 << 0),
SIMU_ENABLE = (1 << 0), HITL_ENABLE = (1 << 0),
SIMU_SIMULATE_BATTERY = (1 << 1), HITL_SIMULATE_BATTERY = (1 << 1),
SIMU_MUTE_BEEPER = (1 << 2), HITL_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) 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)
SIMU_HAS_NEW_GPS_DATA = (1 << 4), HITL_HAS_NEW_GPS_DATA = (1 << 4),
SIMU_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2
SIMU_AIRSPEED = (1 << 6) HITL_AIRSPEED = (1 << 6)
} simulatorFlags_t; } simulatorFlags_t;
typedef struct { typedef struct {
@ -189,11 +188,7 @@ typedef struct {
uint8_t debugIndex; uint8_t debugIndex;
uint8_t vbat; // 126 -> 12.6V uint8_t vbat; // 126 -> 12.6V
uint16_t airSpeed; // cm/s uint16_t airSpeed; // cm/s
int16_t input[4];
int16_t INPUT_STABILIZED_ROLL;
int16_t INPUT_STABILIZED_PITCH;
int16_t INPUT_STABILIZED_YAW;
int16_t INPUT_STABILIZED_THROTTLE;
} simulatorData_t; } simulatorData_t;
extern simulatorData_t simulatorData; 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) STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{ {
#ifdef USE_SIMULATOR #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); imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
imuComputeRotationMatrix(); imuComputeRotationMatrix();
} }

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

@ -321,10 +321,10 @@ void servoMixer(float dT)
#undef GET_RX_CHANNEL_INPUT #undef GET_RX_CHANNEL_INPUT
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
simulatorData.INPUT_STABILIZED_ROLL = input[INPUT_STABILIZED_ROLL]; simulatorData.input[INPUT_STABILIZED_ROLL] = input[INPUT_STABILIZED_ROLL];
simulatorData.INPUT_STABILIZED_PITCH = input[INPUT_STABILIZED_PITCH]; simulatorData.input[INPUT_STABILIZED_PITCH] = input[INPUT_STABILIZED_PITCH];
simulatorData.INPUT_STABILIZED_YAW = input[INPUT_STABILIZED_YAW]; simulatorData.input[INPUT_STABILIZED_YAW] = input[INPUT_STABILIZED_YAW];
simulatorData.INPUT_STABILIZED_THROTTLE = input[INPUT_STABILIZED_THROTTLE]; simulatorData.input[INPUT_STABILIZED_THROTTLE] = input[INPUT_STABILIZED_THROTTLE];
#endif #endif
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { 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 #ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) { 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; vbat = ((uint16_t)simulatorData.vbat) * 10;
batteryFullVoltage = 1260; batteryFullVoltage = 1260;
batteryWarningVoltage = 1020; batteryWarningVoltage = 1020;

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

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