From c7980b0154ba50c78e7f25cd8b90c8e09ffa6234 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 25 Aug 2022 18:38:11 -0300 Subject: [PATCH] final commit --- src/main/drivers/sound_beeper.c | 2 +- src/main/fc/fc_msp.c | 23 ++++++++++++----------- src/main/fc/runtime_config.h | 25 ++++++++++--------------- src/main/flight/imu.c | 2 +- src/main/flight/servos.c | 8 ++++---- src/main/sensors/battery.c | 2 +- src/main/sensors/pitotmeter.c | 4 ++-- 7 files changed, 31 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 37069552c9..bb089b4cb9 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -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; } } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index be25865864..48e6d5a9e9 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3400,14 +3400,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_SIMULATOR 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) { diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 5fb82bab99..c3ed0b4dc6 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -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; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 1b405f2be4..80fb0ce07d 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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(); } diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 6295b04d3e..f3a1b5c6d9 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -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++) { diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index efce6f6a51..28ebb58478 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -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; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 3e3804c2ac..d546379621 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -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