diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index 091cf31fd5d..1954093e9b9 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -285,7 +285,7 @@ function(target_stm32) cmake_parse_arguments( args # Boolean arguments - "DISABLE_MSC;BOOTLOADER" + "DISABLE_MSC;BOOTLOADER;NO_BOOTLOADER" # Single value arguments "HSE_MHZ;LINKER_SCRIPT;NAME;OPENOCD_TARGET;OPTIMIZATION;STARTUP;SVD" # Multi-value arguments @@ -377,7 +377,7 @@ function(target_stm32) setup_firmware_target(${main_target_name} ${name} ${ARGN}) - if(args_BOOTLOADER) + if(args_BOOTLOADER AND NOT args_NO_BOOTLOADER) # Bootloader for the target set(bl_suffix _bl) add_stm32_executable( 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..15c13693866 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 features (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/VTOL.md b/docs/VTOL.md index 8341c81086d..403e80d21e7 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -133,7 +133,7 @@ save ![Alt text](Screenshots/mixerprofile_fw_mixer.png) -You must also assign the tilting servos values using the MAX values. If you don't do this the motors will point in the direction assigned by the transition mode. +You must also assign the tilting servos values using the Fixed Value values (formerly called "MAX"). If you don't do this the motors will point in the direction assigned by the transition mode. # STEP 2: Configuring as a Multi-Copter in Profile 2 @@ -149,7 +149,7 @@ You must also assign the tilting servos values using the MAX values. If you don 2. **Configure the Multicopter/tricopter:** - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and control_profile 2. - - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint. + - Utilize the Fixed Value values (formerly called "MAX") input in the servo mixer to tilt the motors without altering the servo midpoint. - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings. - you can set -1 in motor mixer throttle as a place holder: this will disable that motor but will load following the motor rules - compass is required to enable navigation modes for multi-rotor profile. @@ -199,7 +199,7 @@ The steps below describe how you can fine-tune the tilting servos to obtian the 2. **Switch to Multicopter/Tricopter:** - Assuming that you have set up your mixer similar to STEP1 and STEP2, you can now switch to the tricopter/multicopter mode and your servos should be tilting the motors upwards. If this is not the case, reverse the servo(s) in the Outputs tab such that the servo(s) is/are pointed upwards. - It is OK for the servos not to point exactly 90 degrees upwards, but they should be as close as possible to that position. - - Also, ensure that your MAX values in the Mixer tab are at 100 and -100, so that your servo will move to the maximum position, as shown in the screenshots in STEP1 and STEP2. + - Also, ensure that your Fixed Value values (formerly called "MAX") values in the Mixer tab are at 100 and -100, so that your servo will move to the maximum position, as shown in the screenshots in STEP1 and STEP2. 3. **Adjust the maximum throws for the Multicopter/Tricopter mode:** - While in tricopter mode, navigate to the Outputs tab and adjust the MIN and MAX endpoint values to position the motors slightly backward. @@ -214,7 +214,7 @@ The steps below describe how you can fine-tune the tilting servos to obtian the 5. **Adjsut the vertival position of the tilt servos:** - Switch back to multicopter/tricopter mode and open the Mixer tab. - - Start adjusting the `MAX` mixer lines from STEP2 such that the servos are pointed exactly upwards. In other words, start reducing the values of 100 and -100 to something like 80 and -80 until the motors are are pointed exaxctly upwards. + - Start adjusting the `Fixed Value` mixer lines from STEP2 such that the servos are pointed exactly upwards. In other words, start reducing the values of 100 and -100 to something like 80 and -80 until the motors are are pointed exaxctly upwards. - You will have to `Save & reboot` for adjustement for the changes to take effect, so be patient, take your time and don't forget to `Save & reboot`. - Move the YAW stick to either extreme position and ensure that the servos are tilting the motors both forwards and backwards. - NOTE: When yawing fully left, the left motor should tilt backwards and the right motor should tilt forwards. diff --git a/docs/development/Building in Linux.md b/docs/development/Building in Linux.md index 944872d530c..d85af2f2d5c 100644 --- a/docs/development/Building in Linux.md +++ b/docs/development/Building in Linux.md @@ -92,7 +92,7 @@ cmake -DCOMPILER_VERSION_CHECK=OFF .. `cmake` will generate a number of files in your `build` directory, including a cache of generated build settings `CMakeCache.txt` and a `Makefile`. -## Bulding the firmware +## Building the firmware Once `cmake` has generated the `build/Makefile`, this `Makfile` (with `make`) is used to build the firmware, again from the `build` directory. It is not necessary to re-run `cmake` unless the INAV cmake configuration is changed (i.e. a new release) or you wish to swap between the ARM SDK compiler and a distro or other external compiler. @@ -101,7 +101,7 @@ The generated `Makefile` uses different a target selection mechanism from the ol Typically, to build a single target, just pass the target name to `make`; note that unlike earlier releases, `make` without a target specified will build **all** targets. ``` -# Build the MATEKF405 firmware +# Build the MATEKF405 firmware (includes debug symbols by default) make MATEKF405 ``` @@ -112,6 +112,31 @@ One can also build multiple targets from a single `make` command: make -j $(($(nproc)-1)) MATEKF405 MATEKF722 ``` +### Building All Targets or Release Builds + +**⚠️ Important for CI or building many targets:** By default, INAV builds with `-DCMAKE_BUILD_TYPE=RelWithDebInfo`, which includes maximum debug symbols. When building all ~100 targets, this uses **~109 GB of disk space**. The debug symbols are automatically stripped from the final `.hex` files, so they provide no benefit for production builds. + +**For production builds, CI, or when building many targets, use Release mode:** + +```bash +cd inav +mkdir build-release +cd build-release +cmake -DCMAKE_BUILD_TYPE=Release .. + +# Build specific targets +make MATEKF405 + +# Or build all official release targets +make release +``` + +**Disk usage comparison:** +- `RelWithDebInfo` (default): ~109 GB for all targets +- `Release`: ~4-6 GB for all targets (96% reduction) + +The final `.hex` and `.bin` files are identical in both cases. + The resultant hex file are in the `build` directory. You can then use the INAV Configurator to flash the local `build/inav_x.y.z_TARGET.hex` file, or use `stm32flash` or `dfu-util` directly from the command line. 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/docs/development/release-create.md b/docs/development/release-create.md index f1ba7cf2369..a2cb4994a4c 100644 --- a/docs/development/release-create.md +++ b/docs/development/release-create.md @@ -379,6 +379,25 @@ gh release list --repo iNavFlight/inav-nightly --limit 5 gh release download --repo iNavFlight/inav-nightly --pattern "*.hex" ``` +#### Building Firmware Locally (if needed) + +**⚠️ Important:** Always use Release mode when building firmware for releases to save disk space: + +```bash +cd inav +mkdir build-release +cd build-release +cmake -DCMAKE_BUILD_TYPE=Release .. + +# Build all official release targets +make release + +# Or build specific targets +make MATEKF405 MATEKF722 +``` + +**Disk usage:** Release mode uses ~4-6 GB vs ~109 GB for default RelWithDebInfo mode (96% reduction). The debug symbols are stripped from final `.hex` files anyway, so Release mode produces identical output. + #### Renaming Firmware Files Remove CI suffix and add RC number for RC releases: diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 03a24d8f5c4..75372ff8825 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" @@ -1665,7 +1667,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)); @@ -1676,19 +1678,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. @@ -4061,6 +4063,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src) } #ifdef USE_SIMULATOR + bool isOSDTypeSupportedBySimulator(void) { #ifdef USE_OSD @@ -4206,11 +4209,234 @@ 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"); + } + + const int minSensorBytes = (simMspVersion == SIMULATOR_MSP_VERSION_3) ? 11 : 12; + if (dataSize < minSensorBytes) { + 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)) { + if (simulatorData.flags) { + // Non-zero flags but HITL_ENABLE cleared — reboot to clean state + fcReboot(false); + return MSP_RESULT_NO_REPLY; + } + // flags == 0: clean stop signal — disable HITL and disarm + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { + DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + simulatorData.flags = HITL_RESET_FLAGS; + disarm(DISARM_SWITCH); + } + } 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) { @@ -4317,179 +4543,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..faec9fdef15 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; // dA (deciamperes; * 10 = cA) + uint16_t rangefinder; // cm + } simulatorData_t; extern simulatorData_t simulatorData; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 912c38362bf..e3be177fb16 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -124,12 +124,20 @@ const setting_t *settingFind(const char *name) return NULL; } -const setting_t *settingGet(unsigned index) +// Under release builds (LTO enabled), inlining settingGet and settingGetIndex +// at their call sites in fc_msp.c produces wrong results for the +// settingGetIndex(ptr)->settingGet(index) round-trip, causing MSPV2_SETTING +// to return incorrect byte counts for some uint16_t settings. +// Use the raw attribute rather than NOINLINE because NOINLINE expands to +// nothing on non-F7/H7 targets (common.h:29), but LTO is enabled for all +// release targets and the bug affects all of them. +__attribute__((noinline)) const setting_t *settingGet(unsigned index) { return index < SETTINGS_TABLE_COUNT ? &settingsTable[index] : NULL; } -unsigned settingGetIndex(const setting_t *val) +// noinline: same reason as settingGet above +__attribute__((noinline)) unsigned settingGetIndex(const setting_t *val) { return val - settingsTable; } @@ -215,7 +223,9 @@ size_t settingGetValueSize(const setting_t *val) return 0; // Unreachable } -pgn_t settingGetPgn(const setting_t *val) +// noinline: same reason as settingGet above; also does pointer arithmetic +// against settingsTable and is called from settingGetValuePointer. +__attribute__((noinline)) pgn_t settingGetPgn(const setting_t *val) { uint16_t pos = val - (const setting_t *)settingsTable; uint16_t acc = 0; diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 2e70b8548f7..4963a09b97b 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -243,6 +243,10 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con */ static int drawScreen(displayPort_t *displayPort) // 250Hz { + if (SIMULATOR_HAS_OPTION(HITL_SITL_MODE)) { + vtxActive = true; + } + static uint8_t counter = 0; if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) { // 62.5Hz diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 55bca838f34..d0196308c1c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3609,7 +3609,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!moreThanAh) { osdWriteChar2(buff + strlen(buff), SYM_MAH_MI_0, SYM_MAH_MI_1); } else { - osdWriteChar(buff + strlen(buff), SYM_AH_MI); + osdWriteChar2(buff + strlen(buff), SYM_AH_MI, SYM_BLANK); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3623,7 +3623,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!moreThanAh) { osdWriteChar2(buff + strlen(buff), SYM_MAH_NM_0, SYM_MAH_NM_1); } else { - osdWriteChar(buff + strlen(buff), SYM_AH_NM); + osdWriteChar2(buff + strlen(buff), SYM_AH_NM, SYM_BLANK); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3639,7 +3639,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!moreThanAh) { osdWriteChar2(buff + strlen(buff), SYM_MAH_KM_0, SYM_MAH_KM_1); } else { - osdWriteChar(buff + strlen(buff), SYM_AH_KM); + osdWriteChar2(buff + strlen(buff), SYM_AH_KM, SYM_BLANK); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -6157,8 +6157,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)]; /* WARNING: ensure number of messages returned does not exceed messages array size - * Messages array set 1 larger than maximum expected message count of 6 */ - const char *messages[7]; + * Messages array set 1 larger than maximum expected message count of 7 */ + const char *messages[8]; unsigned messageCount = 0; const char *failsafeInfoMessage = NULL; @@ -6231,66 +6231,66 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (STATE(LANDING_DETECTED)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); } else { + /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ + /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */ #ifdef USE_GEOZONE - char buf[12], buf1[12]; - switch (geozone.messageState) { - case GEOZONE_MESSAGE_STATE_NFZ: - messages[messageCount++] = OSD_MSG_NFZ; - break; - case GEOZONE_MESSAGE_STATE_LEAVING_FZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); - tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: - messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; - break; - case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); - if (geozone.zoneInfo == INT32_MAX) { - tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); - } else { - osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); - } - tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_FB: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: - messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: - messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: - messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_POS_HOLD: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!geozone.sticksLocked) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_NONE: - break; - } + char buf[12], buf1[12]; + switch (geozone.messageState) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_POS_HOLD: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } #endif - /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ - /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ #ifdef USE_FW_AUTOLAND if (canFwLandingBeCancelled()) { @@ -6327,7 +6327,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } - } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { @@ -6341,12 +6340,21 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (FLIGHT_MODE(HEADFREE_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); } - if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { - /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes - * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field. - * In this case indicate ALTHOLD is active via a system message */ - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + if (posControl.flags.isTerrainFollowEnabled) { + if (posControl.flags.estAglStatus == EST_TRUSTED) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_OK); + } else { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_BAD); + } + } else if (!navigationRequiresAngleMode()) { + /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes + * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field. + * In this case indicate ALTHOLD is active via a system message */ + + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + } } } } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 88240ff84c0..e55aeb22461 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -109,6 +109,8 @@ #define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" #define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" +#define OSD_MSG_SURFACE_OK "(SURFACE)" +#define OSD_MSG_SURFACE_BAD "(!SURFACE UNRELIABLE!)" #define OSD_MSG_AUTOTRIM "(AUTOTRIM)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)" #define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 7da742829c5..5d4b7a5b425 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -122,15 +122,24 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) bool adjustMulticopterAltitudeFromRCInput(void) { if (posControl.flags.isTerrainFollowEnabled) { - const float altTarget = scaleRangef(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle(), 0, navConfig()->general.max_terrain_follow_altitude); + const int16_t altTarget = scaleRange(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle(), 0, navConfig()->general.max_terrain_follow_altitude); // In terrain follow mode we apply different logic for terrain control - if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { + if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10) { // We have solid terrain sensor signal - directly map throttle to altitude updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET); } else { - updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); + int16_t climbRate = -50; + + // Increase descent rate when throttle stick below mid from min rate of 0.5m/s up to max 2 m/s + if (posControl.flags.estAglStatus != EST_TRUSTED) { + const int16_t throttleIdle = getThrottleIdleValue(); + const int16_t throttleMid = rcLookupThrottleMid(); + climbRate = scaleRange(constrain(rcCommand[THROTTLE], throttleIdle, throttleMid), throttleIdle, throttleMid, -200, -50); + } + + updateClimbRateToAltitudeController(climbRate, 0, ROC_TO_ALT_CONSTANT); } // In surface tracking we always indicate that we're adjusting altitude @@ -837,10 +846,18 @@ bool isMulticopterLandingDetected(void) /* Basic condition to start looking for landing * Detection active during Failsafe only if throttle below mid hover throttle * and WP mission not active (except landing states). - * Also active in non autonomous flight modes but only when thottle low */ - bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) - || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) - || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow()); + * Also active in non autonomous flight modes but only when throttle low. + * Throttle low detection only allowed during Surface if AGL trusted and below 50cm */ + + bool throttleLowCheckAllowed = !navigationIsFlyingAutonomousMode(); + + if (posControl.flags.isTerrainFollowEnabled) { + throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 50.0f; + } + + bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || + (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) || + (throttleLowCheckAllowed && throttleStickIsLow()); static timeMs_t landingDetectorStartedAt; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 7ec861e934c..188ab7d25e3 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -242,6 +242,8 @@ 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); + /* SITL (X-Plane DataRef) passes zero velNED — use coordinate-difference velocity instead. + * MSP HITL explicitly injects real velNED via readMspSimulatorValues, so let it through. */ if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && gpsSol.flags.validVelNE) { posEstimator.gps.vel.x = gpsSol.velNED[X]; posEstimator.gps.vel.y = gpsSol.velNED[Y]; diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index 38788e20a77..3e9021abaf6 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -49,24 +49,15 @@ extern navigationPosEstimator_t posEstimator; void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt) { const float surfaceDtUs = currentTimeUs - posEstimator.surface.lastUpdateTime; - float newReliabilityMeasurement = 0; + uint8_t newReliabilityMeasurement = 0; // default to 0 for negative values, out of range or failed hardware bool surfaceMeasurementWithinRange = false; posEstimator.surface.lastUpdateTime = currentTimeUs; - if (newSurfaceAlt >= 0) { - if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { - newReliabilityMeasurement = 1.0f; - surfaceMeasurementWithinRange = true; - posEstimator.surface.alt = newSurfaceAlt; - } - else { - newReliabilityMeasurement = 0.0f; - } - } - else { - // Negative values - out of range or failed hardware - newReliabilityMeasurement = 0.0f; + if (newSurfaceAlt >= 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { + newReliabilityMeasurement = 1; + surfaceMeasurementWithinRange = true; + posEstimator.surface.alt = newSurfaceAlt; } /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */ diff --git a/src/main/rx/sim.c b/src/main/rx/sim.c index 62d9a9b864c..61103d6461e 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 volatile 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 79706c8dba7..e2e23cf21d4 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -635,6 +635,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/AIRBOTF7/CMakeLists.txt b/src/main/target/AIRBOTF7/CMakeLists.txt index 3d0e5bf1b83..305c0d44811 100644 --- a/src/main/target/AIRBOTF7/CMakeLists.txt +++ b/src/main/target/AIRBOTF7/CMakeLists.txt @@ -1,2 +1,2 @@ -target_stm32f722xe(AIRBOTF7 SKIP_RELEASES) -target_stm32f722xe(OMNIBUSF7NANOV7 SKIP_RELEASES) +target_stm32f722xe(AIRBOTF7 SKIP_RELEASES NO_BOOTLOADER) +target_stm32f722xe(OMNIBUSF7NANOV7 SKIP_RELEASES NO_BOOTLOADER) diff --git a/src/main/target/ANYFC/CMakeLists.txt b/src/main/target/ANYFC/CMakeLists.txt index 82d68535fa8..480531d92e3 100644 --- a/src/main/target/ANYFC/CMakeLists.txt +++ b/src/main/target/ANYFC/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(ANYFC SKIP_RELEASES) \ No newline at end of file +target_stm32f405xg(ANYFC SKIP_RELEASES NO_BOOTLOADER) \ No newline at end of file diff --git a/src/main/target/BLUEBERRYH743/target.h b/src/main/target/BLUEBERRYH743/target.h index b69400d0674..fe0e7925c9f 100644 --- a/src/main/target/BLUEBERRYH743/target.h +++ b/src/main/target/BLUEBERRYH743/target.h @@ -193,6 +193,7 @@ #define WS2811_PIN PA8 #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 2100 #define CURRENT_METER_SCALE 250 #define CURRENT_METER_OFFSET 400 diff --git a/src/main/target/CLRACINGF4AIR/CMakeLists.txt b/src/main/target/CLRACINGF4AIR/CMakeLists.txt index 26cc0addb43..d53bc5aff04 100644 --- a/src/main/target/CLRACINGF4AIR/CMakeLists.txt +++ b/src/main/target/CLRACINGF4AIR/CMakeLists.txt @@ -1,3 +1,3 @@ -target_stm32f405xg(CLRACINGF4AIR SKIP_RELEASES) -target_stm32f405xg(CLRACINGF4AIRV2 SKIP_RELEASES) -target_stm32f405xg(CLRACINGF4AIRV3 SKIP_RELEASES) +target_stm32f405xg(CLRACINGF4AIR SKIP_RELEASES NO_BOOTLOADER) +target_stm32f405xg(CLRACINGF4AIRV2 SKIP_RELEASES NO_BOOTLOADER) +target_stm32f405xg(CLRACINGF4AIRV3 SKIP_RELEASES NO_BOOTLOADER) diff --git a/src/main/target/FF_F35_LIGHTNING/CMakeLists.txt b/src/main/target/FF_F35_LIGHTNING/CMakeLists.txt index 15380146f56..415a69ebc51 100644 --- a/src/main/target/FF_F35_LIGHTNING/CMakeLists.txt +++ b/src/main/target/FF_F35_LIGHTNING/CMakeLists.txt @@ -1,2 +1,2 @@ -target_stm32f405xg(FF_F35_LIGHTNING) -target_stm32f405xg(WINGFC) +target_stm32f405xg(FF_F35_LIGHTNING NO_BOOTLOADER) +target_stm32f405xg(WINGFC NO_BOOTLOADER) diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt index 6da40b8ae6d..9c9a729ef0a 100644 --- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(FLYINGRCF4WINGMINI_NOT_RECOMMENDED) +target_stm32f405xg(FLYINGRCF4WINGMINI_NOT_RECOMMENDED NO_BOOTLOADER) diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index bf0349c47d1..d3c4d454b2b 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/src/main/target/SITL/sim/xplane.c @@ -22,64 +22,69 @@ * 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 #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 "xplane.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 _Atomic connectionState_t connectionState = CONNECTING; static uint8_t pwmMapping[XP_MAX_PWM_OUTS]; static uint8_t mappingCount; -static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER; -static atomic_bool shouldStopListenThread = false; - static struct sockaddr_storage serverAddr; static socklen_t serverAddrLen; static int sockFd; static pthread_t listenThread = 0; -static atomic_bool initalized = false; static bool useImu = false; + + static float lattitude = 0; static float longitude = 0; static float elevation = 0; @@ -100,11 +105,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, @@ -135,25 +156,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 (uint32_t)buf[3] << 24 | (uint32_t)buf[2] << 16 | (uint32_t)buf[1] << 8 | (uint32_t)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)); @@ -163,7 +200,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) @@ -174,289 +212,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 registerXplaneDrefs(int32_t freq) +{ + // 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 tryRegisterDrefs(void) +static void registerInavXitlDrefs(int32_t freq) { - 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); + // 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]", 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); + 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 void* listenWorker(void* arg) +static bool receiveSingleDref(dref_t in_dref, float* value) { - UNUSED(arg); + uint8_t buf[1024]; + struct sockaddr_storage remoteAddr; + socklen_t slen = sizeof(remoteAddr); + int recvLen; + recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen); + if (recvLen < 0 && errno != EWOULDBLOCK && errno != EAGAIN) { + return false; + } + + if (recvLen < 5 || strncmp((char*)buf, "RREF", 4) != 0) { + return false; + } + + 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; - while (!atomic_load(&shouldStopListenThread)) - { - if (!atomic_load(&initalized)) { - tryRegisterDrefs(); + 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++; } + } + + 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); + } + + 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 && errno != EAGAIN) { + return; + } else if (recvLen == 0 || (recvLen == -1 && errno == ECONNRESET)) { + connectionState = DISCONNECTED; + return; + } + + if (strncmp((char*)buf, "RREF", 4) != 0) { + return; + } - float motorValue = 0; - float yokeValues[3] = { 0 }; - int y = 0; - for (int i = 0; i < mappingCount; i++) { - if (y > 2) { + for (int i = 5; i < recvLen; i += 8) { + dref_t dref = (dref_t)xint2uint32(&buf[i]); + float value = xflt2float(&(buf[i + 4])); + + switch (dref) { + case DREF_LATITUDE: + lattitude = value; 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++; - } - } - 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) { - delay(250); - continue; - } + case DREF_LONGITUDE: + longitude = value; + break; - if (strncmp((char*)buf, "RREF", 4) != 0) { - delay(250); - continue; - } + 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; + case DREF_XITL_NUM_SATS: + numSats = (int)value; + break; - case DREF_JOYSTICK_VALUES_CH6: - joystickRaw[5] = value; - break; + case DREF_XITL_FIX_TYPE: + fixType = (int)value; + break; - case DREF_JOYSTICK_VALUES_CH7: - joystickRaw[6] = value; - break; + case DREF_XITL_MAG_X: + magX = value; + break; - case DREF_JOYSTICK_VALUES_CH8: - joystickRaw[7] = value; - break; + case DREF_XITL_MAG_Y: + magY = value; + break; - default: - break; - } - } + case DREF_XITL_MAG_Z: + magZ = value; + break; - if (hpath < 0) { - hpath += 3600; - } + case DREF_XITL_RANGEFINDER: + rangefinderAltitude = (int32_t)value; + break; - if (yaw < 0){ - yaw += 3600; - } + 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; - 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_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) + ); - fakeBaroSet((int32_t)roundf(barometer * 3386.39f), DEGREES_TO_CENTIDEGREES(21)); - fakePitotSetAirspeed(airspeed * 100.0f); + 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; @@ -471,35 +641,80 @@ static void* listenWorker(void* arg) constrainToInt16(north.y * 1024.0f), constrainToInt16(north.z * 1024.0f) ); + } - if (!atomic_load(&initalized)) { - ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); - // Aircraft can wobble on the runway and prevents calibration of the accelerometer - ENABLE_STATE(ACCELEROMETER_CALIBRATED); - atomic_store(&initalized, true); - pthread_mutex_lock(&initMutex); - pthread_cond_signal(&initCond); - pthread_mutex_unlock(&initMutex); - } + unlockMainPID(); +} + +static void* listenWorker(void* arg) +{ + UNUSED(arg); + + while (connectionState != DISCONNECTED) { + connectionState_t cs = connectionState; + switch (cs) { + case CONNECTING: - unlockMainPID(); + // 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"); 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; } @@ -508,7 +723,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool return false; } else { char addrbuf[IPADDRESS_PRINT_BUFLEN]; - char *nptr = prettyPrintAddress((struct sockaddr *)&serverAddr, 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); } @@ -517,38 +732,32 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool struct timeval tv; tv.tv_sec = 1; tv.tv_usec = 0; - if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { + 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; } - atomic_store(&initalized, false); - atomic_store(&shouldStopListenThread, false); if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) { return false; } - pthread_mutex_lock(&initMutex); - while (!atomic_load(&initalized)) { - pthread_cond_wait(&initCond, &initMutex); + while (connectionState != CONNECTED && connectionState != DISCONNECTED) { + delay(250); } - pthread_mutex_unlock(&initMutex); - return true; + return connectionState == CONNECTED; } void simXPlaneClose(void) { - atomic_store(&shouldStopListenThread, true); - if (listenThread) { - pthread_join(listenThread, NULL); - } - DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); - atomic_store(&initalized, false); - pthread_mutex_destroy(&initMutex); - pthread_cond_destroy(&initCond); - close(sockFd); + connectionState = DISCONNECTED; // signal worker to exit its loop + close(sockFd); // unblock any pending recvfrom + if (listenThread) { + pthread_join(listenThread, NULL); + listenThread = 0; + } + DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); } diff --git a/src/main/target/SITL/sim/xplane.h b/src/main/target/SITL/sim/xplane.h index 619bef615e0..dc90fc3376f 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/SPEDIXH743/CMakeLists.txt b/src/main/target/SPEDIXH743/CMakeLists.txt new file mode 100644 index 00000000000..1cb49214f88 --- /dev/null +++ b/src/main/target/SPEDIXH743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(SPEDIXH743) diff --git a/src/main/target/SPEDIXH743/config.c b/src/main/target/SPEDIXH743/config.c new file mode 100644 index 00000000000..de3f23448bc --- /dev/null +++ b/src/main/target/SPEDIXH743/config.c @@ -0,0 +1,28 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; // PINIO1: VTX power switch + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; // PINIO2: Camera switch +} diff --git a/src/main/target/SPEDIXH743/target.c b/src/main/target/SPEDIXH743/target.c new file mode 100644 index 00000000000..210d45cf210 --- /dev/null +++ b/src/main/target/SPEDIXH743/target.c @@ -0,0 +1,56 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +// Gyro 1: ICM42688P on SPI1, tag=0 +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, ICM42605_1_SPI_BUS, ICM42605_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +// Gyro 2: ICM42688P on SPI4, tag=1 +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, ICM42605_2_SPI_BUS, ICM42605_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_ICM42605_2_ALIGN); + +timerHardware_t timerHardware[] = { + // ---- Motors (TIM1 x4) ---- + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M2 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 3), // M4 + + // ---- Motors (TIM8 x2) ---- + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 4), // M5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 5), // M6 + + // ---- Motors (TIM3 x2) ---- + DEF_TIM(TIM3, CH1, PA6, TIM_USE_OUTPUT_AUTO, 0, 6), // M7 + DEF_TIM(TIM3, CH2, PA7, TIM_USE_OUTPUT_AUTO, 0, 7), // M8 + + // ---- LED Strip (TIM5 CH1) ---- + DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 8), // WS2812 LED Strip + + // ---- Camera Control (PA1 / TIM15_CH1N) ---- + DEF_TIM(TIM15, CH1N, PA1, TIM_USE_ANY, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEDIXH743/target.h b/src/main/target/SPEDIXH743/target.h new file mode 100644 index 00000000000..c80abf4b79b --- /dev/null +++ b/src/main/target/SPEDIXH743/target.h @@ -0,0 +1,187 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SPX7" +#define USBD_PRODUCT_STRING "SPEDIXH743" + +#define USE_TARGET_CONFIG + +// *************** Indicators *************************** +#define LED0 PC4 +#define LED1 PC5 + +#define BEEPER PD14 +#define BEEPER_INVERTED + +// *************** IMU generic ************************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_IMU_ICM42605 + +// *************** SPI1 — Gyro 1 (ICM42688P) ************ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define ICM42605_1_SPI_BUS BUS_SPI1 +#define ICM42605_1_CS_PIN PA15 +#define IMU_ICM42605_ALIGN CW0_DEG + +// *************** SPI4 — Gyro 2 (ICM42688P) ************ +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + +#define ICM42605_2_SPI_BUS BUS_SPI4 +#define ICM42605_2_CS_PIN PE3 +#define IMU_ICM42605_2_ALIGN CW0_DEG + +// *************** SPI2 — OSD (MAX7456) ***************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SPI3 — Flash (M25P16 blackbox) ******* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA8 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C — Baro (BMP280/DPS310) *********** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define USE_RANGEFINDER + +// *************** UART ********************************* +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PD1 +#define UART4_RX_PIN PD0 + +#define USE_UART5 +#define UART5_TX_PIN PB6 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +// VCP + 8 UARTs +#define SERIAL_PORT_COUNT 9 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** ADC ********************************** +#define USE_ADC +#define ADC_INSTANCE ADC3 // PC-port pins require ADC3 on H743 + +#define ADC_CHANNEL_1_PIN PC1 // VBat — ADC123_INP11 (works on ADC3) +#define ADC_CHANNEL_2_PIN PC2 // Current — ADC3_INP0 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#define VBAT_SCALE_DEFAULT 1100 + +// *************** PINIO ******************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA2 // VTX power switch +#define PINIO2_PIN PA3 // Camera switch + +// *************** LED STRIP **************************** +#define USE_LED_STRIP +#define WS2811_PIN PA0 + +// *************** Features / defaults ****************** +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_DSHOT_DMAR +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 8 + +// *************** Pin availability ********************* +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff diff --git a/src/main/target/common.h b/src/main/target/common.h index 58b572495fe..6990cfd4415 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -181,6 +181,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