diff --git a/docs/SITL/SITL.md b/docs/SITL/SITL.md index 96ed4c19f98..49868ce1e1c 100644 --- a/docs/SITL/SITL.md +++ b/docs/SITL/SITL.md @@ -18,7 +18,7 @@ INAV SITL communicates for sensor data and control directly with the correspondi AS SITL is still an inav software, but running on PC, it is possible to use HITL interface for communication. -INAV-X-Plane-HITL plugin https://github.com/RomanLut/INAV-X-Plane-HITL can be used with SITL. +[INAV-X-Plane-HITL](https://github.com/RomanLut/INAV-X-Plane-HITL) or [INAV-X-Plane-XITL](https://github.com/Scavanger/INAV-X-Plane-XITL) plugin can be used with SITL. ## Sensors The following sensors are emulated: diff --git a/docs/SITL/X-Plane.md b/docs/SITL/X-Plane.md index 6a838c19f59..0b50be1bd5a 100644 --- a/docs/SITL/X-Plane.md +++ b/docs/SITL/X-Plane.md @@ -7,6 +7,9 @@ X-Plane is not a model flight simulator, but is based on real world data and is ## Aircraft It is recommended to use the "AR Wing" of the INAV HITL project: https://github.com/RomanLut/INAV-X-Plane-HITL +## INAV Plugin +For advanced SITL featues (like OSD, virtual RX, simulated hardware failures, power train simulation) you can use the [INAV-X-Plane-XITL plugin](https://github.com/Scavanger/INAV-X-Plane-XITL) + ## General settings In Settings / Network select "Accept incoming connections". The port can be found under "UDP PORTS", "Port we receive on". If no connection is established, the port can be changed. diff --git a/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md b/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md index c166c5117ac..5a97b1b1a14 100644 --- a/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md +++ b/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md @@ -8,4 +8,10 @@ https://github.com/RomanLut/INAV-X-Plane-HITL +or for latest HITL features (INAV >= 9.0) + + **INAV-X-Plane-XITL** + + https://github.com/Scavanger/INAV-X-Plane-XITL + HITL technique can be used to test features during development. Please check page above for installation instructions. diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3e81b028de2..3bf7a354058 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -106,6 +106,7 @@ #include "msp/msp.h" #include "msp/msp_protocol.h" #include "msp/msp_serial.h" +#include "io/rangefinder.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" //for MSP_SIMULATOR @@ -115,6 +116,7 @@ #include "rx/msp.h" #include "rx/srxl2.h" #include "rx/crsf.h" +#include "rx/sim.h" #include "scheduler/scheduler.h" @@ -1628,7 +1630,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_OUTPUT_MAPPING_EXT: for (uint8_t i = 0; i < timerHardwareCount; ++i) if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { - #if defined(SITL_BUILD) + #if defined(SITL_BUILD) || defined(WASM_BUILD) sbufWriteU8(dst, i); #else sbufWriteU8(dst, timer2id(timerHardware[i].tim)); @@ -1639,19 +1641,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_OUTPUT_MAPPING_EXT2: { - #if !defined(SITL_BUILD) && defined(WS2811_PIN) + #if !(defined(SITL_BUILD) || defined(WASM_BUILD)) && defined(WS2811_PIN) ioTag_t led_tag = IO_TAG(WS2811_PIN); #endif for (uint8_t i = 0; i < timerHardwareCount; ++i) if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { - #if defined(SITL_BUILD) + #if defined(SITL_BUILD) || defined(WASM_BUILD) sbufWriteU8(dst, i); #else sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); - #if defined(SITL_BUILD) || !defined(WS2811_PIN) + #if defined(SITL_BUILD) || defined(WASM_BUILD) || !defined(WS2811_PIN) sbufWriteU8(dst, 0); #else // Extra label to help identify repurposed PINs. @@ -3897,6 +3899,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src) } #ifdef USE_SIMULATOR + bool isOSDTypeSupportedBySimulator(void) { #ifdef USE_OSD @@ -4042,11 +4045,224 @@ void mspWriteSimulatorOSD(sbuf_t *dst) sbufWriteU8(dst, 0); } } + +static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_t simMspVersion) +{ + if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once +#ifdef USE_BARO + if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { + sensorsSet(SENSOR_BARO); + setTaskEnabled(TASK_BARO, true); + DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); + baroStartCalibration(); + } +#endif + +#ifdef USE_MAG + if (compassConfig()->mag_hardware != MAG_NONE) { + sensorsSet(SENSOR_MAG); + ENABLE_STATE(COMPASS_CALIBRATED); + DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); + mag.magADC[X] = 800; + mag.magADC[Y] = 0; + mag.magADC[Z] = 0; + } #endif + ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + ENABLE_STATE(ACCELEROMETER_CALIBRATED); + LOG_DEBUG(SYSTEM, "Simulator enabled"); + } + + if (dataSize < 14) { + DISABLE_STATE(GPS_FIX); + return; + } + + if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { + gpsSolDRV.fixType = sbufReadU8(src); + gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.numSat = sbufReadU8(src); + + if (gpsSolDRV.fixType != GPS_NO_FIX) { + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.validTime = false; + + gpsSolDRV.llh.lat = sbufReadU32(src); + gpsSolDRV.llh.lon = sbufReadU32(src); + gpsSolDRV.llh.alt = sbufReadU32(src); + gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); + gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); + + gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); + + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; + } else { + sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); + } + // Feed data to navigation + gpsProcessNewDriverData(); + gpsProcessNewSolutionData(false); + } else { + 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_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); + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } + + // Get the acceleration in 1G units + acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accVibeSq[X] = 0.0f; + acc.accVibeSq[Y] = 0.0f; + acc.accVibeSq[Z] = 0.0f; + + // Get the angular velocity in DPS + gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; + + if (sensors(SENSOR_BARO)) { + baro.baroPressure = (int32_t)sbufReadU32(src); + baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); + } else { + sbufAdvance(src, sizeof(uint32_t)); + } + + if (sensors(SENSOR_MAG)) { + mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT + mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero + mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } + + if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { + simulatorData.vbat = sbufReadU8(src); + } else { + simulatorData.vbat = SIMULATOR_FULL_BATTERY; + } + + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + simulatorData.airSpeed = sbufReadU16(src); + } else if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + sbufReadU16(src); + } + + if (simMspVersion == SIMULATOR_MSP_VERSION_3) { + + if (SIMULATOR_HAS_OPTION(HITL_RANGEFINDER)) { + simulatorData.rangefinder = sbufReadU16(src); + if (simulatorData.rangefinder == 0xFFFF) { + fakeRangefindersSetData(-1); + } else { + fakeRangefindersSetData(simulatorData.rangefinder); + } + + } else { + sbufReadU16(src); + } + + if (SIMULATOR_HAS_OPTION(HITL_CURRENT_SENSOR)) { + simulatorData.current = sbufReadU16(src); + } else { + sbufReadU16(src); + } + + if (SIMULATOR_HAS_OPTION(HITL_SIM_RC_INPUT)) { + for (int i = 0; i < HITL_SIM_MAX_RC_INPUTS; i++) { + simulatorData.rcInput[i] = sbufReadU16(src); + } + rxSimSetChannelValue(simulatorData.rcInput, HITL_SIM_MAX_RC_INPUTS); + simulatorData.rssi = sbufReadU16(src); + rxSimSetRssi(simulatorData.rssi); + } else { + sbufAdvance(src, sizeof(uint16_t) * HITL_SIM_MAX_RC_INPUTS + sizeof(uint16_t)); // + RSSI + } + + rxSimSetFailsafe(SIMULATOR_HAS_OPTION(HITL_FAILSAFE_TRIGGERED)); + } + + // Backward compatibility for HITL Plugin 1.X + if (simMspVersion == SIMULATOR_MSP_VERSION_2 && SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; + } +} + +static mspResult_e mspProcessSimulatorCommand(sbuf_t *dst, sbuf_t *src, const int dataSize) +{ + if (dataSize < 2) { + return MSP_RESULT_ERROR; + } + + const uint8_t simMspVersion = sbufReadU8(src); // Get the Simulator MSP version + if (simMspVersion != SIMULATOR_MSP_VERSION_2 && simMspVersion != SIMULATOR_MSP_VERSION_3) { + return MSP_RESULT_ERROR; + } + + if (simMspVersion == SIMULATOR_MSP_VERSION_3) { + if (dataSize < 3) { + return MSP_RESULT_ERROR; + } + simulatorData.flags = sbufReadU16(src); + } else { + simulatorData.flags = sbufReadU8(src); + } + + const int remainingPayload = (int)sbufBytesRemaining(src); + + if (!SIMULATOR_HAS_OPTION(HITL_SITL_MODE)) { + if (!SIMULATOR_HAS_OPTION(HITL_ENABLE) && simulatorData.flags) { + fcReboot(false); + return MSP_RESULT_NO_REPLY; + } else { + readMspSimulatorValues(src, remainingPayload, simMspVersion); + } + } + + 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) { + simulatorData.debugIndex = 0; + } + + const uint8_t debugIndex = simulatorData.debugIndex | + ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | + (ARMING_FLAG(ARMED) ? 64 : 0) | + (!feature(FEATURE_OSD) ? 32: 0) | + (!isOSDTypeSupportedBySimulator() ? 16 : 0); + + sbufWriteU8(dst, debugIndex); + sbufWriteU32(dst, debug[simulatorData.debugIndex]); + + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, attitude.values.yaw); + + mspWriteSimulatorOSD(dst); + + return MSP_RESULT_ACK; +} + +#endif + bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret) { - uint8_t tmp_u8; const unsigned int dataSize = sbufBytesRemaining(src); switch (cmdMSP) { @@ -4153,179 +4369,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #endif #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_HAS_OPTION(HITL_ENABLE)) { - - if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once - DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); - -#ifdef USE_BARO - if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { - baroStartCalibration(); - } -#endif -#ifdef USE_MAG - DISABLE_STATE(COMPASS_CALIBRATED); - compassInit(); -#endif - simulatorData.flags = HITL_RESET_FLAGS; - // Review: Many states were affected. Reboot? - - disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } - } else { - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once -#ifdef USE_BARO - if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { - sensorsSet(SENSOR_BARO); - setTaskEnabled(TASK_BARO, true); - DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - baroStartCalibration(); - } -#endif - -#ifdef USE_MAG - if (compassConfig()->mag_hardware != MAG_NONE) { - sensorsSet(SENSOR_MAG); - ENABLE_STATE(COMPASS_CALIBRATED); - DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - mag.magADC[X] = 800; - mag.magADC[Y] = 0; - mag.magADC[Z] = 0; - } -#endif - ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); - ENABLE_STATE(ACCELEROMETER_CALIBRATED); - LOG_DEBUG(SYSTEM, "Simulator enabled"); - } - - if (dataSize >= 14) { - - if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { - gpsSolDRV.fixType = sbufReadU8(src); - gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSolDRV.numSat = sbufReadU8(src); - - if (gpsSolDRV.fixType != GPS_NO_FIX) { - gpsSolDRV.flags.validVelNE = true; - gpsSolDRV.flags.validVelD = true; - gpsSolDRV.flags.validEPE = true; - gpsSolDRV.flags.validTime = false; - - gpsSolDRV.llh.lat = sbufReadU32(src); - gpsSolDRV.llh.lon = sbufReadU32(src); - gpsSolDRV.llh.alt = sbufReadU32(src); - gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); - gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); - - gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); - - gpsSolDRV.eph = 100; - gpsSolDRV.epv = 100; - } else { - sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); - } - // Feed data to navigation - gpsProcessNewDriverData(); - gpsProcessNewSolutionData(false); - } else { - 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_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); - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } - - // Get the acceleration in 1G units - acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accVibeSq[X] = 0.0f; - acc.accVibeSq[Y] = 0.0f; - acc.accVibeSq[Z] = 0.0f; - - // Get the angular velocity in DPS - gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; - - if (sensors(SENSOR_BARO)) { - baro.baroPressure = (int32_t)sbufReadU32(src); - baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); - } else { - sbufAdvance(src, sizeof(uint32_t)); - } - - if (sensors(SENSOR_MAG)) { - mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT - mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero - mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } - - if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { - simulatorData.vbat = sbufReadU8(src); - } else { - simulatorData.vbat = SIMULATOR_FULL_BATTERY; - } - - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - simulatorData.airSpeed = sbufReadU16(src); - } else { - if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { - sbufReadU16(src); - } - } - - if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { - simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; - } - } else { - DISABLE_STATE(GPS_FIX); - } - } - - 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) { - simulatorData.debugIndex = 0; - } - - tmp_u8 = simulatorData.debugIndex | - ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | - (ARMING_FLAG(ARMED) ? 64 : 0) | - (!feature(FEATURE_OSD) ? 32: 0) | - (!isOSDTypeSupportedBySimulator() ? 16 : 0); - - sbufWriteU8(dst, tmp_u8); - sbufWriteU32(dst, debug[simulatorData.debugIndex]); - - sbufWriteU16(dst, attitude.values.roll); - sbufWriteU16(dst, attitude.values.pitch); - sbufWriteU16(dst, attitude.values.yaw); - - mspWriteSimulatorOSD(dst); - - *ret = MSP_RESULT_ACK; + *ret = mspProcessSimulatorCommand(dst, src, dataSize); break; #endif #ifndef SITL_BUILD diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 892df87b3ac..325ce862ce6 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -167,6 +167,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) simulatorData_t simulatorData = { .flags = 0, .debugIndex = 0, - .vbat = 0 + .vbat = 0, + .current = 0, + .rangefinder = 0 }; #endif diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 452256a6b64..153eae1be4a 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -178,7 +178,9 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); #ifdef USE_SIMULATOR -#define SIMULATOR_MSP_VERSION 2 // Simulator MSP version +#define SIMULATOR_MSP_VERSION_2 2 // Simulator MSP version +#define SIMULATOR_MSP_VERSION_3 3 +#define HITL_SIM_MAX_RC_INPUTS 8 #define SIMULATOR_BARO_TEMP 25 // °C #define SIMULATOR_FULL_BATTERY 126 // Volts*10 #define SIMULATOR_HAS_OPTION(flag) ((simulatorData.flags & flag) != 0) @@ -194,7 +196,12 @@ typedef enum { HITL_AIRSPEED = (1 << 6), HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 HITL_GPS_TIMEOUT = (1 << 8), - HITL_PITOT_FAILURE = (1 << 9) + HITL_PITOT_FAILURE = (1 << 9), + HITL_CURRENT_SENSOR = (1 << 10), + HITL_SIM_RC_INPUT = (1 << 11), // Simulate RC input from Joystick inputs in XPlane + HITL_RANGEFINDER = (1 << 12), // Simulate Rangefinder data + HITL_FAILSAFE_TRIGGERED = (1 << 13), // Simulate Failsafe triggered condition + HITL_SITL_MODE = (1 << 14), // For INAV XITL in Sitl mode (sends no emulated sensor data) } simulatorFlags_t; typedef struct { @@ -203,6 +210,11 @@ typedef struct { uint8_t vbat; // 126 -> 12.6V uint16_t airSpeed; // cm/s int16_t input[4]; + uint16_t rcInput[HITL_SIM_MAX_RC_INPUTS]; + uint16_t rssi; + uint16_t current; + uint16_t rangefinder; // cm + } simulatorData_t; extern simulatorData_t simulatorData; diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 9dc99889bc1..9188bee74b0 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -273,6 +273,8 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con */ static int drawScreen(displayPort_t *displayPort) // 250Hz { + vtxActive = SIMULATOR_HAS_OPTION(HITL_SITL_MODE); + static uint8_t counter = 0; if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) { // 62.5Hz diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..ee3fc28c47b 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -231,7 +231,7 @@ void onNewGPSData(void) /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); - if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && gpsSol.flags.validVelNE) { + if (!(ARMING_FLAG(SIMULATOR_MODE_SITL) || ARMING_FLAG(SIMULATOR_MODE_HITL)) && gpsSol.flags.validVelNE) { posEstimator.gps.vel.x = gpsSol.velNED[X]; posEstimator.gps.vel.y = gpsSol.velNED[Y]; } diff --git a/src/main/rx/sim.c b/src/main/rx/sim.c index 62d9a9b864c..c2747d81fc9 100644 --- a/src/main/rx/sim.c +++ b/src/main/rx/sim.c @@ -32,6 +32,7 @@ static uint16_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool hasNewData = false; static uint16_t rssi = 0; +static bool isFailsafe = false; static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { UNUSED(rxRuntimeConfigPtr); @@ -56,6 +57,11 @@ static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { } hasNewData = false; + + if (isFailsafe) { + return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; + } + return RX_FRAME_COMPLETE; } @@ -68,7 +74,11 @@ void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfig->rcFrameStatusFn = rxSimFrameStatus; } -void rxSimSetRssi(uint16_t value) { +void rxSimSetRssi(const uint16_t value) { rssi = value; } + +void rxSimSetFailsafe(const bool value) { + isFailsafe = value; +} #endif diff --git a/src/main/rx/sim.h b/src/main/rx/sim.h index 9c8ff36d1b0..e29d8d34cf2 100644 --- a/src/main/rx/sim.h +++ b/src/main/rx/sim.h @@ -20,5 +20,6 @@ #include "rx/rx.h" void rxSimSetChannelValue(uint16_t* values, uint8_t count); -void rxSimSetRssi(uint16_t value); +void rxSimSetRssi(const uint16_t value); +void rxSimSetFailsafe(const bool value); void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 38d410610e1..34e67aa5abc 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -633,6 +633,12 @@ void currentMeterUpdate(timeUs_t timeDelta) break; } +#ifdef USE_SIMULATOR + if (ARMING_FLAG(SIMULATOR_MODE_HITL) && SIMULATOR_HAS_OPTION(HITL_CURRENT_SENSOR)) { + amperage = ((uint16_t)simulatorData.current) * 10; + } +#endif + // Clamp amperage to positive values amperage = MAX(0, amperage); diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index 612f4a54a0a..fbc1824e35d 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/src/main/target/SITL/sim/xplane.c @@ -22,46 +22,56 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include "target/SITL/sim/xplane.h" + +#include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include #include -#include -#include #include -#include -#include -#include -#include - -#include "platform.h" -#include "target.h" -#include "target/SITL/sim/xplane.h" -#include "target/SITL/sim/simHelper.h" -#include "fc/runtime_config.h" -#include "drivers/time.h" +#include "common/maths.h" +#include "common/utils.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/barometer/barometer_fake.h" -#include "sensors/battery_sensor_fake.h" -#include "sensors/acceleration.h" -#include "drivers/pitotmeter/pitotmeter_fake.h" #include "drivers/compass/compass_fake.h" +#include "drivers/pitotmeter/pitotmeter_fake.h" #include "drivers/rangefinder/rangefinder_virtual.h" -#include "io/rangefinder.h" -#include "common/utils.h" -#include "common/maths.h" +#include "drivers/time.h" +#include "fc/runtime_config.h" +#include "flight/imu.h" #include "flight/mixer.h" #include "flight/servos.h" -#include "flight/imu.h" #include "io/gps.h" +#include "io/rangefinder.h" +#include "platform.h" #include "rx/sim.h" +#include "sensors/acceleration.h" +#include "sensors/battery_sensor_fake.h" +#include "target.h" +#include "target/SITL/sim/simHelper.h" #define XP_PORT 49000 #define XPLANE_JOYSTICK_AXIS_COUNT 8 +#define XITL_DREF_VERSION 2 +typedef enum +{ + DISCONNECTED, + CONNECTING, + INIT_CONNECTION, + CONNECTED, +} connectionState_t; + +static connectionState_t connectionState = CONNECTING; static uint8_t pwmMapping[XP_MAX_PWM_OUTS]; static uint8_t mappingCount; @@ -70,9 +80,10 @@ static struct sockaddr_storage serverAddr; static socklen_t serverAddrLen; static int sockFd; static pthread_t listenThread; -static bool initialized = false; static bool useImu = false; + + static float lattitude = 0; static float longitude = 0; static float elevation = 0; @@ -93,11 +104,27 @@ static float gyro_x = 0; static float gyro_y = 0; static float gyro_z = 0; static float barometer = 0; -static bool hasJoystick = false; +static bool hasJoystick = false; static float joystickRaw[XPLANE_JOYSTICK_AXIS_COUNT]; +// INAV-XITL specific variables +static int inavXitlDrefVersion = 0; +static int numSats = 16; +static int fixType = GPS_FIX_3D; +static float magX = 0; +static float magY = 0; +static float magZ = 0; +static int32_t rangefinderAltitude = -1; // cm +static float batteryVoltage = 12.6f; +static float batteryCurrent = 0; +static uint16_t rssi = 0; +static bool failsafe = false; + typedef enum { + DREF_XITL_DataRef_Version, + + // Common and XPLane DREFs DREF_LATITUDE, DREF_LONGITUDE, DREF_ELEVATION, @@ -128,25 +155,41 @@ typedef enum DREF_JOYSTICK_VALUES_CH6, DREF_JOYSTICK_VALUES_CH7, DREF_JOYSTICK_VALUES_CH8, + + // INAV-XITL DREFs + DREF_XITL_HEARTBEAT, + DREF_XITL_NUM_SATS, + DREF_XITL_FIX_TYPE, + DREF_XITL_MAG_X, + DREF_XITL_MAG_Y, + DREF_XITL_MAG_Z, + DREF_XITL_RANGEFINDER, + DREF_XITL_BATTERY_VOLTAGE, + DREF_XITL_BATTERY_CURRENT, + DREF_XITL_RSSI, + DREF_XITL_FAILSAFE, + + DREF_LAST } dref_t; -uint32_t xint2uint32 (uint8_t * buf) +uint32_t xint2uint32(uint8_t* buf) { - return buf[3] << 24 | buf [2] << 16 | buf [1] << 8 | buf [0]; + return buf[3] << 24 | buf[2] << 16 | buf[1] << 8 | buf[0]; } -float xflt2float (uint8_t * buf) +float xflt2float(uint8_t* buf) { - union { - float f; - uint32_t i; - } v; + union + { + float f; + uint32_t i; + } v; - v.i = xint2uint32 (buf); - return v.f; + v.i = xint2uint32(buf); + return v.f; } -static void registerDref(dref_t id, char* dref, uint32_t freq) +static bool registerDref(dref_t id, char* dref, uint32_t freq) { char buf[413]; memset(buf, 0, sizeof(buf)); @@ -156,7 +199,8 @@ static void registerDref(dref_t id, char* dref, uint32_t freq) memcpy(buf + 9, &id, 4); memcpy(buf + 13, dref, strlen(dref) + 1); - sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen); + ssize_t sentBytes = sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen); + return (sentBytes >= 0); } static void sendDref(char* dref, float value) @@ -167,250 +211,421 @@ static void sendDref(char* dref, float value) memset(buf + 9, ' ', sizeof(buf) - 9); strcpy(buf + 9, dref); - sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen); + ssize_t sentBytes = sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen); + if (sentBytes < 0) { + connectionState = DISCONNECTED; + } } -static void* listenWorker(void* arg) +static void registerXplaneDrefs(int32_t freq) { - UNUSED(arg); + // GPS + registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", freq); + registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", freq); + registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", freq); + registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", freq); + registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", freq); + registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", freq); + registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", freq); + + // Speed + registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", freq); + registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", freq); +} + +static void registerInavXitlDrefs(int32_t freq) +{ + // GPS + registerDref(DREF_XITL_NUM_SATS, "inav_xitl/gps/numSats", freq); + registerDref(DREF_XITL_FIX_TYPE, "inav_xitl/gps/fix", freq); + registerDref(DREF_LATITUDE, "inav_xitl/gps/latitude", freq); + registerDref(DREF_LONGITUDE, "inav_xitl/gps/longitude", freq); + registerDref(DREF_ELEVATION, "inav_xitl/gps/elevation", freq); + registerDref(DREF_LOCAL_VX, "inav_xitl/gps/velocities[0]", freq); + registerDref(DREF_LOCAL_VY, "inav_xitl/gps/velocities[1]", freq); + registerDref(DREF_LOCAL_VZ, "inav_xitl/gps/velocities[2]", freq); + + // Speed + registerDref(DREF_GROUNDSPEED, "inav_xitl/gps/groundspeed", freq); + registerDref(DREF_TRUE_AIRSPEED, "inav_xitl/sensors/airspeed", freq); + registerDref(DREF_XITL_MAG_X, "inav_xitl/sensors/magnetometer[0]", freq); + registerDref(DREF_XITL_MAG_Y, "inav_xitl/sensors/magnetometer[1]", freq); + registerDref(DREF_XITL_MAG_Z, "inav_xitl/sensors/magnetometer[2]", freq); + registerDref(DREF_XITL_RANGEFINDER, "inav_xitl/sensors/rangefinder", freq); + + // Battery + registerDref(DREF_XITL_BATTERY_VOLTAGE, "inav_xitl/sensors/battery_voltage", freq); + registerDref(DREF_XITL_BATTERY_CURRENT, "inav_xitl/sensors/battery_current", freq); + + // RC + registerDref(DREF_XITL_RSSI, "inav_xitl/rc/rssi", freq); + registerDref(DREF_XITL_FAILSAFE, "inav_xitl/rc/failsafe", freq); +} + +static void registerCommonDrefs(int32_t freq) +{ + registerDref(DREF_XITL_DataRef_Version, "inav_xitl/plugin/xitlDrefVersion", + freq); + registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", freq); + registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", freq); + registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", freq); + registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", freq); + registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", freq); + registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", freq); + registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", freq); + + registerDref(DREF_POS_P, "sim/flightmodel/position/P", freq); + registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", freq); + registerDref(DREF_POS_R, "sim/flightmodel/position/R", freq); + + registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", freq); + registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", freq); + registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", freq); + registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", freq); + registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", freq); + // Abusing cowl flaps for other channels + registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", freq); + registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", freq); + registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", freq); + registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", freq); + registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", freq); +} +static bool receiveSingleDref(dref_t in_dref, float* value) +{ uint8_t buf[1024]; struct sockaddr_storage remoteAddr; socklen_t slen = sizeof(remoteAddr); int recvLen; - while (true) - { + recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen); + if (recvLen < 0 && errno != EWOULDBLOCK) { + return false; + } - float motorValue = 0; - float yokeValues[3] = { 0 }; - int y = 0; - for (int i = 0; i < mappingCount; i++) { - if (y > 2) { - break; - } - if (pwmMapping[i] & 0x80) { // Motor - motorValue = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]); - } else { - yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]); - y++; - } - } + if (recvLen < 5 || strncmp((char*)buf, "RREF", 4) != 0) { + return false; + } - sendDref("sim/operation/override/override_joystick", 1); - sendDref("sim/cockpit2/engine/actuators/throttle_ratio_all", motorValue); - sendDref("sim/joystick/yoke_roll_ratio", yokeValues[0]); - sendDref("sim/joystick/yoke_pitch_ratio", yokeValues[1]); - sendDref("sim/joystick/yoke_heading_ratio", yokeValues[2]); - sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[0]", 0); - sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[1]", 0); - sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[2]", 0); - sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[3]", 0); - sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[4]", 0); - - recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen); - if (recvLen < 0 && errno != EWOULDBLOCK) { - continue; + for (int i = 5; i < recvLen; i += 8) { + dref_t dref = (dref_t)xint2uint32(&buf[i]); + if (dref == in_dref) { + *value = xflt2float(&(buf[i + 4])); + return true; } + } + + return false; +} + +static void exchangeDataWithXPlane(void) +{ + uint8_t buf[1024]; + struct sockaddr_storage remoteAddr; + socklen_t slen = sizeof(remoteAddr); + int recvLen; - if (strncmp((char*)buf, "RREF", 4) != 0) { - continue; + float motorValue = 0; + float yokeValues[3] = {0}; + int y = 0; + for (int i = 0; i < mappingCount; i++) { + if (y > 2) { + break; } + if (pwmMapping[i] & 0x80) { // Motor + motorValue = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]); + } else { + yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]); + y++; + } + } - for (int i = 5; i < recvLen; i += 8) { - dref_t dref = (dref_t)xint2uint32(&buf[i]); - float value = xflt2float(&(buf[i + 4])); + sendDref("sim/operation/override/override_joystick", 1); + if (inavXitlDrefVersion >= XITL_DREF_VERSION) { + sendDref("inav_xitl/plugin/heartbeat", 1.0f); + // Do not send motor value in Sitl mode, INAV XITL sets throttle itself + } else if (inavXitlDrefVersion < XITL_DREF_VERSION) { + sendDref("sim/cockpit2/engine/actuators/throttle_ratio_all",motorValue); + } - switch (dref) - { - case DREF_LATITUDE: - lattitude = value; - break; + sendDref("sim/joystick/yoke_roll_ratio", yokeValues[0]); + sendDref("sim/joystick/yoke_pitch_ratio", yokeValues[1]); + sendDref("sim/joystick/yoke_heading_ratio", yokeValues[2]); + sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[0]", 0); + sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[1]", 0); + sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[2]", 0); + sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[3]", 0); + sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[4]", 0); + + recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, + (struct sockaddr*)&remoteAddr, &slen); + if (recvLen < 0 && errno != EWOULDBLOCK) { + return; + } else if (recvLen == 0 || (recvLen == -1 && errno == ECONNRESET)) { + connectionState = DISCONNECTED; + return; + } - case DREF_LONGITUDE: - longitude = value; - break; + if (strncmp((char*)buf, "RREF", 4) != 0) { + return; + } - case DREF_ELEVATION: - elevation = value; - break; + for (int i = 5; i < recvLen; i += 8) { + dref_t dref = (dref_t)xint2uint32(&buf[i]); + float value = xflt2float(&(buf[i + 4])); - case DREF_AGL: - agl = value; - break; + switch (dref) { + case DREF_LATITUDE: + lattitude = value; + break; - case DREF_LOCAL_VX: - local_vx = value; - break; + case DREF_LONGITUDE: + longitude = value; + break; - case DREF_LOCAL_VY: - local_vy = value; - break; + case DREF_ELEVATION: + elevation = value; + break; - case DREF_LOCAL_VZ: - local_vz = value; - break; + case DREF_AGL: + agl = value; + break; - case DREF_GROUNDSPEED: - groundspeed = value; - break; + case DREF_LOCAL_VX: + local_vx = value; + break; - case DREF_TRUE_AIRSPEED: - airspeed = value; - break; + case DREF_LOCAL_VY: + local_vy = value; + break; - case DREF_POS_PHI: - roll = value; - break; + case DREF_LOCAL_VZ: + local_vz = value; + break; - case DREF_POS_THETA: - pitch = value; - break; + case DREF_GROUNDSPEED: + groundspeed = value; + break; - case DREF_POS_PSI: - yaw = value; - break; + case DREF_TRUE_AIRSPEED: + airspeed = value; + break; - case DREF_POS_HPATH: - hpath = value; - break; + case DREF_POS_PHI: + roll = value; + break; - case DREF_FORCE_G_AXI1: - accel_x = value; - break; + case DREF_POS_THETA: + pitch = value; + break; - case DREF_FORCE_G_SIDE: - accel_y = value; - break; + case DREF_POS_PSI: + yaw = value; + break; - case DREF_FORCE_G_NRML: - accel_z = value; - break; + case DREF_POS_HPATH: + hpath = value; + break; - case DREF_POS_P: - gyro_x = value; - break; + case DREF_FORCE_G_AXI1: + accel_x = value; + break; - case DREF_POS_Q: - gyro_y = value; - break; + case DREF_FORCE_G_SIDE: + accel_y = value; + break; - case DREF_POS_R: - gyro_z = value; - break; + case DREF_FORCE_G_NRML: + accel_z = value; + break; - case DREF_POS_BARO_CURRENT_INHG: - barometer = value; - break; + case DREF_POS_P: + gyro_x = value; + break; - case DREF_HAS_JOYSTICK: - hasJoystick = value >= 1 ? true : false; - break; + case DREF_POS_Q: + gyro_y = value; + break; - case DREF_JOYSTICK_VALUES_ROll: - joystickRaw[0] = value; - break; + case DREF_POS_R: + gyro_z = value; + break; - case DREF_JOYSTICK_VALUES_PITCH: - joystickRaw[1] = value; - break; + case DREF_POS_BARO_CURRENT_INHG: + barometer = value; + break; - case DREF_JOYSTICK_VALUES_THROTTLE: - joystickRaw[2] = value; - break; + case DREF_HAS_JOYSTICK: + hasJoystick = value >= 1 ? true : false; + break; - case DREF_JOYSTICK_VALUES_YAW: - joystickRaw[3] = value; - break; + case DREF_JOYSTICK_VALUES_ROll: + joystickRaw[0] = value; + break; - case DREF_JOYSTICK_VALUES_CH5: - joystickRaw[4] = value; - break; + case DREF_JOYSTICK_VALUES_PITCH: + joystickRaw[1] = value; + break; - case DREF_JOYSTICK_VALUES_CH6: - joystickRaw[5] = value; - break; + case DREF_JOYSTICK_VALUES_THROTTLE: + joystickRaw[2] = value; + break; - case DREF_JOYSTICK_VALUES_CH7: - joystickRaw[6] = value; - break; + case DREF_JOYSTICK_VALUES_YAW: + joystickRaw[3] = value; + break; - case DREF_JOYSTICK_VALUES_CH8: - joystickRaw[7] = value; - break; + case DREF_JOYSTICK_VALUES_CH5: + joystickRaw[4] = value; + break; - default: - break; - } - } + case DREF_JOYSTICK_VALUES_CH6: + joystickRaw[5] = value; + break; - if (hpath < 0) { - hpath += 3600; - } + case DREF_JOYSTICK_VALUES_CH7: + joystickRaw[6] = value; + break; - if (yaw < 0){ - yaw += 3600; - } + case DREF_JOYSTICK_VALUES_CH8: + joystickRaw[7] = value; + break; + + case DREF_XITL_NUM_SATS: + numSats = (int)value; + break; + + case DREF_XITL_FIX_TYPE: + fixType = (int)value; + break; + + case DREF_XITL_MAG_X: + magX = value; + break; + + case DREF_XITL_MAG_Y: + magY = value; + break; + + case DREF_XITL_MAG_Z: + magZ = value; + break; - if (hasJoystick) { - uint16_t channelValues[XPLANE_JOYSTICK_AXIS_COUNT]; - channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[0]); - channelValues[1] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[1]); - channelValues[2] = FLOAT_0_1_TO_PWM(joystickRaw[2]); - channelValues[3] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[3]); - channelValues[4] = FLOAT_0_1_TO_PWM(joystickRaw[4]); - channelValues[5] = FLOAT_0_1_TO_PWM(joystickRaw[5]); - channelValues[6] = FLOAT_0_1_TO_PWM(joystickRaw[6]); - channelValues[7] = FLOAT_0_1_TO_PWM(joystickRaw[7]); - - rxSimSetChannelValue(channelValues, XPLANE_JOYSTICK_AXIS_COUNT); + case DREF_XITL_RANGEFINDER: + rangefinderAltitude = (int32_t)value; + break; + + case DREF_XITL_BATTERY_VOLTAGE: + batteryVoltage = value; + break; + + case DREF_XITL_BATTERY_CURRENT: + batteryCurrent = value; + break; + + case DREF_XITL_RSSI: + rssi = (uint16_t)value; + break; + + case DREF_XITL_FAILSAFE: + failsafe = value >= 1 ? true : false; + break; + + default: + break; } + } - gpsFakeSet( - GPS_FIX_3D, - 16, - (int32_t)roundf(lattitude * 10000000), - (int32_t)roundf(longitude * 10000000), - (int32_t)roundf(elevation * 100), - (int16_t)roundf(groundspeed * 100), - (int16_t)roundf(hpath * 10), - 0, //(int16_t)roundf(-local_vz * 100), - 0, //(int16_t)roundf(local_vx * 100), - 0, //(int16_t)roundf(-local_vy * 100), - 0 - ); + if (hpath < 0) { + hpath += 3600; + } + + if (yaw < 0) { + yaw += 3600; + } + if (hasJoystick) { + uint16_t channelValues[XPLANE_JOYSTICK_AXIS_COUNT]; + channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[0]); + channelValues[1] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[1]); + channelValues[2] = FLOAT_0_1_TO_PWM(joystickRaw[2]); + channelValues[3] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[3]); + channelValues[4] = FLOAT_0_1_TO_PWM(joystickRaw[4]); + channelValues[5] = FLOAT_0_1_TO_PWM(joystickRaw[5]); + channelValues[6] = FLOAT_0_1_TO_PWM(joystickRaw[6]); + channelValues[7] = FLOAT_0_1_TO_PWM(joystickRaw[7]); + + rxSimSetChannelValue(channelValues, XPLANE_JOYSTICK_AXIS_COUNT); + } + + gpsFakeSet( + fixType, + numSats, + (int32_t)roundf(lattitude * 10000000), + (int32_t)roundf(longitude * 10000000), + (int32_t)roundf(elevation * 100), + (int16_t)roundf(groundspeed * 100), (int16_t)roundf(hpath * 10), + 0, //(int16_t)roundf(-local_vz * 100), + 0, //(int16_t)roundf(local_vx * 100), + 0, //(int16_t)roundf(-local_vy * 100), + 0 + ); + + if (inavXitlDrefVersion >= XITL_DREF_VERSION) { + if (rangefinderAltitude == 0xffff) { + fakeRangefindersSetData(-1); + } else { + fakeRangefindersSetData(rangefinderAltitude); + } + } else { + // Use AGL from X-Plane as rangefinder input const int32_t altitideOverGround = (int32_t)roundf(agl * 100); - if (altitideOverGround > 0 && altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) { + if (altitideOverGround > 0 && + altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) { fakeRangefindersSetData(altitideOverGround); } else { fakeRangefindersSetData(-1); } + } - const int16_t roll_inav = roll * 10; - const int16_t pitch_inav = -pitch * 10; - const int16_t yaw_inav = yaw * 10; + const int16_t roll_inav = roll * 10; + const int16_t pitch_inav = -pitch * 10; + const int16_t yaw_inav = yaw * 10; - if (!useImu) { - imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav); - imuUpdateAttitude(micros()); - } + if (!useImu) { + imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav); + imuUpdateAttitude(micros()); + } - fakeAccSet( - constrainToInt16(-accel_x * GRAVITY_MSS * 1000.0f), - constrainToInt16(accel_y * GRAVITY_MSS * 1000.0f), - constrainToInt16(accel_z * GRAVITY_MSS * 1000.0f) - ); + fakeAccSet( + constrainToInt16(-accel_x * GRAVITY_MSS * 1000.0f), + constrainToInt16(accel_y * GRAVITY_MSS * 1000.0f), + constrainToInt16(accel_z * GRAVITY_MSS * 1000.0f) + ); - fakeGyroSet( - constrainToInt16(gyro_x * 16.0f), - constrainToInt16(-gyro_y * 16.0f), - constrainToInt16(-gyro_z * 16.0f) - ); + fakeGyroSet( + constrainToInt16(gyro_x * 16.0f), + constrainToInt16(-gyro_y * 16.0f), + constrainToInt16(-gyro_z * 16.0f) + ); + + fakePitotSetAirspeed(airspeed * 100.0f); - fakeBaroSet((int32_t)roundf(barometer * 3386.39f), DEGREES_TO_CENTIDEGREES(21)); - fakePitotSetAirspeed(airspeed * 100.0f); + fakeBaroSet((int32_t)roundf(barometer * 3386.39f), + DEGREES_TO_CENTIDEGREES(21)); + if (inavXitlDrefVersion >= XITL_DREF_VERSION) { + fakeBattSensorSetVbat(batteryVoltage * 100); + fakeBattSensorSetAmperage(batteryCurrent * 100); + rxSimSetRssi(rssi); + rxSimSetFailsafe(failsafe); + + fakeMagSet( + constrainToInt16(magX * 1024.0f), + constrainToInt16(magY * 1024.0f), + constrainToInt16(magZ * 1024.0f) + ); + } else { fakeBattSensorSetVbat(16.8f * 100); fpQuaternion_t quat; @@ -425,32 +640,81 @@ static void* listenWorker(void* arg) constrainToInt16(north.y * 1024.0f), constrainToInt16(north.z * 1024.0f) ); + } - if (!initialized) { - ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); - // Aircraft can wobble on the runway and prevents calibration of the accelerometer - ENABLE_STATE(ACCELEROMETER_CALIBRATED); - initialized = true; - } + unlockMainPID(); +} + +static void* listenWorker(void* arg) +{ + UNUSED(arg); - unlockMainPID(); + while (connectionState != DISCONNECTED) { + switch (connectionState) { + case CONNECTING: + + // First register all DREFs with 0 freq to clear previous ones + registerCommonDrefs(0); + registerInavXitlDrefs(0); + registerXplaneDrefs(0); + + registerCommonDrefs(100); + + float inavXitlDrefVersionF = 0; + // Wait to determine connection type and test if X-PLane is present + if (receiveSingleDref(DREF_XITL_DataRef_Version, &inavXitlDrefVersionF)) { + inavXitlDrefVersion = (int)inavXitlDrefVersionF; + connectionState = INIT_CONNECTION; + break; + } + delay(100); + break; + + case INIT_CONNECTION: { + if (inavXitlDrefVersion >= XITL_DREF_VERSION) { + registerInavXitlDrefs(100); + printf("[SIM] X-Plane INAV-XITL plugin detected. DataRef version %d.\n", inavXitlDrefVersion); + } else { + registerXplaneDrefs(100); + } + // Get valid data to initialize sensors + exchangeDataWithXPlane(); + + ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); + // Aircraft can wobble on the runway and prevents + // calibration of the accelerometer + ENABLE_STATE(ACCELEROMETER_CALIBRATED); + connectionState = CONNECTED; + + break; + } + case CONNECTED: + exchangeDataWithXPlane(); + break; + default: + break; + } } + fprintf(stderr, "[SOCKET] X-Plane connection lost.\n"); + exit(0); + return NULL; } - -bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu) +bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, + bool imu) { memcpy(pwmMapping, mapping, mapCount); mappingCount = mapCount; useImu = imu; + connectionState = CONNECTING; if (port == 0) { - port = XP_PORT; // use default port + port = XP_PORT; // use default port } - if(lookupAddress(ip, port, SOCK_DGRAM, (struct sockaddr*)&serverAddr, &serverAddrLen) != 0) { + if (lookupAddress(ip, port, SOCK_DGRAM, (struct sockaddr*)&serverAddr, &serverAddrLen) != 0) { return false; } @@ -458,21 +722,21 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool if (sockFd < 0) { return false; } else { - char addrbuf[IPADDRESS_PRINT_BUFLEN]; - char *nptr = prettyPrintAddress((struct sockaddr *)&serverAddr, addrbuf, IPADDRESS_PRINT_BUFLEN ); + char addrbuf[IPADDRESS_PRINT_BUFLEN]; + char* nptr = prettyPrintAddress((struct sockaddr*)&serverAddr, addrbuf, IPADDRESS_PRINT_BUFLEN); if (nptr != NULL) { fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd); } } struct timeval tv; - tv.tv_sec = 1; - tv.tv_usec = 0; - if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { + tv.tv_sec = 1; + tv.tv_usec = 0; + if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval*)&tv, sizeof(struct timeval))) { return false; } - if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { + if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval*)&tv, sizeof(struct timeval))) { return false; } @@ -480,37 +744,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool return false; } - while (!initialized) { - registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100); - registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100); - registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100); - registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100); - registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100); - registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100); - registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100); - registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100); - registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100); - registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100); - registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100); - registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100); - registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100); - registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100); - registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100); - registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100); - registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100); - registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100); - registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100); - registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100); - registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100); - registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100); - registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100); - registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100); - // Abusing cowl flaps for other channels - registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100); + while (connectionState != CONNECTED) { delay(250); } diff --git a/src/main/target/SITL/sim/xplane.h b/src/main/target/SITL/sim/xplane.h index 1777a30af2b..626a9bc1984 100644 --- a/src/main/target/SITL/sim/xplane.h +++ b/src/main/target/SITL/sim/xplane.h @@ -23,6 +23,7 @@ */ #include +#include #define XP_MAX_PWM_OUTS 4 diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index f7963dfdd95..46ad6d9dad4 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -67,8 +67,6 @@ #define USE_FAKE_BARO #define USE_FAKE_MAG #define USE_GPS_FAKE -#define USE_RANGEFINDER_FAKE -#define USE_RX_SIM #undef MAX_MIXER_PROFILE_COUNT #define MAX_MIXER_PROFILE_COUNT 2 diff --git a/src/main/target/common.h b/src/main/target/common.h index 45eb12ac4bc..4eceac2a124 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -180,6 +180,8 @@ #define USE_SIMULATOR #define USE_PITOT_VIRTUAL #define USE_FAKE_BATT_SENSOR +#define USE_RANGEFINDER_FAKE +#define USE_RX_SIM #define USE_CMS_FONT_PREVIEW