diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ed45dfc5bb..09e27045ca 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -38,9 +38,9 @@ jobs: - name: Build targets (${{ matrix.id }}) run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci - name: Upload artifacts - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v3 with: - name: ${{ env.BUILD_NAME }}.zip + name: ${{ env.BUILD_NAME }} path: ./build/*.hex build-SITL-Linux: @@ -68,9 +68,45 @@ jobs: - name: Build SITL run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja - name: Upload artifacts - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v3 with: - name: ${{ env.BUILD_NAME }}_SITL.zip + name: ${{ env.BUILD_NAME }}_SITL + path: ./build_SITL/*_SITL + + build-SITL-Mac: + runs-on: macos-latest + steps: + - uses: actions/checkout@v3 + - name: Install dependencies + run: | + brew install cmake ninja ruby + + - name: Setup environment + env: + ACTIONS_ALLOW_UNSECURE_COMMANDS: true + run: | + # This is the hash of the commit for the PR + # when the action is triggered by PR, empty otherwise + COMMIT_ID=${{ github.event.pull_request.head.sha }} + # This is the hash of the commit when triggered by push + # but the hash of refs/pull//merge, which is different + # from the hash of the latest commit in the PR, that's + # why we try github.event.pull_request.head.sha first + COMMIT_ID=${COMMIT_ID:-${{ github.sha }}} + BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID}) + VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') + echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + - name: Build SITL + run: | + mkdir -p build_SITL && cd build_SITL + cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. + ninja + + - name: Upload artifacts + uses: actions/upload-artifact@v3 + with: + name: ${{ env.BUILD_NAME }}_SITL-MacOS path: ./build_SITL/*_SITL build-SITL-Windows: @@ -103,9 +139,9 @@ jobs: - name: Build SITL run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja - name: Upload artifacts - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v3 with: - name: ${{ env.BUILD_NAME }}_SITL-WIN.zip + name: ${{ env.BUILD_NAME }}_SITL-WIN path: ./build_SITL/*.exe diff --git a/CMakeLists.txt b/CMakeLists.txt index d3806df408..af42e31d7c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,10 +14,10 @@ option(SITL "SITL build for host system" OFF) set(TOOLCHAIN_OPTIONS none arm-none-eabi host) if (SITL) - set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}") - if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + if (CMAKE_HOST_APPLE) set(MACOSX TRUE) endif() + set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}") else() set(TOOLCHAIN "arm-none-eabi" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}") endif() diff --git a/cmake/at32.cmake b/cmake/at32.cmake index 2722798669..aa902593e9 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -15,6 +15,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP") set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include") set(CMSIS_DSP_SRC + BasicMathFunctions/arm_scale_f32.c + BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c diff --git a/cmake/host.cmake b/cmake/host.cmake index 1ed8c7432b..ea29feb826 100644 --- a/cmake/host.cmake +++ b/cmake/host.cmake @@ -16,7 +16,11 @@ set(CMAKE_CXX_COMPILER "g++${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "c++ compil set(CMAKE_OBJCOPY "objcopy${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "objcopy tool") set(CMAKE_OBJDUMP "objdump${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "objdump tool") set(CMAKE_SIZE "size${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "size tool") -set(CMAKE_DEBUGGER "gdb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger") +if(CMAKE_HOST_APPLE) + set(CMAKE_DEBUGGER "lldb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger") +else() + set(CMAKE_DEBUGGER "gdb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger") +endif() set(CMAKE_CPPFILT "c++filt${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "c++filt") set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "Build Type" FORCE) @@ -24,7 +28,11 @@ set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${CMAKE_CONFIGURATION_TYPES set(debug_options "-Og -O0 -g") set(release_options "-Os -DNDEBUG") -set(relwithdebinfo_options "-ggdb3 ${release_options}") +if(CMAKE_HOST_APPLE) + set(relwithdebinfo_options "-g ${release_options}") +else() + set(relwithdebinfo_options "-ggdb3 ${release_options}") +endif() set(CMAKE_C_FLAGS_DEBUG ${debug_options} CACHE INTERNAL "c compiler flags debug") set(CMAKE_CXX_FLAGS_DEBUG ${debug_options} CACHE INTERNAL "c++ compiler flags debug") diff --git a/cmake/main.cmake b/cmake/main.cmake index 8c10523671..d8e2dddf3e 100644 --- a/cmake/main.cmake +++ b/cmake/main.cmake @@ -83,7 +83,7 @@ function(setup_executable exe name) set_target_properties(${exe} PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin" ) - if(IS_RELEASE_BUILD) + if(IS_RELEASE_BUILD AND NOT (CMAKE_HOST_APPLE AND SITL)) set_target_properties(${exe} PROPERTIES INTERPROCEDURAL_OPTIMIZATION ON ) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index ab03c7b0d9..24a4ae9b27 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -26,7 +26,7 @@ main_sources(SITL_SRC ) -if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") +if(CMAKE_HOST_APPLE) set(MACOSX ON) endif() @@ -59,6 +59,12 @@ if(NOT MACOSX) -Wno-error=maybe-uninitialized -fsingle-precision-constant ) + if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0) + set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-Wl,--no-warn-rwx-segments") + endif() +else() + set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} + ) endif() set(SITL_DEFINITIONS diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index f852e44e0e..f02185e9af 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -16,6 +16,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP") set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include") set(CMSIS_DSP_SRC + BasicMathFunctions/arm_scale_f32.c + BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c diff --git a/cmake/stm32h7.cmake b/cmake/stm32h7.cmake index 70400fb9f0..08e7686b8f 100644 --- a/cmake/stm32h7.cmake +++ b/cmake/stm32h7.cmake @@ -149,6 +149,8 @@ main_sources(STM32H7_SRC drivers/bus_i2c_hal.c drivers/dma_stm32h7xx.c drivers/bus_spi_hal_ll.c + drivers/bus_quadspi.c + drivers/bus_quadspi_hal.c drivers/memprot.h drivers/memprot_hal.c drivers/memprot_stm32h7xx.c diff --git a/docs/Buzzer.md b/docs/Buzzer.md index 62e2c8527e..aaa04b5d08 100644 --- a/docs/Buzzer.md +++ b/docs/Buzzer.md @@ -62,12 +62,14 @@ beeper list giving: ``` -Available: RUNTIME_CALIBRATION HW_FAILURE RX_LOST RX_LOST_LANDING DISARMING ARMING ARMING_GPS_FIX BAT_CRIT_LOW BAT_LOW GPS_STATUS RX_SET ACTION_SUCCESS ACTION_FAIL READY_BEEP MULTI_BEEPS DISARM_REPEAT ARMED SYSTEM_INIT ON_USB LAUNCH_MODE CAM_CONNECTION_OPEN CAM_CONNECTION_CLOSED ALL PREFERED +Available: RUNTIME_CALIBRATION HW_FAILURE RX_LOST RX_LOST_LANDING DISARMING ARMING ARMING_GPS_FIX BAT_CRIT_LOW +BAT_LOW GPS_STATUS RX_SET ACTION_SUCCESS ACTION_FAIL READY_BEEP MULTI_BEEPS DISARM_REPEAT ARMED SYSTEM_INIT +ON_USB LAUNCH_MODE CAM_CONNECTION_OPEN CAM_CONNECTION_CLOSED ALL PREFERED ``` The `beeper` command syntax follows that of the `feature` command; a minus (`-`) in front of a name disables that function. -So to disable the beeper / buzzer when connected to USB (may enhance domestic harmony) +So to disable the beeper / buzzer when powered by USB (may enhance domestic harmony): ``` beeper -ON_USB @@ -78,17 +80,39 @@ Now the `beeper` command will show: ``` # beeper Disabled: ON_USB +``` + +*Note: SYSTEM_INIT sequence is not affected by ON_USB setting and will still be played on USB connection. Disable both ON_USB and SYSTEM_INIT to disable buzzer completely when FC is powered from USB.* + +*Note: ON_USB setting requires present and configured battery voltage metter.* + +To disable all features use: ``` +beeper -ALL +``` + +To store current set to preferences use (preferences also require ```save```): + +``` +beeper PREFERED +``` + +To restore set from preferences use: + +``` +beeper -PREFERED As with other CLI commands, the `save` command is needed to save the new settings. ## Types of buzzer supported -The buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board. +Most FCs require ACTIVE buzzers. Active buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board. This means the buzzer must be able to generate its own tone simply by having power applied to it. -Buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all. +Passive buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all. + +Passive buzzers are supported on FCs which are designed to work with passive buzzers only (so far there is no available, except rare cases like Matek F765-WSE where passive buzzer is preinstalled). Examples of a known-working buzzers. diff --git a/docs/Controls.md b/docs/Controls.md index f4013cbaff..1b63f90e2a 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -44,6 +44,10 @@ The stick positions are combined to activate different functions: | Bypass Nav Arm disable | LOW | HIGH | CENTER | CENTER | | Save setting | LOW | LOW | LOW | HIGH | | Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | +| Enter Camera OSD(RuncamDevice)| RIGHT | CENTER | CENTER | CENTER | +| Exit Camera OSD (RuncamDevice)| LEFT | CENTER | CENTER | CENTER | +| Confirm - Camera OSD | RIGHT | CENTER | CENTER | CENTER | +| Navigation - Camera OSD | CENTER | CENTER | * | * | For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/). ![Stick Positions](assets/images/StickPositions.png) diff --git a/docs/Navigation.md b/docs/Navigation.md index 559b586161..6caf4dc628 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -4,11 +4,14 @@ The navigation system in INAV is responsible for assisting the pilot allowing al ## NAV ALTHOLD mode - altitude hold -Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. GPS is available as an altitude source for airplanes only. +Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - quad goes up, pilot moves stick down - quad descends, you keep stick at neutral position - quad hovers. +By default, GPS is available as an altitude source for airplanes only. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. + ### CLI parameters affecting ALTHOLD mode: * *nav_use_midthr_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position. +* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. ### Related PIDs PIDs affecting altitude hold: ALT & VEL @@ -22,12 +25,13 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co ## NAV POSHOLD mode - position hold -Position hold requires GPS, accelerometer and compass sensors. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated. +Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated. When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From INAV 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically. ### CLI parameters affecting POSHOLD mode: * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own. - +* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. + ### Related PIDs PIDs affecting position hold: POS, POSR PID meaning: @@ -38,9 +42,9 @@ PID meaning: Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors. -If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot. +RTH requires barometer for multirotor, unless unless *inav_use_gps_no_baro* is enabled. -If barometer is present, RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings. +RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings. When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables): * 0 (NAV_RTH_NO_ALT) - keep current altitude during whole RTH sequence (*nav_rth_altitude* is ignored) * 1 (NAV_RTH_EXTRA_ALT) - climb to current altitude plus extra margin prior to heading home (*nav_rth_altitude* defines the extra altitude (cm)) diff --git a/docs/OSD.md b/docs/OSD.md new file mode 100644 index 0000000000..64530f0e2c --- /dev/null +++ b/docs/OSD.md @@ -0,0 +1,167 @@ +# On Screen Display + +The On Screen Display, or OSD, is a feature that overlays flight data over the video image. This can be done on the flight controller, using the analogue MAX7456 chip. Digital systems take the OSD data, via MSP DisplayPort, send it to the video receiver; which combines the data with the image. You can specify what elements are displayed, and their locations on the image. Most systems are character based, and use the MAX7456 analogue setup, or MSP DisplayPort. However, there are some different systems which are also supported. Such as the canvas based FrSKY PixelOSD on analogue. Canvas OSDs draw shapes on the image. Whereas character based OSDs use font characters to display the data. + +## Features and Limitations +Not all OSDs are created equally. This table shows the differences between the different systems available. + +| OSD System | Character grid | Character | Canvas | MSP DisplayPort | All elements supported | +|---------------|-------------------|-----------|-----------|-------------------|---------------------------| +| Analogue PAL | 30 x 16 | X | | | YES | +| Analogue NTSC | 30 x 13 | X | | | YES | +| PixelOSD | As PAL or NTSC | | X | | YES | +| DJI OSD | 30 x 16 | X | | | NO - BF Characters only | +| DJI WTFOS | 60 x 22 | X | | X | YES | +| HDZero | 50 x 18 | X | | X | YES | +| Avatar | 53 x 20 | X | | X | YES | +| DJI O3 | 53 x 20 (HD) | X | | X (partial) | NO - BF Characters only | + +## OSD Elements +Here are the OSD Elements provided by INAV. + +| ID | Element | Added | +|-------|-----------------------------------------------|-------| +| 0 | OSD_RSSI_VALUE | 1.0.0 | +| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 | +| 2 | OSD_CROSSHAIRS | 1.0.0 | +| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 | +| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 | +| 5 | OSD_ONTIME | 1.0.0 | +| 6 | OSD_FLYTIME | 1.0.0 | +| 7 | OSD_FLYMODE | 1.0.0 | +| 8 | OSD_CRAFT_NAME | 1.0.0 | +| 9 | OSD_THROTTLE_POS | 1.0.0 | +| 10 | OSD_VTX_CHANNEL | 1.0.0 | +| 11 | OSD_CURRENT_DRAW | 1.0.0 | +| 12 | OSD_MAH_DRAWN | 1.0.0 | +| 13 | OSD_GPS_SPEED | 1.0.0 | +| 14 | OSD_GPS_SATS | 1.0.0 | +| 15 | OSD_ALTITUDE | 1.0.0 | +| 16 | OSD_ROLL_PIDS | 1.6.0 | +| 17 | OSD_PITCH_PIDS | 1.6.0 | +| 18 | OSD_YAW_PIDS | 1.6.0 | +| 19 | OSD_POWER | 1.6.0 | +| 20 | OSD_GPS_LON | 1.6.0 | +| 21 | OSD_GPS_LAT | 1.6.0 | +| 22 | OSD_HOME_DIR | 1.6.0 | +| 23 | OSD_HOME_DIST | 1.6.0 | +| 24 | OSD_HEADING | 1.6.0 | +| 25 | OSD_VARIO | 1.6.0 | +| 26 | OSD_VARIO_NUM | 1.6.0 | +| 27 | OSD_AIR_SPEED | 1.7.3 | +| 28 | OSD_ONTIME_FLYTIME | 1.8.0 | +| 29 | OSD_RTC_TIME | 1.8.0 | +| 30 | OSD_MESSAGES | 1.8.0 | +| 31 | OSD_GPS_HDOP | 1.8.0 | +| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 | +| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 | +| 34 | OSD_HEADING_GRAPH | 1.8.0 | +| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 | +| 36 | OSD_WH_DRAWN | 1.9.0 | +| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 | +| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 | +| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 | +| 40 | OSD_TRIP_DIST | 1.9.1 | +| 41 | OSD_ATTITUDE_PITCH | 2.0.0 | +| 42 | OSD_ATTITUDE_ROLL | 2.0.0 | +| 43 | OSD_MAP_NORTH | 2.0.0 | +| 44 | OSD_MAP_TAKEOFF | 2.0.0 | +| 45 | OSD_RADAR | 2.0.0 | +| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 | +| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 | +| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 | +| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 | +| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 | +| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 | +| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 | +| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 | +| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 | +| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 | +| 56 | OSD_LEVEL_PIDS | 2.0.0 | +| 57 | OSD_POS_XY_PIDS | 2.0.0 | +| 58 | OSD_POS_Z_PIDS | 2.0.0 | +| 59 | OSD_VEL_XY_PIDS | 2.0.0 | +| 60 | OSD_VEL_Z_PIDS | 2.0.0 | +| 61 | OSD_HEADING_P | 2.0.0 | +| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 | +| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 | +| 64 | OSD_RC_EXPO | 2.0.0 | +| 65 | OSD_RC_YAW_EXPO | 2.0.0 | +| 66 | OSD_THROTTLE_EXPO | 2.0.0 | +| 67 | OSD_PITCH_RATE | 2.0.0 | +| 68 | OSD_ROLL_RATE | 2.0.0 | +| 69 | OSD_YAW_RATE | 2.0.0 | +| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 | +| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 | +| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 | +| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 | +| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 | +| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 | +| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 | +| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 | +| 78 | OSD_DEBUG | 2.0.0 | +| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 | +| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 | +| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 | +| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 | +| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 | +| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 | +| 85 | OSD_3D_SPEED | 2.1.0 | +| 86 | OSD_IMU_TEMPERATURE | 2.1.0 | +| 87 | OSD_BARO_TEMPERATURE | 2.1.0 | +| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 | +| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 | +| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 | +| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 | +| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 | +| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 | +| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 | +| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 | +| 96 | OSD_ALTITUDE_MSL | 2.1.0 | +| 97 | OSD_PLUS_CODE | 2.1.0 | +| 98 | OSD_MAP_SCALE | 2.2.0 | +| 99 | OSD_MAP_REFERENCE | 2.2.0 | +| 100 | OSD_GFORCE | 2.2.0 | +| 101 | OSD_GFORCE_X | 2.2.0 | +| 102 | OSD_GFORCE_Y | 2.2.0 | +| 103 | OSD_GFORCE_Z | 2.2.0 | +| 104 | OSD_RC_SOURCE | 2.2.0 | +| 105 | OSD_VTX_POWER | 2.2.0 | +| 106 | OSD_ESC_RPM | 2.3.0 | +| 107 | OSD_ESC_TEMPERATURE | 2.5.0 | +| 108 | OSD_AZIMUTH | 2.6.0 | +| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 | +| 110 | OSD_CRSF_LQ | 2.6.0 | +| 111 | OSD_CRSF_SNR_DB | 2.6.0 | +| 112 | OSD_CRSF_TX_POWER | 2.6.0 | +| 113 | OSD_GVAR_0 | 2.6.0 | +| 114 | OSD_GVAR_1 | 2.6.0 | +| 115 | OSD_GVAR_2 | 2.6.0 | +| 116 | OSD_GVAR_3 | 2.6.0 | +| 117 | OSD_TPA | 2.6.0 | +| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 | +| 119 | OSD_VERSION | 3.0.0 | +| 120 | OSD_RANGEFINDER | 3.0.0 | +| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 | +| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 | +| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 | +| 124 | OSD_GLIDESLOPE | 3.0.1 | +| 125 | OSD_GPS_MAX_SPEED | 4.0.0 | +| 126 | OSD_3D_MAX_SPEED | 4.0.0 | +| 127 | OSD_AIR_MAX_SPEED | 4.0.0 | +| 128 | OSD_ACTIVE_PROFILE | 4.0.0 | +| 129 | OSD_MISSION | 4.0.0 | +| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 | +| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 | +| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 | +| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 | +| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 | +| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 | +| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 | +| 137 | OSD_GLIDE_RANGE | 6.0.0 | +| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 | +| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 | +| 140 | OSD_GROUND_COURSE | 6.0.0 | +| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 | +| 142 | OSD_PILOT_NAME | 6.0.0 | +| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 | \ No newline at end of file diff --git a/docs/Settings.md b/docs/Settings.md index f628beb44e..29eca27981 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1432,6 +1432,26 @@ Which SBAS mode to be used --- +### gps_ublox_nav_hz + +Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximum number of satellites tracked when set to a higher rate or even stop sending navigation updates if the value is too high. Some M10 devices can do up to 25Hz. 10 is a safe value for M8 and newer. + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 5 | 200 | + +--- + +### gps_ublox_use_beidou + +Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### gps_ublox_use_galileo Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. @@ -1442,6 +1462,16 @@ Enable use of Galileo satellites. This is at the expense of other regional const --- +### gps_ublox_use_glonass + +Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### ground_test_mode For developer ground test use. Disables motors, sets heading status = Trusted on FW. @@ -3202,6 +3232,16 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal --- +### nav_landing_bump_detection + +Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### nav_manual_climb_rate Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 68e9105d80..0fd5971cff 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions.svg b/docs/assets/images/StickPositions.svg deleted file mode 100644 index 218c5e1094..0000000000 --- a/docs/assets/images/StickPositions.svg +++ /dev/null @@ -1,1042 +0,0 @@ - - - - - - - - - - image/svg+xml - - - - - - - - - - - - - - - - - - - - Profile 1 - - - - - Profile 2 - - - - - Profile 3 - - - - - Calibrate Gyro - - - - - Calibrate Acc - - - - - Calibrate Compass - - - - - In-flight Calibration Controls - - - - - Trim Acc Left - - - - - Trim Acc Right - - - - - Trim Acc Forwards - - - - - Trim Acc Backwards - - - - - Save Waypoint Mission - - - - - - - Load Waypoint Mission - Unload Waypoint Mission - - - - - - - Save Setting - Enter OSD Menu (CMS) - Mode 2 Stick Functions - - - - Battery profile 1 - - - Battery profile 2 - - - Battery profile 3 - - - - - - - - - - - - - Inc WP Mission Index - - - - - Dec WP Mission Index - Bypass NAV Arm Disable - - - - - - - diff --git a/docs/assets/images/StickPositions_trans.png b/docs/assets/images/StickPositions_trans.png deleted file mode 100644 index 8ecf4ca4af..0000000000 Binary files a/docs/assets/images/StickPositions_trans.png and /dev/null differ diff --git a/docs/development/How to test a pull request.md b/docs/development/How to test a pull request.md new file mode 100644 index 0000000000..cb7abc57a4 --- /dev/null +++ b/docs/development/How to test a pull request.md @@ -0,0 +1,53 @@ +# Introduction +While many of the instructions here are somewhat generic and will likely work for other projects, the goal of these instructions is to assist a non-developer INAV user to acquire firmware that includes a pull request so he can flash it on his supported fc. +Building the pull request manually or using custom/unofficial targets is not the focus of this document. + +# Why should you test a pull request? +- You want to volunteer time and resources helping improving INAV for everyone by catching issues before they are introduced in the next release of INAV! +- You reported or are affected by a bug that has been addressed by this pull request and want to help test the fix +- You are interested in testing a new feature implemented by this pull request + +# Why should you not test a pull request? +- Pull requests are beta code and may have bugs; bugs may cause you to crash and damage your model +- Upgrading from the stable version of INAV may require changes to your config that are not yet fully documented + +# Before you proceed +- Read the comments on the pull request you want to test. It may provide useful context and information on known issues, required configuration changes or what branch of the inav-configurator is required. +- Make sure the pull request has passed all checks, otherwise you may not have pre-compiled firmware images. +- Make a diff all backup of your existing INAV configuration. +- Take notes of what INAV target you are using. +- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](http://seyrsnys.myzen.co.uk/inav-configurator-next/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI. + +# Finding the pull request +This is easy, but you will need to be logged in to your GitHub account. + +Navigate to the INAV github project and click on [``Pull Requests``](https://github.com/iNavFlight/inav/pulls). + +You can just scroll through the list to find a pull request you are interested in, or use the filter bar by typing the name of the pull request, or the number, if you know it. + +![Search results](assets/pr_testing/pr_search_result.png) + +Once you find the one you are looking for, go ahead an open it! + +Click on the ``Checks`` tab + +Click on ``Build firmware``, it should take you to the ``Actions`` tab. +![Search results](assets/pr_testing/build_firmware.png) + +You should see a summary with a column saying ``Artifacts`` and a number. Click on the number to be taken to the list of artifacts. + +![Search results](assets/pr_testing/actions_summary.png) + +On the ``Artifacts`` list, there should be an artifact without SITL in its name. + +![Search results](assets/pr_testing/artifact_listing.png) + + Click on it to download the zip file containing pre-compiled firmware images for all INAV official targets. Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings. + +# I have flashed the new firmware, what should I do next? + +- You should configure your model, either manually from scratch, or by loading your diff file. Keep in mind that loading a diff file may not always work, as there may have been some other changes in INAV that require attention. But even if you start from scratch, there are usually many sections that are safe to copy over from your diff. +- Try to reproduce the bug reported or play around with the new feature. +- Once you are done testing, don't forget to report your results on the pull request. Both positive results and issues are valid and welcome feedback. + + diff --git a/docs/development/assets/pr_testing/actions_summary.png b/docs/development/assets/pr_testing/actions_summary.png new file mode 100644 index 0000000000..88673a49be Binary files /dev/null and b/docs/development/assets/pr_testing/actions_summary.png differ diff --git a/docs/development/assets/pr_testing/artifact_listing.png b/docs/development/assets/pr_testing/artifact_listing.png new file mode 100644 index 0000000000..083e73060f Binary files /dev/null and b/docs/development/assets/pr_testing/artifact_listing.png differ diff --git a/docs/development/assets/pr_testing/build_firmware.png b/docs/development/assets/pr_testing/build_firmware.png new file mode 100644 index 0000000000..f4116508a3 Binary files /dev/null and b/docs/development/assets/pr_testing/build_firmware.png differ diff --git a/docs/development/assets/pr_testing/pr_search_result.png b/docs/development/assets/pr_testing/pr_search_result.png new file mode 100644 index 0000000000..e47101b46e Binary files /dev/null and b/docs/development/assets/pr_testing/pr_search_result.png differ diff --git a/lib/main/MAVLink/mavlink_helpers.h b/lib/main/MAVLink/mavlink_helpers.h index fed04d6885..b99f41ccab 100755 --- a/lib/main/MAVLink/mavlink_helpers.h +++ b/lib/main/MAVLink/mavlink_helpers.h @@ -574,8 +574,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { - int bufferIndex = 0; - status->msg_received = MAVLINK_FRAMING_INCOMPLETE; switch (status->parse_state) @@ -801,7 +799,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, break; } - bufferIndex++; // If a message has been sucessfully decoded, check index if (status->msg_received == MAVLINK_FRAMING_OK) { diff --git a/readme.md b/readme.md index 8382adc188..046cff3404 100644 --- a/readme.md +++ b/readme.md @@ -2,10 +2,6 @@ ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png) -# INAV 6 Horizon Hawk feature freeze - -> INAV 6 feature freeze will happen on 29th of January 2023. No new features for INAV 6 will be accepted after that date. - # INAV Community * [INAV Discord Server](https://discord.gg/peg2hhbYwN) @@ -18,13 +14,13 @@ * On Screen Display (OSD) - both character and pixel style * DJI OSD integration: all elements, system messages and warnings * Outstanding performance out of the box -* Position Hold, Altitude Hold, Return To Home and Missions -* Excellent support for fixed wing UAVs: airplanes, flying wings +* Position Hold, Altitude Hold, Return To Home and Waypoint Missions +* Excellent support for fixed wing UAVs: airplanes, flying wings * Fully configurable mixer that allows to run any hardware you want: multirotor, fixed wing, rovers, boats and other experimental devices * Multiple sensor support: GPS, Pitot tube, sonar, lidar, temperature, ESC with BlHeli_32 telemetry * SmartAudio and IRC Tramp VTX support * Blackbox flight recorder logging -* Telemetry: SmartPort, FPort, MAVlink, LTM +* Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF * Multi-color RGB LED Strip support * Advanced gyro filtering * Logic Conditions, Global Functions and Global Variables: you can program INAV with a GUI @@ -36,15 +32,19 @@ For a list of features, changes and some discussion please review consult the re ### INAV Configurator -Official tool for INAV can be downloaded [here](https://github.com/iNavFlight/inav-configurator/releases). It can be run on Windows, MacOS and Linux machines and standalone application. +Official tool for INAV can be downloaded [here](https://github.com/iNavFlight/inav-configurator/releases). It can be run on Windows, MacOS and Linux machines and standalone application. ### INAV Blackbox Explorer Tool for Blackbox logs analysis is available [here](https://github.com/iNavFlight/blackbox-log-viewer/releases) -### Telemetry screen for OpenTX +### INAV Blackbox Tools -Users of OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) +Command line tools (`blackbox_decode`, `blackbox_render`) for Blackbox log conversion and analysis [here](https://github.com/iNavFlight/blackbox-tools). + +### Telemetry screen for EdgeTX and OpenTX + +Users of EdgeTX and OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) ### INAV magnetometer alignment helper @@ -81,7 +81,7 @@ Contributions are welcome and encouraged. You can contribute in many ways: * Telling us your ideas and suggestions. * Buying your hardware from this [link](https://inavflight.com/shop/u/bg/) -A good place to start is Telegram channel or Facebook group. Drop in, say hi. +A good place to start is the Discord channel, Telegram channel or Facebook group. Drop in, say hi. Github issue tracker is a good place to search for existing issues or report a new bug/feature request: diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 2a31144354..5badd5c5f1 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -499,6 +499,7 @@ main_sources(COMMON_SRC io/gps.c io/gps.h io/gps_ublox.c + io/gps_ublox_utils.c io/gps_nmea.c io/gps_msp.c io/gps_fake.c @@ -531,6 +532,8 @@ main_sources(COMMON_SRC io/vtx_ffpv24g.h io/vtx_control.c io/vtx_control.h + io/vtx_msp.c + io/vtx_msp.h navigation/navigation.c navigation/navigation.h diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0123e0d63c..0169a30995 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1326,12 +1326,19 @@ static void loadSlowState(blackboxSlowState_t *slow) #endif bool valid_temp; - valid_temp = getIMUTemperature(&slow->imuTemperature); - if (!valid_temp) slow->imuTemperature = TEMPERATURE_INVALID_VALUE; + int16_t newTemp = 0; + valid_temp = getIMUTemperature(&newTemp); + if (valid_temp) + slow->imuTemperature = newTemp; + else + slow->imuTemperature = TEMPERATURE_INVALID_VALUE; #ifdef USE_BARO - valid_temp = getBaroTemperature(&slow->baroTemperature); - if (!valid_temp) slow->baroTemperature = TEMPERATURE_INVALID_VALUE; + valid_temp = getBaroTemperature(&newTemp); + if (valid_temp) + slow->baroTemperature = newTemp; + else + slow->baroTemperature = TEMPERATURE_INVALID_VALUE; #endif #ifdef USE_TEMPERATURE_SENSOR diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 54add560df..4bde1291d5 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -330,13 +330,6 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_BACK_AND_END_ENTRY, }; -#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR) && defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) -// All CMS OSD elements should be enabled in this case. The menu has 2 extra -// elements (label, back+end), but there's an OSD element that we intentionally -// don't show here (OSD_DEBUG). -_Static_assert(ARRAYLEN(menuOsdElemsEntries) - 2 + 1 == OSD_ITEM_COUNT, "missing OSD elements in CMS"); -#endif - const CMS_Menu menuOsdElements = { #ifdef CMS_MENU_DEBUG .GUARD_text = "MENUOSDE", diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index 6ae5a61013..9b2b06646c 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -192,7 +192,7 @@ typedef struct OSD_SETTING_s { const uint8_t step; } __attribute__((packed)) OSD_SETTING_t; -#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type) { name, NULL, &(const OSD_SETTING_t){ setting, step }, OME_Setting, type } +#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type) { name, { NULL }, &(const OSD_SETTING_t){ setting, step }, OME_Setting, type } #define OSD_SETTING_ENTRY_TYPE(name, setting, type) OSD_SETTING_ENTRY_STEP_TYPE(name, setting, 0, type) #define OSD_SETTING_ENTRY_STEP(name, setting, step) OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, 0) #define OSD_SETTING_ENTRY(name, setting) OSD_SETTING_ENTRY_STEP(name, setting, 0) diff --git a/src/main/common/constants.h b/src/main/common/constants.h index 540046cfa3..5447fa184e 100644 --- a/src/main/common/constants.h +++ b/src/main/common/constants.h @@ -18,9 +18,9 @@ #pragma once #define FEET_PER_MILE 5280 -#define FEET_PER_NAUTICALMILE 6076.118 +#define FEET_PER_NAUTICALMILE 6076.118f #define FEET_PER_KILOFEET 1000 // Used for altitude #define METERS_PER_KILOMETER 1000 -#define METERS_PER_MILE 1609.344 -#define METERS_PER_FOOT 3.28084 -#define METERS_PER_NAUTICALMILE 1852.001 +#define METERS_PER_MILE 1609.344f +#define METERS_PER_FOOT 3.28084f +#define METERS_PER_NAUTICALMILE 1852.001f diff --git a/src/main/common/maths.c b/src/main/common/maths.c index ac7a70d24c..1bc61ae4c3 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -380,36 +380,36 @@ void sensorCalibrationResetState(sensorCalibrationState_t * state) } } -void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]) +void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3]) { - state->XtX[0][0] += (float)sample[0] * sample[0]; - state->XtX[0][1] += (float)sample[0] * sample[1]; - state->XtX[0][2] += (float)sample[0] * sample[2]; - state->XtX[0][3] += (float)sample[0]; + state->XtX[0][0] += sample[0] * sample[0]; + state->XtX[0][1] += sample[0] * sample[1]; + state->XtX[0][2] += sample[0] * sample[2]; + state->XtX[0][3] += sample[0]; - state->XtX[1][0] += (float)sample[1] * sample[0]; - state->XtX[1][1] += (float)sample[1] * sample[1]; - state->XtX[1][2] += (float)sample[1] * sample[2]; - state->XtX[1][3] += (float)sample[1]; + state->XtX[1][0] += sample[1] * sample[0]; + state->XtX[1][1] += sample[1] * sample[1]; + state->XtX[1][2] += sample[1] * sample[2]; + state->XtX[1][3] += sample[1]; - state->XtX[2][0] += (float)sample[2] * sample[0]; - state->XtX[2][1] += (float)sample[2] * sample[1]; - state->XtX[2][2] += (float)sample[2] * sample[2]; - state->XtX[2][3] += (float)sample[2]; + state->XtX[2][0] += sample[2] * sample[0]; + state->XtX[2][1] += sample[2] * sample[1]; + state->XtX[2][2] += sample[2] * sample[2]; + state->XtX[2][3] += sample[2]; - state->XtX[3][0] += (float)sample[0]; - state->XtX[3][1] += (float)sample[1]; - state->XtX[3][2] += (float)sample[2]; + state->XtX[3][0] += sample[0]; + state->XtX[3][1] += sample[1]; + state->XtX[3][2] += sample[2]; state->XtX[3][3] += 1; - float squareSum = ((float)sample[0] * sample[0]) + ((float)sample[1] * sample[1]) + ((float)sample[2] * sample[2]); + float squareSum = (sample[0] * sample[0]) + (sample[1] * sample[1]) + (sample[2] * sample[2]); state->XtY[0] += sample[0] * squareSum; state->XtY[1] += sample[1] * squareSum; state->XtY[2] += sample[2] * squareSum; state->XtY[3] += squareSum; } -void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target) +void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target) { for (int i = 0; i < 3; i++) { float scaledSample = (float)sample[i] / (float)target; @@ -521,7 +521,7 @@ float bellCurve(const float x, const float curveWidth) return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth))); } -float fast_fsqrtf(const double value) { +float fast_fsqrtf(const float value) { float ret = 0.0f; #ifdef USE_ARM_MATH arm_sqrt_f32(value, &ret); @@ -538,11 +538,57 @@ float fast_fsqrtf(const double value) { // function to calculate the normalization (pythagoras) of a 2-dimensional vector float NOINLINE calc_length_pythagorean_2D(const float firstElement, const float secondElement) { - return fast_fsqrtf(sq(firstElement) + sq(secondElement)); + return fast_fsqrtf(sq(firstElement) + sq(secondElement)); } // function to calculate the normalization (pythagoras) of a 3-dimensional vector float NOINLINE calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement) { - return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement)); -} \ No newline at end of file + return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement)); +} + +#ifdef SITL_BUILD + +/** + * @brief Floating-point vector subtraction, equivalent of CMSIS arm_sub_f32. +*/ +void arm_sub_f32( + float * pSrcA, + float * pSrcB, + float * pDst, + uint32_t blockSize) +{ + for (uint32_t i = 0; i < blockSize; i++) { + pDst[i] = pSrcA[i] - pSrcB[i]; + } +} + +/** + * @brief Floating-point vector scaling, equivalent of CMSIS arm_scale_f32. +*/ +void arm_scale_f32( + float * pSrc, + float scale, + float * pDst, + uint32_t blockSize) +{ + for (uint32_t i = 0; i < blockSize; i++) { + pDst[i] = pSrc[i] * scale; + } +} + +/** + * @brief Floating-point vector multiplication, equivalent of CMSIS arm_mult_f32. +*/ +void arm_mult_f32( + float * pSrcA, + float * pSrcB, + float * pDst, + uint32_t blockSize) +{ + for (uint32_t i = 0; i < blockSize; i++) { + pDst[i] = pSrcA[i] * pSrcB[i]; + } +} + +#endif \ No newline at end of file diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 690ca7c0d9..cc3d1bc517 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -36,13 +36,13 @@ #define RAD (M_PIf / 180.0f) #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) -#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100) +#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f) -#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10) +#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f) #define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10) #define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) -#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10) +#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10.0f) #define DEGREES_PER_DEKADEGREE 10 #define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE) @@ -56,15 +56,15 @@ #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD) -#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048) -#define CENTIMETERS_TO_FEET(cm) (cm / 30.48) -#define CENTIMETERS_TO_METERS(cm) (cm / 100) +#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) +#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) +#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f) #define METERS_TO_CENTIMETERS(m) (m * 100) -#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363) -#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6) -#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845) +#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) +#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) +#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) #define C_TO_KELVIN(temp) (temp + 273.15f) @@ -137,8 +137,8 @@ typedef struct { } sensorCalibrationState_t; void sensorCalibrationResetState(sensorCalibrationState_t * state); -void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]); -void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target); +void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3]); +void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target); bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]); bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]); @@ -188,6 +188,23 @@ float acos_approx(float x); void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); float bellCurve(const float x, const float curveWidth); -float fast_fsqrtf(const double value); +float fast_fsqrtf(const float value); float calc_length_pythagorean_2D(const float firstElement, const float secondElement); float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement); + +/* + * The most significat byte is placed at the lowest address + * in other words, the most significant byte is "first", on even indexes + */ +#define int16_val_big_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx] << 8) | v[2 * idx + 1])) +/* + * The most significat byte is placed at the highest address + * in other words, the most significant byte is "last", on odd indexes + */ +#define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx])) + +#ifdef SITL_BUILD +void arm_sub_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize); +void arm_scale_f32(float * pSrc, float scale, float * pDst, uint32_t blockSize); +void arm_mult_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize); +#endif \ No newline at end of file diff --git a/src/main/common/memory.c b/src/main/common/memory.c index 97a87e7a83..c80c541ca5 100644 --- a/src/main/common/memory.c +++ b/src/main/common/memory.c @@ -52,7 +52,7 @@ void * memAllocate(size_t wantedSize, resourceOwner_e owner) retPointer = &dynHeap[dynHeapFreeWord]; dynHeapFreeWord += wantedWords; dynHeapUsage[owner] += wantedWords * sizeof(uint32_t); - LOG_DEBUG(SYSTEM, "Memory allocated. Free memory = %d", memGetAvailableBytes()); + LOG_DEBUG(SYSTEM, "Memory allocated. Free memory = %ld", (unsigned long)memGetAvailableBytes()); } else { // OOM diff --git a/src/main/common/quaternion.h b/src/main/common/quaternion.h index 6fbbfc7527..39e969e163 100644 --- a/src/main/common/quaternion.h +++ b/src/main/common/quaternion.h @@ -58,7 +58,7 @@ static inline void quaternionToAxisAngle(fpAxisAngle_t * result, const fpQuatern a.angle -= 2.0f * M_PIf; } - const float sinVal = sqrt(1.0f - q->q0 * q->q0); + const float sinVal = sqrtf(1.0f - q->q0 * q->q0); // Axis is only valid when rotation is large enough sin(0.0057 deg) = 0.0001 if (sinVal > 1e-4f) { diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 468e42f6b0..d10dd77879 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -90,10 +90,9 @@ static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; } // using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions // than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions) -#ifdef UNIT_TEST -// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32) +#if defined(UNIT_TEST) || defined(SITL_BUILD) #include -static inline void memcpy_fn(void *destination, const void *source, size_t num) { memcpy(destination, source, num); }; +#define memcpy_fn(destination, source, num) memcpy(destination, source, num) #else void *memcpy_fn(void *destination, const void *source, size_t num) asm("memcpy"); #endif diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index ec9846144d..c5c18cedb1 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -44,8 +44,8 @@ typedef struct gyroDev_s { sensorGyroInterruptStatusFuncPtr intStatusFn; sensorGyroUpdateFuncPtr updateFn; float scale; // scalefactor - int16_t gyroADCRaw[XYZ_AXIS_COUNT]; - int16_t gyroZero[XYZ_AXIS_COUNT]; + float gyroADCRaw[XYZ_AXIS_COUNT]; + float gyroZero[XYZ_AXIS_COUNT]; uint8_t imuSensorToUse; uint8_t lpf; // Configuration value: Hardware LPF setting uint32_t requestedSampleIntervalUs; // Requested sample interval @@ -59,7 +59,7 @@ typedef struct accDev_s { sensorAccInitFuncPtr initFn; // initialize function sensorAccReadFuncPtr readFn; // read 3 axis data function uint16_t acc_1G; - int16_t ADCRaw[XYZ_AXIS_COUNT]; + float ADCRaw[XYZ_AXIS_COUNT]; uint8_t imuSensorToUse; sensor_align_e accAlign; } accDev_t; diff --git a/src/main/drivers/accgyro/accgyro_bmi088.c b/src/main/drivers/accgyro/accgyro_bmi088.c index 6376289dea..8988685c83 100644 --- a/src/main/drivers/accgyro/accgyro_bmi088.c +++ b/src/main/drivers/accgyro/accgyro_bmi088.c @@ -136,9 +136,9 @@ static bool bmi088GyroRead(gyroDev_t *gyro) uint8_t gyroRaw[6]; if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) { - gyro->gyroADCRaw[X] = (int16_t)((gyroRaw[1] << 8) | gyroRaw[0]); - gyro->gyroADCRaw[Y] = (int16_t)((gyroRaw[3] << 8) | gyroRaw[2]); - gyro->gyroADCRaw[Z] = (int16_t)((gyroRaw[5] << 8) | gyroRaw[4]); + gyro->gyroADCRaw[X] = (float) int16_val_little_endian(gyroRaw, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(gyroRaw, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(gyroRaw, 2); return true; } @@ -151,9 +151,9 @@ static bool bmi088AccRead(accDev_t *acc) if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) { // first byte is discarded, see datasheet - acc->ADCRaw[X] = (int32_t)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4); - acc->ADCRaw[Y] = (int32_t)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4); - acc->ADCRaw[Z] = (int32_t)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4); + acc->ADCRaw[X] = (float)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4); + acc->ADCRaw[Y] = (float)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4); + acc->ADCRaw[Z] = (float)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4); return true; } diff --git a/src/main/drivers/accgyro/accgyro_bmi160.c b/src/main/drivers/accgyro/accgyro_bmi160.c index 34b708f08e..62fe2f3538 100644 --- a/src/main/drivers/accgyro/accgyro_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_bmi160.c @@ -180,9 +180,9 @@ bool bmi160GyroReadScratchpad(gyroDev_t *gyro) ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI160_REG_GYR_DATA_X_LSB, ctx->gyroRaw, 6 + 6); if (ctx->lastReadStatus) { - gyro->gyroADCRaw[X] = (int16_t)((ctx->gyroRaw[1] << 8) | ctx->gyroRaw[0]); - gyro->gyroADCRaw[Y] = (int16_t)((ctx->gyroRaw[3] << 8) | ctx->gyroRaw[2]); - gyro->gyroADCRaw[Z] = (int16_t)((ctx->gyroRaw[5] << 8) | ctx->gyroRaw[4]); + gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2); return true; } @@ -195,9 +195,9 @@ bool bmi160AccReadScratchpad(accDev_t *acc) bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); if (ctx->lastReadStatus) { - acc->ADCRaw[X] = (int16_t)((ctx->accRaw[1] << 8) | ctx->accRaw[0]); - acc->ADCRaw[Y] = (int16_t)((ctx->accRaw[3] << 8) | ctx->accRaw[2]); - acc->ADCRaw[Z] = (int16_t)((ctx->accRaw[5] << 8) | ctx->accRaw[4]); + acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0); + acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1); + acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2); return true; } diff --git a/src/main/drivers/accgyro/accgyro_bmi270.c b/src/main/drivers/accgyro/accgyro_bmi270.c index 9f8bc21aca..0b37518a3c 100644 --- a/src/main/drivers/accgyro/accgyro_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_bmi270.c @@ -254,9 +254,9 @@ static bool bmi270yroReadScratchpad(gyroDev_t *gyro) ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI270_REG_ACC_DATA_X_LSB, &ctx->__padding_dummy, 6 + 6 + 1); if (ctx->lastReadStatus) { - gyro->gyroADCRaw[X] = (int16_t)((ctx->gyroRaw[1] << 8) | ctx->gyroRaw[0]); - gyro->gyroADCRaw[Y] = (int16_t)((ctx->gyroRaw[3] << 8) | ctx->gyroRaw[2]); - gyro->gyroADCRaw[Z] = (int16_t)((ctx->gyroRaw[5] << 8) | ctx->gyroRaw[4]); + gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2); return true; } @@ -269,9 +269,9 @@ static bool bmi270AccReadScratchpad(accDev_t *acc) bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); if (ctx->lastReadStatus) { - acc->ADCRaw[X] = (int16_t)((ctx->accRaw[1] << 8) | ctx->accRaw[0]); - acc->ADCRaw[Y] = (int16_t)((ctx->accRaw[3] << 8) | ctx->accRaw[2]); - acc->ADCRaw[Z] = (int16_t)((ctx->accRaw[5] << 8) | ctx->accRaw[4]); + acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->accRaw, 0); + acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->accRaw, 1); + acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->accRaw, 2); return true; } diff --git a/src/main/drivers/accgyro/accgyro_fake.c b/src/main/drivers/accgyro/accgyro_fake.c index 645ae4e77d..3891b36692 100644 --- a/src/main/drivers/accgyro/accgyro_fake.c +++ b/src/main/drivers/accgyro/accgyro_fake.c @@ -32,7 +32,7 @@ #ifdef USE_IMU_FAKE -static int16_t fakeGyroADC[XYZ_AXIS_COUNT]; +static float fakeGyroADC[XYZ_AXIS_COUNT]; static void fakeGyroInit(gyroDev_t *gyro) { diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c index 05ebbfbbdd..5f536527c7 100644 --- a/src/main/drivers/accgyro/accgyro_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_icm42605.c @@ -98,9 +98,9 @@ static bool icm42605AccRead(accDev_t *acc) return false; } - acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]); - acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]); - acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]); + acc->ADCRaw[X] = (float) int16_val_big_endian(data, 0); + acc->ADCRaw[Y] = (float) int16_val_big_endian(data, 1); + acc->ADCRaw[Z] = (float) int16_val_big_endian(data, 2); return true; } @@ -231,9 +231,9 @@ static bool icm42605GyroRead(gyroDev_t *gyro) return false; } - gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]); - gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]); - gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]); + gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2); return true; } diff --git a/src/main/drivers/accgyro/accgyro_lsm6dxx.c b/src/main/drivers/accgyro/accgyro_lsm6dxx.c index e665558891..5e7d24f44e 100644 --- a/src/main/drivers/accgyro/accgyro_lsm6dxx.c +++ b/src/main/drivers/accgyro/accgyro_lsm6dxx.c @@ -184,9 +184,9 @@ static bool lsm6dxxAccRead(accDev_t *acc) if (!ack) { return false; } - acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[0]); - acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[2]); - acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[4]); + acc->ADCRaw[X] = (float) int16_val_little_endian(data, 0); + acc->ADCRaw[Y] = (float) int16_val_little_endian(data, 1); + acc->ADCRaw[Z] = (float) int16_val_little_endian(data, 2); return true; } @@ -197,9 +197,9 @@ static bool lsm6dxxGyroRead(gyroDev_t *gyro) if (!ack) { return false; } - gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[0]); - gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[2]); - gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[4]); + gyro->gyroADCRaw[X] = (float) int16_val_little_endian(data, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(data, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(data, 2); return true; } diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index c12a684d0c..710865436b 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -43,8 +43,6 @@ // Check busDevice scratchpad memory size STATIC_ASSERT(sizeof(mpuContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small); -#define int16_val(v, idx) ((int16_t)(((uint8_t)v[2 * idx] << 8) | v[2 * idx + 1])) - static const gyroFilterAndRateConfig_t mpuGyroConfigs[] = { { GYRO_LPF_256HZ, 8000, { MPU_DLPF_256HZ, 0 } }, { GYRO_LPF_256HZ, 4000, { MPU_DLPF_256HZ, 1 } }, @@ -83,9 +81,9 @@ bool mpuGyroRead(gyroDev_t *gyro) return false; } - gyro->gyroADCRaw[X] = int16_val(data, 0); - gyro->gyroADCRaw[Y] = int16_val(data, 1); - gyro->gyroADCRaw[Z] = int16_val(data, 2); + gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2); return true; } @@ -102,9 +100,9 @@ bool mpuGyroReadScratchpad(gyroDev_t *gyro) mpuContextData_t * ctx = busDeviceGetScratchpadMemory(busDev); if (mpuUpdateSensorContext(busDev, ctx)) { - gyro->gyroADCRaw[X] = int16_val(ctx->gyroRaw, 0); - gyro->gyroADCRaw[Y] = int16_val(ctx->gyroRaw, 1); - gyro->gyroADCRaw[Z] = int16_val(ctx->gyroRaw, 2); + gyro->gyroADCRaw[X] = (float) int16_val_big_endian(ctx->gyroRaw, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(ctx->gyroRaw, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(ctx->gyroRaw, 2); return true; } @@ -116,9 +114,9 @@ bool mpuAccReadScratchpad(accDev_t *acc) mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); if (ctx->lastReadStatus) { - acc->ADCRaw[X] = int16_val(ctx->accRaw, 0); - acc->ADCRaw[Y] = int16_val(ctx->accRaw, 1); - acc->ADCRaw[Z] = int16_val(ctx->accRaw, 2); + acc->ADCRaw[X] = (float) int16_val_big_endian(ctx->accRaw, 0); + acc->ADCRaw[Y] = (float) int16_val_big_endian(ctx->accRaw, 1); + acc->ADCRaw[Z] = (float) int16_val_big_endian(ctx->accRaw, 2); return true; } @@ -131,7 +129,7 @@ bool mpuTemperatureReadScratchpad(gyroDev_t *gyro, int16_t * data) if (ctx->lastReadStatus) { // Convert to degC*10: degC = raw / 340 + 36.53 - *data = int16_val(ctx->tempRaw, 0) / 34 + 365; + *data = int16_val_big_endian(ctx->tempRaw, 0) / 34 + 365; return true; } diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index ca53c36715..3b1459ab1f 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -85,7 +85,7 @@ static i2cDevice_t i2cHardwareMap[I2CDEV_COUNT] = { { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1L(I2C2), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 }, { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1L(I2C3), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 }, #if defined(USE_I2C_DEVICE_4) - { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1L(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 } + { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB4(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 } #endif #endif }; diff --git a/src/main/drivers/bus_quadspi.c b/src/main/drivers/bus_quadspi.c new file mode 100644 index 0000000000..867e664f01 --- /dev/null +++ b/src/main/drivers/bus_quadspi.c @@ -0,0 +1,310 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_QUADSPI + +#include "drivers/bus_quadspi.h" +#include "drivers/bus_quadspi_impl.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/rcc.h" + +static const quadSpiConfig_t quadSpiConfig[] = { +#ifdef USE_QUADSPI_DEVICE_1 + { .device = QUADSPIDEV_1, .clk = IO_TAG(QUADSPI1_SCK_PIN), + .bk1CS = IO_TAG(QUADSPI1_BK1_CS_PIN), + .bk1IO0 = IO_TAG(QUADSPI1_BK1_IO0_PIN), .bk1IO1 = IO_TAG(QUADSPI1_BK1_IO1_PIN), .bk1IO2 = IO_TAG(QUADSPI1_BK1_IO2_PIN), .bk1IO3 = IO_TAG(QUADSPI1_BK1_IO3_PIN), + .bk2CS = IO_TAG(QUADSPI1_BK2_CS_PIN), + .bk2IO0 = IO_TAG(QUADSPI1_BK2_IO0_PIN), .bk2IO1 = IO_TAG(QUADSPI1_BK2_IO1_PIN), .bk2IO2 = IO_TAG(QUADSPI1_BK2_IO2_PIN), .bk2IO3 = IO_TAG(QUADSPI1_BK2_IO3_PIN), + .mode = QUADSPI1_MODE, .csFlags = QUADSPI1_CS_FLAGS + } +#endif +}; + +quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT] = { 0 }; + +QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance) +{ +#ifdef USE_QUADSPI_DEVICE_1 + if (instance == QUADSPI) { + return QUADSPIDEV_1; + } +#endif + + return QUADSPIINVALID; +} + +QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device) +{ + if (device == QUADSPIINVALID || device >= QUADSPIDEV_COUNT) { + return NULL; + } + + return quadSpiDevice[device].dev; +} + +bool quadSpiInit(QUADSPIDevice device) +{ + switch (device) { + case QUADSPIINVALID: + return false; + case QUADSPIDEV_1: +#ifdef USE_QUADSPI_DEVICE_1 + quadSpiInitDevice(device); + return true; +#else + break; +#endif + } + return false; +} + +uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (device == QUADSPIINVALID) { + return -1; + } + quadSpiDevice[device].errorCount++; + return quadSpiDevice[device].errorCount; +} + +uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (device == QUADSPIINVALID) { + return 0; + } + return quadSpiDevice[device].errorCount; +} + +void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (device != QUADSPIINVALID) { + quadSpiDevice[device].errorCount = 0; + } +} + +const quadSpiHardware_t quadSpiHardware[] = { +#ifdef STM32H7 + { + .device = QUADSPIDEV_1, + .reg = QUADSPI, + .clkPins = { + { DEFIO_TAG_E(PB2), GPIO_AF9_QUADSPI }, + }, + .bk1IO0Pins = { + { DEFIO_TAG_E(PC9), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PD11), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF8), GPIO_AF10_QUADSPI }, + }, + .bk1IO1Pins = { + { DEFIO_TAG_E(PC10), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PD12), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF9), GPIO_AF10_QUADSPI }, + }, + .bk1IO2Pins = { + { DEFIO_TAG_E(PE2), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF7), GPIO_AF9_QUADSPI }, + }, + .bk1IO3Pins = { + { DEFIO_TAG_E(PA1), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PD13), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF6), GPIO_AF9_QUADSPI }, + }, + .bk1CSPins = { + { DEFIO_TAG_E(PB6), GPIO_AF10_QUADSPI }, + { DEFIO_TAG_E(PB10), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PG6), GPIO_AF10_QUADSPI }, + }, + .bk2IO0Pins = { + { DEFIO_TAG_E(PE7), GPIO_AF10_QUADSPI }, + //{ DEFIO_TAG_E(PH7), GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H' + }, + .bk2IO1Pins = { + { DEFIO_TAG_E(PE8), GPIO_AF10_QUADSPI }, + //{ DEFIO_TAG_E(PH3), GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H' + }, + .bk2IO2Pins = { + { DEFIO_TAG_E(PE9), GPIO_AF10_QUADSPI }, + { DEFIO_TAG_E(PG9), GPIO_AF9_QUADSPI }, + }, + .bk2IO3Pins = { + { DEFIO_TAG_E(PE10), GPIO_AF10_QUADSPI }, + { DEFIO_TAG_E(PG14), GPIO_AF9_QUADSPI }, + }, + .bk2CSPins = { + { DEFIO_TAG_E(PC11), GPIO_AF9_QUADSPI }, + }, + .rcc = RCC_AHB3(QSPI), + }, +#endif +}; + +const quadSpiConfig_t * getQuadSpiConfig(QUADSPIDevice device) +{ + const quadSpiConfig_t * pConfig = NULL; + + for (size_t configIndex = 0; configIndex < ARRAYLEN(quadSpiConfig); configIndex++) { + if (quadSpiConfig[configIndex].device == device) { + pConfig = &quadSpiConfig[configIndex]; + break; + } + } + + return pConfig; +} + + +void quadSpiPinConfigure(QUADSPIDevice device) +{ + if (device == QUADSPIINVALID) { + return; + } + + const quadSpiConfig_t * pConfig = getQuadSpiConfig(device); + + if (pConfig == NULL) { + return; + } + + for (size_t hwindex = 0; hwindex < ARRAYLEN(quadSpiHardware); hwindex++) { + const quadSpiHardware_t *hw = &quadSpiHardware[hwindex]; + + if (hw->device != device) { + continue; + } + + quadSpiDevice_t *pDev = &quadSpiDevice[device]; + + for (int pindex = 0; pindex < MAX_QUADSPI_PIN_SEL; pindex++) { + if (pConfig->clk == hw->clkPins[pindex].pin) { + pDev->clk = hw->clkPins[pindex].pin; + } + // + // BK1 + // + if (pConfig->bk1IO0 == hw->bk1IO0Pins[pindex].pin) { + pDev->bk1IO0 = hw->bk1IO0Pins[pindex].pin; + pDev->bk1IO0AF = hw->bk1IO0Pins[pindex].af; + } + if (pConfig->bk1IO1 == hw->bk1IO1Pins[pindex].pin) { + pDev->bk1IO1 = hw->bk1IO1Pins[pindex].pin; + pDev->bk1IO1AF = hw->bk1IO1Pins[pindex].af; + } + if (pConfig->bk1IO2 == hw->bk1IO2Pins[pindex].pin) { + pDev->bk1IO2 = hw->bk1IO2Pins[pindex].pin; + pDev->bk1IO2AF = hw->bk1IO2Pins[pindex].af; + } + if (pConfig->bk1IO3 == hw->bk1IO3Pins[pindex].pin) { + pDev->bk1IO3 = hw->bk1IO3Pins[pindex].pin; + pDev->bk1IO3AF = hw->bk1IO3Pins[pindex].af; + } + if (pConfig->bk1CS == hw->bk1CSPins[pindex].pin) { + pDev->bk1CS = hw->bk1CSPins[pindex].pin; + pDev->bk1CSAF = hw->bk1CSPins[pindex].af; + } + // + // BK2 + // + if (pConfig->bk2IO0 == hw->bk2IO0Pins[pindex].pin) { + pDev->bk2IO0 = hw->bk2IO0Pins[pindex].pin; + pDev->bk2IO0AF = hw->bk2IO0Pins[pindex].af; + } + if (pConfig->bk2IO1 == hw->bk2IO1Pins[pindex].pin) { + pDev->bk2IO1 = hw->bk2IO1Pins[pindex].pin; + pDev->bk2IO1AF = hw->bk2IO1Pins[pindex].af; + } + if (pConfig->bk2IO1 == hw->bk2IO2Pins[pindex].pin) { + pDev->bk2IO2 = hw->bk2IO2Pins[pindex].pin; + pDev->bk2IO2AF = hw->bk2IO2Pins[pindex].af; + } + if (pConfig->bk2IO2 == hw->bk2IO3Pins[pindex].pin) { + pDev->bk2IO3 = hw->bk2IO3Pins[pindex].pin; + pDev->bk2IO3AF = hw->bk2IO3Pins[pindex].af; + } + if (pConfig->bk2IO3 == hw->bk2CSPins[pindex].pin) { + pDev->bk2CS = hw->bk2CSPins[pindex].pin; + pDev->bk2CSAF = hw->bk2CSPins[pindex].af; + } + } + + if ((pConfig->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + pDev->bk1CS = pConfig->bk1CS; + } + if ((pConfig->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + pDev->bk2CS = pConfig->bk2CS; + } + + bool haveResources = true; + + // clock pins + + haveResources = haveResources && pDev->clk; + + // data pins + + bool needBK1 = (pConfig->mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig->mode == QUADSPI_MODE_BK1_ONLY); + if (needBK1) { + bool haveBK1Resources = pDev->bk1IO0 && pDev->bk1IO1 && pDev->bk1IO2 && pDev->bk1IO3 && pDev->bk1CS; + haveResources = haveResources && haveBK1Resources; + } + + bool needBK2 = (pConfig->mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig->mode == QUADSPI_MODE_BK2_ONLY); + if (needBK2) { + bool haveBK2Resources = pDev->bk2IO0 && pDev->bk2IO1 && pDev->bk2IO2 && pDev->bk2IO3; + haveResources = haveResources && haveBK2Resources; + } + + // cs pins + + if (needBK1) { + haveResources = haveResources && pDev->bk1CS; + } + + bool needBK2CS = + (pConfig->mode == QUADSPI_MODE_DUAL_FLASH && (pConfig->csFlags & QUADSPI_CS_MODE_MASK) == QUADSPI_CS_MODE_SEPARATE) || + (pConfig->mode == QUADSPI_MODE_BK2_ONLY); + + if (needBK2CS) { + haveResources = haveResources && pDev->bk2CS; + } + + if (haveResources) { + pDev->dev = hw->reg; + pDev->rcc = hw->rcc; + } + + // copy mode and flags + pDev->mode = pConfig->mode; + pDev->csFlags = pConfig->csFlags; + } +} + +#endif diff --git a/src/main/drivers/bus_quadspi.h b/src/main/drivers/bus_quadspi.h new file mode 100644 index 0000000000..e267aa24bb --- /dev/null +++ b/src/main/drivers/bus_quadspi.h @@ -0,0 +1,158 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +#pragma once + +#include "drivers/io_types.h" +#include "drivers/rcc_types.h" + +/* + * Quad SPI supports 1/2/4 wire modes + * + * 1LINE is like SPI MISO/MOSI using D0 (MOSI)/D1(MISO). + * 2LINE uses D0, D1 (bidirectional). + * 4LINE uses D0..D3 (bidirectional) + * + * See ST Micros' AN4760 "Quad-SPI (QSPI) interface on STM32 microcontrollers" + */ + +#ifdef USE_QUADSPI + +#if !defined(STM32H7) +#error Quad SPI unsupported on this MCU +#endif + +#define QUADSPI_IO_AF_BK_IO_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) +#define QUADSPI_IO_AF_CLK_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) +#define QUADSPI_IO_AF_BK_CS_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP) +#define QUADSPI_IO_BK_CS_CFG IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_HIGH, GPIO_PULLUP) + +typedef enum { + /* QSPI freq = CLK /(1 + ClockPrescaler) = 200 MHz/(1+x) */ + QUADSPI_CLOCK_INITIALISATION = 255, // 0.78125 Mhz + QUADSPI_CLOCK_SLOW = 19, // 10.00000 Mhz + QUADSPI_CLOCK_STANDARD = 9, // 20.00000 MHz + QUADSPI_CLOCK_FAST = 3, // 50.00000 MHz + QUADSPI_CLOCK_ULTRAFAST = 1 //100.00000 MHz +} QUADSPIClockDivider_e; + +typedef enum QUADSPIDevice { + QUADSPIINVALID = -1, + QUADSPIDEV_1 = 0, +} QUADSPIDevice; + +#define QUADSPIDEV_COUNT 1 + +// Macros to convert between CLI bus number and SPIDevice. +#define QUADSPI_CFG_TO_DEV(x) ((x) - 1) +#define QUADSPI_DEV_TO_CFG(x) ((x) + 1) + +typedef enum { + QUADSPI_MODE_BK1_ONLY = 0, + QUADSPI_MODE_BK2_ONLY, + QUADSPI_MODE_DUAL_FLASH, +} quadSpiMode_e; + +// +// QUADSPI1_CS_FLAGS +// +#define QUADSPI_BK1_CS_MASK ((1 << 1) | (1 << 0)) + +#define QUADSPI_BK1_CS_NONE ((0 << 1) | (0 << 0)) +#define QUADSPI_BK1_CS_HARDWARE ((0 << 1) | (1 << 0)) // pin must support QSPI Alternate function for BK1_NCS +#define QUADSPI_BK1_CS_SOFTWARE ((1 << 1) | (0 << 0)) // use any GPIO pin for BK1 CS + +#define QUADSPI_BK2_CS_MASK ((1 << 3) | (1 << 2)) + +#define QUADSPI_BK2_CS_NONE ((0 << 3) | (0 << 2)) +#define QUADSPI_BK2_CS_HARDWARE ((0 << 3) | (1 << 2)) // pin must support QSPI Alternate function for BK2_NCS +#define QUADSPI_BK2_CS_SOFTWARE ((1 << 3) | (0 << 2)) // use any GPIO pin for BK2 CS + +#define QUADSPI_CS_MODE_MASK (1 << 4) + +#define QUADSPI_CS_MODE_SEPARATE (0 << 4) +#define QUADSPI_CS_MODE_LINKED (1 << 4) + +// H7 QSPI notes: +// Hardware supports BK1 and BK2 connected to both flash chips when using DUAL FLASH mode. +// Hardware does NOT support using BK1_NCS for single flash chip on BK2. +// It's possible to use BK1_NCS for single chip on BK2 using software CS via QUADSPI_BK2_CS_SOFTWARE + + +void quadSpiPreInit(void); + +bool quadSpiInit(QUADSPIDevice device); +void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor); + +bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length); +bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length); +bool quadSpiReceive4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length); + +bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length); + +bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length); +bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length); +bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length); +bool quadSpiTransmitWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length); + + +bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize); + +//bool quadSpiIsBusBusy(SPI_TypeDef *instance); + +uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance); +void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance); + +QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance); +QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device); + +// +// Config +// + +typedef struct quadSpiConfig_s { + QUADSPIDevice device; + ioTag_t clk; + + // Note: Either or both CS pin may be used in DUAL_FLASH mode, any unused pins should be IO_NONE + ioTag_t bk1IO0; + ioTag_t bk1IO1; + ioTag_t bk1IO2; + ioTag_t bk1IO3; + ioTag_t bk1CS; + + ioTag_t bk2IO0; + ioTag_t bk2IO1; + ioTag_t bk2IO2; + ioTag_t bk2IO3; + ioTag_t bk2CS; + + uint8_t mode; + + // CS pins can be under software control, useful when using BK1CS as the CS pin for BK2 in non-DUAL-FLASH mode. + uint8_t csFlags; +} quadSpiConfig_t; + +const quadSpiConfig_t * getQuadSpiConfig(QUADSPIDevice device); +void quadSpiPinConfigure(QUADSPIDevice device); + +#endif diff --git a/src/main/drivers/bus_quadspi_hal.c b/src/main/drivers/bus_quadspi_hal.c new file mode 100644 index 0000000000..0d5215ab2f --- /dev/null +++ b/src/main/drivers/bus_quadspi_hal.c @@ -0,0 +1,575 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_QUADSPI + +#include "bus_quadspi.h" +#include "bus_quadspi_impl.h" +#include "dma.h" +#include "io.h" +#include "io_impl.h" +#include "nvic.h" +#include "rcc.h" + +static void Error_Handler(void) { while (1) { } } + +void quadSpiInitDevice(QUADSPIDevice device) +{ + quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]); + + // Enable QUADSPI clock + RCC_ClockCmd(quadSpi->rcc, ENABLE); + //RCC_ResetCmd(quadSpi->rcc, ENABLE); + + IOInit(IOGetByTag(quadSpi->clk), OWNER_QUADSPI, RESOURCE_QUADSPI_CLK, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO0), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO0, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO1), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO1, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO2), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO2, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO3), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO3, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1CS), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1CS, RESOURCE_INDEX(device)); + + IOInit(IOGetByTag(quadSpi->bk2IO0), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO0, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2IO1), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO1, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2IO2), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO2, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2IO3), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO3, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2CS), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2CS, RESOURCE_INDEX(device)); + +#if defined(STM32H7) + // clock is only on AF9 + // IO and CS lines are on AF9 and AF10 + IOConfigGPIOAF(IOGetByTag(quadSpi->clk), QUADSPI_IO_AF_CLK_CFG, GPIO_AF9_QUADSPI); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO0AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO1AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO2AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO3AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO0AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO1AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO2AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO3AF); + + if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_HARDWARE) { + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk1CSAF); + } else { + IOConfigGPIO(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_BK_CS_CFG); + } + + if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_HARDWARE) { + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk2CSAF); + } else { + IOConfigGPIO(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_BK_CS_CFG); + } +#endif + + quadSpi->hquadSpi.Instance = quadSpi->dev; + // DeInit QUADSPI hardware + HAL_QSPI_DeInit(&quadSpi->hquadSpi); + + quadSpi->hquadSpi.Init.ClockPrescaler = QUADSPI_CLOCK_INITIALISATION; + quadSpi->hquadSpi.Init.FifoThreshold = 1; + quadSpi->hquadSpi.Init.SampleShifting = QSPI_SAMPLE_SHIFTING_NONE; + quadSpi->hquadSpi.Init.FlashSize = 23; // address bits + 1 + quadSpi->hquadSpi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_1_CYCLE; + quadSpi->hquadSpi.Init.ClockMode = QSPI_CLOCK_MODE_0; + + switch (quadSpi->mode) { + case QUADSPI_MODE_BK1_ONLY: + quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_1; + quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE; + break; + case QUADSPI_MODE_BK2_ONLY: + quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_2; + quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE; + break; + case QUADSPI_MODE_DUAL_FLASH: + quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_ENABLE; + break; + } + + // Init QUADSPI hardware + if (HAL_QSPI_Init(&quadSpi->hquadSpi) != HAL_OK) + { + Error_Handler(); + } +} + +static const uint32_t quadSpi_addressSizeMap[] = { + QSPI_ADDRESS_8_BITS, + QSPI_ADDRESS_16_BITS, + QSPI_ADDRESS_24_BITS, + QSPI_ADDRESS_32_BITS +}; + +static uint32_t quadSpi_addressSizeFromValue(uint8_t addressSize) +{ + return quadSpi_addressSizeMap[((addressSize + 1) / 8) - 1]; // rounds to nearest QSPI_ADDRESS_* value that will hold the address. +} + +/** + * Return true if the bus is currently in the middle of a transmission. + */ +bool quadSpiIsBusBusy(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if(quadSpiDevice[device].hquadSpi.State == HAL_QSPI_STATE_BUSY) + return true; + else + return false; +} + +#define QUADSPI_DEFAULT_TIMEOUT 10 + +void quadSpiSelectDevice(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + + quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]); + + IO_t bk1CS = IOGetByTag(quadSpi->bk1CS); + IO_t bk2CS = IOGetByTag(quadSpi->bk2CS); + + switch(quadSpi->mode) { + case QUADSPI_MODE_DUAL_FLASH: + if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOLo(bk1CS); + } + if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOLo(bk2CS); + } + break; + case QUADSPI_MODE_BK1_ONLY: + if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOLo(bk1CS); + } + break; + case QUADSPI_MODE_BK2_ONLY: + if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOLo(bk2CS); + } + break; + } +} + +void quadSpiDeselectDevice(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + + quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]); + + IO_t bk1CS = IOGetByTag(quadSpi->bk1CS); + IO_t bk2CS = IOGetByTag(quadSpi->bk2CS); + + switch(quadSpi->mode) { + case QUADSPI_MODE_DUAL_FLASH: + if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOHi(bk1CS); + } + if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOHi(bk2CS); + } + break; + case QUADSPI_MODE_BK1_ONLY: + if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOHi(bk1CS); + } + break; + case QUADSPI_MODE_BK2_ONLY: + if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOHi(bk2CS); + } + break; + } +} + + + +bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_NONE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_NONE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.NbData = length; + + if (out) { + cmd.DataMode = QSPI_DATA_1_LINE; + } + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + if (out && length > 0) { + status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_NONE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT); + + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiReceive4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_4_LINES; + cmd.AddressMode = QSPI_ADDRESS_NONE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_4_LINES; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT); + + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + + +bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_4_LINES; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} +bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + + if (!timeout) { + status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiTransmitWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_4_LINES; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + + if (!timeout) { + status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_NONE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = 0; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + + return true; +} + +bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_NONE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout =(status != HAL_OK); + + if (!timeout) { + status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (HAL_QSPI_DeInit(&quadSpiDevice[device].hquadSpi) != HAL_OK) + { + Error_Handler(); + } + + quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]); + + quadSpi->hquadSpi.Init.ClockPrescaler = divisor; + + HAL_QSPI_Init(&quadSpi->hquadSpi); +} +#endif diff --git a/src/main/drivers/bus_quadspi_impl.h b/src/main/drivers/bus_quadspi_impl.h new file mode 100644 index 0000000000..5ea444ba49 --- /dev/null +++ b/src/main/drivers/bus_quadspi_impl.h @@ -0,0 +1,94 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + * + * Author: Dominic Clifton + */ + +#pragma once + + +typedef struct quadSpiPinDef_s { + ioTag_t pin; +#if defined(STM32H7) + uint8_t af; +#endif +} quadSpiPinDef_t; + +#if defined(STM32H7) +#define MAX_QUADSPI_PIN_SEL 3 +#endif + +typedef struct quadSpiHardware_s { + QUADSPIDevice device; + QUADSPI_TypeDef *reg; + quadSpiPinDef_t clkPins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO0Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO1Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO2Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO3Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1CSPins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO0Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO1Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO2Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO3Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2CSPins[MAX_QUADSPI_PIN_SEL]; + + rccPeriphTag_t rcc; +} quadSpiHardware_t; + +extern const quadSpiHardware_t quadSpiHardware[]; + +typedef struct QUADSPIDevice_s { + QUADSPI_TypeDef *dev; + ioTag_t clk; + ioTag_t bk1IO0; + ioTag_t bk1IO1; + ioTag_t bk1IO2; + ioTag_t bk1IO3; + ioTag_t bk1CS; + ioTag_t bk2IO0; + ioTag_t bk2IO1; + ioTag_t bk2IO2; + ioTag_t bk2IO3; + ioTag_t bk2CS; +#if defined(STM32H7) + uint8_t bk1IO0AF; + uint8_t bk1IO1AF; + uint8_t bk1IO2AF; + uint8_t bk1IO3AF; + uint8_t bk1CSAF; + uint8_t bk2IO0AF; + uint8_t bk2IO1AF; + uint8_t bk2IO2AF; + uint8_t bk2IO3AF; + uint8_t bk2CSAF; +#endif + uint8_t mode; + uint8_t csFlags; + rccPeriphTag_t rcc; + volatile uint16_t errorCount; +#if defined(USE_HAL_DRIVER) + QSPI_HandleTypeDef hquadSpi; +#endif +} quadSpiDevice_t; + +extern quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT]; + +void quadSpiInitDevice(QUADSPIDevice device); +uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance); diff --git a/src/main/drivers/compass/compass_msp.c b/src/main/drivers/compass/compass_msp.c index f9c1cb794b..d0b2ffc627 100644 --- a/src/main/drivers/compass/compass_msp.c +++ b/src/main/drivers/compass/compass_msp.c @@ -45,7 +45,7 @@ #define MSP_MAG_TIMEOUT_MS 250 // Less than 4Hz updates is considered a failure -static int32_t mspMagData[XYZ_AXIS_COUNT]; +static float mspMagData[XYZ_AXIS_COUNT]; static timeMs_t mspMagLastUpdateMs; static bool mspMagInit(magDev_t *magDev) @@ -62,9 +62,9 @@ void mspMagReceiveNewData(uint8_t * bufferPtr) { const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr; - mspMagData[X] = pkt->magX; - mspMagData[Y] = pkt->magY; - mspMagData[Z] = pkt->magZ; + mspMagData[X] = (float) pkt->magX; + mspMagData[Y] = (float) pkt->magY; + mspMagData[Z] = (float) pkt->magZ; applySensorAlignment(mspMagData, mspMagData, CW90_DEG_FLIP); diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index 30002f17fc..145aeafd56 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -218,7 +218,7 @@ static void flashConfigurePartitions(void) #endif } -flashPartition_t *flashPartitionFindByType(uint8_t type) +flashPartition_t *flashPartitionFindByType(flashPartitionType_e type) { for (int index = 0; index < FLASH_MAX_PARTITIONS; index++) { flashPartition_t *candidate = &flashPartitionTable.partitions[index]; diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 60a3ad46bd..9842a80995 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -27,13 +27,20 @@ #include "drivers/bus.h" #include "drivers/time.h" +#if defined(USE_QUADSPI) +#include "drivers/bus_quadspi.h" +#include "drivers/bus_quadspi_impl.h" +#endif + #define M25P16_INSTRUCTION_RDID 0x9F #define M25P16_INSTRUCTION_READ_BYTES 0x03 +#define M25P16_INSTRUCTION_QUAD_READ 0x6B #define M25P16_INSTRUCTION_READ_STATUS_REG 0x05 #define M25P16_INSTRUCTION_WRITE_STATUS_REG 0x01 #define M25P16_INSTRUCTION_WRITE_ENABLE 0x06 #define M25P16_INSTRUCTION_WRITE_DISABLE 0x04 #define M25P16_INSTRUCTION_PAGE_PROGRAM 0x02 +#define M25P16_INSTRUCTION_QPAGE_PROGRAM 0x32 #define M25P16_INSTRUCTION_SECTOR_ERASE 0xD8 #define M25P16_INSTRUCTION_BULK_ERASE 0xC7 @@ -42,6 +49,8 @@ #define W25Q256_INSTRUCTION_ENTER_4BYTE_ADDRESS_MODE 0xB7 +#define M25P16_FAST_READ_DUMMY_CYCLES 8 + struct { uint32_t jedecID; flashSector_t sectors; @@ -118,7 +127,12 @@ struct { static flashGeometry_t geometry = {.pageSize = M25P16_PAGESIZE}; +#if !defined(M25P16_QUADSPI_DEVICE) static busDevice_t * busDev = NULL; +#else +static QUADSPI_TypeDef * qspi = NULL; +#endif /* !defined(M25P16_QUADSPI_DEVICE) */ + static bool isLargeFlash = false; static uint32_t timeoutAt = 0; @@ -134,7 +148,11 @@ static bool couldBeBusy = false; */ static void m25p16_performOneByteCommand(uint8_t command) { +#if !defined(M25P16_QUADSPI_DEVICE) busTransfer(busDev, NULL, &command, 1); +#else + quadSpiTransmit1LINE(qspi, command, 0, NULL, 0); +#endif /* !defined(M25P16_QUADSPI_DEVICE) */ } /** @@ -151,12 +169,18 @@ static void m25p16_writeEnable(void) static uint8_t m25p16_readStatus(void) { + uint8_t status; +#if !defined(M25P16_QUADSPI_DEVICE) uint8_t command[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; uint8_t in[2]; busTransfer(busDev, in, command, sizeof(command)); + status = in[1]; +#else + quadSpiReceive1LINE(qspi, M25P16_INSTRUCTION_READ_STATUS_REG, 0, &status, 1); +#endif /* !defined(M25P16_QUADSPI_DEVICE) */ - return in[1]; + return status; } bool m25p16_isReady(void) @@ -201,7 +225,6 @@ bool m25p16_waitForReady(uint32_t timeoutMillis) */ static bool m25p16_readIdentification(void) { - uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 }; uint8_t in[4]; uint32_t chipID; @@ -212,7 +235,16 @@ static bool m25p16_readIdentification(void) */ in[1] = 0; +#if !defined(M25P16_QUADSPI_DEVICE) + uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 }; + busTransfer(busDev, in, out, sizeof(out)); +#else + bool status = quadSpiReceive1LINE(qspi, M25P16_INSTRUCTION_RDID, 0, &in[1], sizeof(in) - 1); + if (!status) { + return false; + } +#endif /* !defined(M25P16_QUADSPI_DEVICE) */ // Manufacturer, memory type, and capacity chipID = (in[1] << 16) | (in[2] << 8) | (in[3]); @@ -256,6 +288,9 @@ static bool m25p16_readIdentification(void) */ bool m25p16_init(int flashNumToUse) { + bool detected; +#if !defined(M25P16_QUADSPI_DEVICE) + // SPI Mode busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_M25P16, flashNumToUse, OWNER_FLASH); if (busDev == NULL) { return false; @@ -264,8 +299,25 @@ bool m25p16_init(int flashNumToUse) #ifndef M25P16_SPI_SHARED busSetSpeed(busDev, BUS_SPEED_FAST); #endif +#else + // QUADSPI Mode + UNUSED(flashNumToUse); + quadSpiPinConfigure(M25P16_QUADSPI_DEVICE); + quadSpiInitDevice(M25P16_QUADSPI_DEVICE); - return m25p16_readIdentification(); + qspi = quadSpiInstanceByDevice(M25P16_QUADSPI_DEVICE); + quadSpiSetDivisor(qspi, QUADSPI_CLOCK_INITIALISATION); +#endif /* M25P16_QUADSPI_DEVICE */ + + detected = m25p16_readIdentification(); + +#if defined(M25P16_QUADSPI_DEVICE) + if (detected) { + quadSpiSetDivisor(qspi, QUADSPI_CLOCK_ULTRAFAST); + } +#endif + + return detected; } void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddress) @@ -284,17 +336,20 @@ void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddres */ void m25p16_eraseSector(uint32_t address) { - uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE }; - - m25p16_setCommandAddress(&out[1], address, isLargeFlash); - if (!m25p16_waitForReady(0)) { return; } m25p16_writeEnable(); +#if !defined(M25P16_QUADSPI_DEVICE) + uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE }; + m25p16_setCommandAddress(&out[1], address, isLargeFlash); + busTransfer(busDev, NULL, out, isLargeFlash ? 5 : 4); +#else + quadSpiInstructionWithAddress1LINE(qspi, M25P16_INSTRUCTION_SECTOR_ERASE, 0, address, isLargeFlash ? 32 : 24); +#endif /* !defined(M25P16_QUADSPI_DEVICE) */ m25p16_setTimeout(SECTOR_ERASE_TIMEOUT_MILLIS); } @@ -329,6 +384,14 @@ void m25p16_eraseCompletely(void) */ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length) { + if (!m25p16_waitForReady(0)) { + // return same address to indicate timeout + return address; + } + + m25p16_writeEnable(); + +#if !defined(M25P16_QUADSPI_DEVICE) uint8_t command[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM }; busTransferDescriptor_t txn[2] = { @@ -338,14 +401,10 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length) m25p16_setCommandAddress(&command[1], address, isLargeFlash); - if (!m25p16_waitForReady(0)) { - // return same address to indicate timeout - return address; - } - - m25p16_writeEnable(); - busTransferMultiple(busDev, txn, 2); +#else + quadSpiTransmitWithAddress4LINES(qspi, M25P16_INSTRUCTION_QPAGE_PROGRAM, 0, address, isLargeFlash ? 32 : 24, data, length); +#endif /* !defined(M25P16_QUADSPI_DEVICE) */ m25p16_setTimeout(DEFAULT_TIMEOUT_MILLIS); @@ -362,6 +421,11 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length) */ int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length) { + if (!m25p16_waitForReady(0)) { + return 0; + } + +#if !defined(M25P16_QUADSPI_DEVICE) uint8_t command[5] = { M25P16_INSTRUCTION_READ_BYTES }; busTransferDescriptor_t txn[2] = { @@ -371,11 +435,11 @@ int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length) m25p16_setCommandAddress(&command[1], address, isLargeFlash); - if (!m25p16_waitForReady(0)) { - return 0; - } - busTransferMultiple(busDev, txn, 2); +#else + quadSpiReceiveWithAddress4LINES(qspi, M25P16_INSTRUCTION_QUAD_READ, M25P16_FAST_READ_DUMMY_CYCLES, + address, isLargeFlash ? 32 : 24, buffer, length); +#endif /* !defined(M25P16_QUADSPI_DEVICE) */ return length; } diff --git a/src/main/drivers/opflow/opflow.h b/src/main/drivers/opflow/opflow.h index ef3033b940..e9330944d2 100644 --- a/src/main/drivers/opflow/opflow.h +++ b/src/main/drivers/opflow/opflow.h @@ -31,7 +31,7 @@ struct opflowDev_s; typedef struct opflowData_s { timeDelta_t deltaTime; // Integration timeframe of motionX/Y - int32_t flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions + float flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions int16_t quality; } opflowData_t; diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l0x.c b/src/main/drivers/rangefinder/rangefinder_vl53l0x.c index d911348ca6..d2ddea3413 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l0x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l0x.c @@ -367,7 +367,7 @@ uint16_t encodeTimeout(uint16_t timeout_mclks) // Defaults to 0.25 MCPS as initialized by the ST API and this library. bool setSignalRateLimit(busDevice_t * busDev, float limit_Mcps) { - if (limit_Mcps < 0 || limit_Mcps > 511.99) { + if (limit_Mcps < 0 || limit_Mcps > 511.99f) { return false; } diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 8d3940816a..2061913523 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -19,7 +19,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM/IO", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "TIMER", - "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", + "RANGEFINDER", "SYSTEM", "SPI", "QUADSPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER", "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY", "PINIO", "IRLOCK" @@ -33,6 +33,8 @@ const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { "EXTI", "SCL", "SDA", "SCK", "MOSI", "MISO", "CS", + "CLK","BK1IO0", "BK1IO1", "BK1IO2", "BK1IO3", "BK1CS", + "BK2IO0", "BK2IO1", "BK2IO2", "BK2IO3", "BK2CS", "CH1", "CH2", "CH3", "CH4", "CE" }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 6e60599041..be092ac209 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -31,6 +31,7 @@ typedef enum { OWNER_RANGEFINDER, OWNER_SYSTEM, OWNER_SPI, + OWNER_QUADSPI, OWNER_I2C, OWNER_SDCARD, OWNER_FLASH, @@ -68,6 +69,10 @@ typedef enum { RESOURCE_EXTI, RESOURCE_I2C_SCL, RESOURCE_I2C_SDA, RESOURCE_SPI_SCK, RESOURCE_SPI_MOSI, RESOURCE_SPI_MISO, RESOURCE_SPI_CS, + RESOURCE_QUADSPI_CLK, RESOURCE_QUADSPI_BK1IO0, RESOURCE_QUADSPI_BK1IO1, + RESOURCE_QUADSPI_BK1IO2, RESOURCE_QUADSPI_BK1IO3, RESOURCE_QUADSPI_BK1CS, + RESOURCE_QUADSPI_BK2IO0, RESOURCE_QUADSPI_BK2IO1, RESOURCE_QUADSPI_BK2IO2, + RESOURCE_QUADSPI_BK2IO3, RESOURCE_QUADSPI_BK2CS, RESOURCE_ADC_CH1, RESOURCE_ADC_CH2, RESOURCE_ADC_CH3, RESOURCE_ADC_CH4, RESOURCE_RX_CE, RESOURCE_TOTAL_COUNT diff --git a/src/main/drivers/sdcard/sdcard_spi.c b/src/main/drivers/sdcard/sdcard_spi.c index df92b67d58..1b374e70e6 100644 --- a/src/main/drivers/sdcard/sdcard_spi.c +++ b/src/main/drivers/sdcard/sdcard_spi.c @@ -90,10 +90,6 @@ static void sdcardSpi_reset(void) return; } - if (sdcard.state >= SDCARD_STATE_READY) { - busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION); - } - sdcard.failureCount++; if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { sdcard.state = SDCARD_STATE_NOT_PRESENT; @@ -474,6 +470,8 @@ static bool sdcardSpi_poll(void) sdcard.state = SDCARD_STATE_NOT_PRESENT; } } + + busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD); break; case SDCARD_STATE_CARD_INIT_IN_PROGRESS: @@ -511,6 +509,8 @@ static bool sdcardSpi_poll(void) } } } + + busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD); break; case SDCARD_STATE_INITIALIZATION_RECEIVE_CID: if (sdcardSpi_receiveCID()) { @@ -532,6 +532,8 @@ static bool sdcardSpi_poll(void) sdcard.state = SDCARD_STATE_READY; goto doMore; } // else keep waiting for the CID to arrive + + busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD); break; case SDCARD_STATE_SENDING_WRITE: // Have we finished sending the write yet? @@ -851,16 +853,18 @@ void sdcardSpi_init(void) busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION); // SDCard wants 1ms minimum delay after power is applied to it - delay(1000); + delay(1); // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up - busDeselectDevice(sdcard.dev); - busTransfer(sdcard.dev, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); + IOHi(sdcard.dev->busdev.spi.csnPin); + SPI_TypeDef * instance = spiInstanceByDevice(sdcard.dev->busdev.spi.spiBus); + spiTransfer(instance, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: int time = 100000; while (busIsBusy(sdcard.dev)) { if (time-- == 0) { + busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD); busDeviceDeInit(sdcard.dev); sdcard.dev = NULL; sdcard.state = SDCARD_STATE_NOT_PRESENT; diff --git a/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c b/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c index 05a4d8464c..d8cf16ba83 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c @@ -127,14 +127,22 @@ void sdioPinConfigure(void) sdioHardware = &sdioPinHardware[SDCARD_SDIO_DEVICE]; - sdioPin[SDIO_PIN_CK] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCK[0]; - sdioPin[SDIO_PIN_CMD] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCMD[0]; - sdioPin[SDIO_PIN_D0] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD0[0]; +#ifdef SDCARD_SDIO2_CK_ALT + sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[1]; +#else + sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[0]; +#endif +#ifdef SDCARD_SDIO2_CMD_ALT + sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[1]; +#else + sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[0]; +#endif + sdioPin[SDIO_PIN_D0] = sdioHardware->sdioPinD0[0]; #ifdef SDCARD_SDIO_4BIT - sdioPin[SDIO_PIN_D1] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD1[0]; - sdioPin[SDIO_PIN_D2] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD2[0]; - sdioPin[SDIO_PIN_D3] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD3[0]; + sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0]; + sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0]; + sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0]; #endif } @@ -254,7 +262,11 @@ bool SD_Init(void) hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested #endif hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE; - hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV +#ifdef SDCARD_SDIO_NORMAL_SPEED + hsd1.Init.ClockDiv = SDMMC_NSpeed_CLK_DIV; +#else + hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV +#endif status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index f962fe3b78..1e2c78f197 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -37,6 +37,7 @@ #include #include #include +#include #include "common/utils.h" @@ -46,66 +47,6 @@ static const struct serialPortVTable tcpVTable[]; static tcpPort_t tcpPorts[SERIAL_PORT_COUNT]; -static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len ) -{ - struct addrinfo *servinfo, *p; - struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG}; - if (name == NULL) { - hints.ai_flags |= AI_PASSIVE; - } - /* - This nonsense is to uniformly deliver the same sa_family regardless of whether - name is NULL or non-NULL - Otherwise, at least on Linux, we get - - V6,V4 for the non-null case and - - V4,V6 for the null case, regardless of gai.conf - Which may confuse consumers. - FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks in all cases - unless a quad dotted address is specificied - */ - struct addrinfo *p4 = NULL; - struct addrinfo *p6 = NULL; - int result; - char aport[16]; - snprintf(aport, sizeof(aport), "%d", port); - if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) { - fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result)); - return result; - } else { - for(p = servinfo; p != NULL; p = p->ai_next) { - if(p->ai_family == AF_INET6) - p6 = p; - else if(p->ai_family == AF_INET) - p4 = p; - } - if (p6 != NULL) - p = p6; - else if (p4 != NULL) - p = p4; - else - return -1; - memcpy(addr, p->ai_addr, p->ai_addrlen); - *len = p->ai_addrlen; - freeaddrinfo(servinfo); - } - return 0; -} - -static char *tcpGetAddressString(struct sockaddr *addr) -{ - static char straddr[INET6_ADDRSTRLEN]; - void *ipaddr; - if (addr->sa_family == AF_INET6) { - struct sockaddr_in6 * ip = (struct sockaddr_in6*)addr; - ipaddr = &ip->sin6_addr; - } else { - struct sockaddr_in * ip = (struct sockaddr_in*)addr; - ipaddr = &ip->sin_addr; - } - const char *res = inet_ntop(addr->sa_family, ipaddr, straddr, sizeof straddr); - return (char *)res; -} - static void *tcpReceiveThread(void* arg) { tcpPort_t *port = (tcpPort_t*)arg; @@ -125,8 +66,8 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) } uint16_t tcpPort = BASE_IP_ADDRESS + id - 1; - if (lookup_address(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) { - return NULL; + if (lookupAddress(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) { + return NULL; } port->socketFd = socket(((struct sockaddr*)&port->sockAddress)->sa_family, SOCK_STREAM, IPPROTO_TCP); @@ -138,15 +79,16 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) #ifdef __CYGWIN__ // Sadly necesary to enforce dual-stack behaviour on Windows networking ,,, if (((struct sockaddr*)&port->sockAddress)->sa_family == AF_INET6) { - int v6only=0; - err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only)); - if (err != 0) { - fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno)); - } + int v6only=0; + err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only)); + if (err != 0) { + fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno)); + } } #endif int one = 1; + err = setsockopt(port->socketFd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); err = setsockopt(port->socketFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); err = fcntl(port->socketFd, F_SETFL, fcntl(port->socketFd, F_GETFL, 0) | O_NONBLOCK); @@ -168,16 +110,17 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) fprintf(stderr, "[SOCKET] Unable to listen.\n"); return NULL; } - - fprintf(stderr, "[SOCKET] Bind TCP %s port %d to UART%d\n", - tcpGetAddressString((struct sockaddr*)&port->sockAddress), tcpPort, id); - + char addrbuf[IPADDRESS_PRINT_BUFLEN]; + char *addrptr = prettyPrintAddress((struct sockaddr *)&port->sockAddress, addrbuf, IPADDRESS_PRINT_BUFLEN); + if (addrptr != NULL) { + fprintf(stderr, "[SOCKET] Bind TCP %s to UART%d\n", addrptr, id); + } return port; } int tcpReceive(tcpPort_t *port) { - + char addrbuf[IPADDRESS_PRINT_BUFLEN]; if (!port->isClientConnected) { fd_set fds; @@ -190,14 +133,17 @@ int tcpReceive(tcpPort_t *port) return -1; } - socklen_t addrLen = sizeof(struct sockaddr_storage); + socklen_t addrLen = sizeof(struct sockaddr_storage); port->clientSocketFd = accept(port->socketFd,(struct sockaddr*)&port->clientAddress, &addrLen); if (port->clientSocketFd < 1) { fprintf(stderr, "[SOCKET] Can't accept connection.\n"); return -1; } - fprintf(stderr, "[SOCKET] %s connected to UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id); + char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN); + if (addrptr != NULL) { + fprintf(stderr, "[SOCKET] %s connected to UART%d\n", addrptr, port->id); + } port->isClientConnected = true; } @@ -206,12 +152,14 @@ int tcpReceive(tcpPort_t *port) // recv() under cygwin does not recognise the closed connection under certain circumstances, but returns ECONNRESET as an error. if (port->isClientConnected && (recvSize == 0 || ( recvSize == -1 && errno == ECONNRESET))) { - fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id); - close(port->clientSocketFd); - memset(&port->clientAddress, 0, sizeof(port->clientAddress)); - port->isClientConnected = false; - - return 0; + char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN); + if (addrptr != NULL) { + fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", addrptr, port->id); + } + close(port->clientSocketFd); + memset(&port->clientAddress, 0, sizeof(port->clientAddress)); + port->isClientConnected = false; + return 0; } for (ssize_t i = 0; i < recvSize; i++) { diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 6f4cd52373..b175dad868 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -24,6 +24,7 @@ #include "platform.h" #include "build/debug.h" +#include "common/log.h" #include "vtx_common.h" @@ -73,8 +74,9 @@ void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t ch if (!vtxDevice) return; - if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) + if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) { return; + } if (vtxDevice->vTable->setBandAndChannel) { vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel); diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 1910075a83..ed4d5e251a 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -36,7 +36,7 @@ #define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) #define VTX_SETTINGS_POWER_COUNT 5 #define VTX_SETTINGS_DEFAULT_POWER 1 @@ -63,6 +63,7 @@ typedef enum { VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, VTXDEV_FFPV = 5, + VTXDEV_MSP = 6, VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1708c796c7..a14aac38a2 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -89,6 +89,7 @@ bool cliMode = false; #include "io/beeper.h" #include "io/flashfs.h" #include "io/gps.h" +#include "io/gps_ublox.h" #include "io/ledstrip.h" #include "io/osd.h" #include "io/serial.h" @@ -3468,6 +3469,23 @@ static void cliStatus(char *cmdline) cliPrintLinefeed(); #endif + if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) { + cliPrint("GPS: "); + cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate()); + cliPrintLinefeed(); + //cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate()); + cliPrintLinef(" GNSS Capabilities:"); + cliPrintLine(" GNSS Provider active/default"); + cliPrintLine(" GPS 1/1"); + if(gpsUbloxHasGalileo()) + cliPrintLinef(" Galileo %d/%d", gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault()); + if(gpsUbloxHasBeidou()) + cliPrintLinef(" BeiDou %d/%d", gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault()); + if(gpsUbloxHasGlonass()) + cliPrintLinef(" Glonass %d/%d", gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault()); + cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss()); + } + // If we are blocked by PWM init - provide more information if (getPwmInitError() != PWM_INIT_ERROR_NONE) { cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage()); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 9aa5fb8ca9..3b884db747 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -469,11 +469,11 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } -void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT]) +void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]) { - gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X]; - gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y]; - gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z]; + gyroConfigMutable()->gyro_zero_cal[X] = (int16_t) getGyroZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = (int16_t) getGyroZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = (int16_t) getGyroZero[Z]; } void setGravityCalibration(float getGravity) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index b7a8d40b66..e32a705e1a 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -138,7 +138,7 @@ uint8_t getConfigBatteryProfile(void); bool setConfigBatteryProfile(uint8_t profileIndex); void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); -void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT]); +void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]); void setGravityCalibration(float getGravity); bool canSoftwareSerialBeUsed(void); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4e825f374a..ac8cf9a335 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -983,12 +983,12 @@ void taskUpdateRxMain(timeUs_t currentTimeUs) } // returns seconds -float getFlightTime() +float getFlightTime(void) { return US2S(flightTime); } -float getArmTime() +float getArmTime(void) { return US2S(armTime); } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7c36b49f0b..1b6d2df471 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -119,6 +119,7 @@ #include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" +#include "io/vtx_msp.h" #include "io/vtx_ffpv24g.h" #include "io/piniobox.h" @@ -174,7 +175,7 @@ void flashLedsAndBeep(void) LED1_TOGGLE; LED0_TOGGLE; delay(25); - if (!(getPreferredBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) + if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON; delay(25); BEEP_OFF; @@ -665,6 +666,10 @@ void init(void) vtxFuriousFPVInit(); #endif +#ifdef USE_VTX_MSP + vtxMspInit(); +#endif + #endif // USE_VTX_CONTROL // Now that everything has powered up the voltage and cell count be determined. diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 346afe7488..7ea2ff20eb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2482,12 +2482,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel const uint8_t newBand = (newFrequency / 8) + 1; const uint8_t newChannel = (newFrequency % 8) + 1; + + if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { + vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel); + } + vtxSettingsConfigMutable()->band = newBand; vtxSettingsConfigMutable()->channel = newChannel; } if (sbufBytesRemaining(src) > 1) { - vtxSettingsConfigMutable()->power = sbufReadU8(src); + uint8_t newPower = sbufReadU8(src); + uint8_t currentPower = 0; + vtxCommonGetPowerIndex(vtxDevice, ¤tPower); + if (newPower != currentPower) { + vtxCommonSetPowerByIndex(vtxDevice, newPower); + vtxSettingsConfigMutable()->power = newPower; + } + // Delegate pitmode to vtx directly const uint8_t newPitmode = sbufReadU8(src); uint8_t currentPitmode = 0; @@ -3484,7 +3496,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu 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); @@ -3495,10 +3509,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! } - } else if (!areSensorsCalibrating()) { + } else { if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once #ifdef USE_BARO - baroStartCalibration(); + 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 diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 7b2ba3b5d5..b41acdb51b 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -64,6 +64,7 @@ #include "io/rcdevice_cam.h" #include "io/smartport_master.h" #include "io/vtx.h" +#include "io/vtx_msp.h" #include "io/osd_dji_hd.h" #include "io/displayport_msp_osd.h" #include "io/servo_sbus.h" @@ -109,6 +110,9 @@ void taskHandleSerial(timeUs_t currentTimeUs) #ifdef USE_MSP_OSD // Capture MSP Displayport messages to determine if VTX is connected mspOsdSerialProcess(mspFcProcessCommand); +#ifdef USE_VTX_MSP + mspVtxSerialProcess(mspFcProcessCommand); +#endif #endif } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 457d1630d4..15dadd1c59 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -179,7 +179,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) #ifdef USE_SIMULATOR simulatorData_t simulatorData = { - flags: 0, - debugIndex: 0 + .flags = 0, + .debugIndex = 0 }; #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4e9e4e8d86..fb25aeee7c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1542,12 +1542,30 @@ groups: default_value: OFF field: ubloxUseGalileo type: bool + - name: gps_ublox_use_beidou + description: "Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]." + default_value: OFF + field: ubloxUseBeidou + type: bool + - name: gps_ublox_use_glonass + description: "Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]." + default_value: OFF + field: ubloxUseGlonass + type: bool - name: gps_min_sats description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count." default_value: 6 field: gpsMinSats min: 5 max: 10 + - name: gps_ublox_nav_hz + description: "Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximum number of satellites tracked when set to a higher rate or even stop sending navigation updates if the value is too high. Some M10 devices can do up to 25Hz. 10 is a safe value for M8 and newer." + default_value: 10 + field: ubloxNavHz + type: uint8_t + min: 5 + max: 200 + - name: PG_RC_CONTROLS_CONFIG type: rcControlsConfig_t @@ -2297,6 +2315,11 @@ groups: field: general.land_detect_sensitivity min: 1 max: 15 + - name: nav_landing_bump_detection + description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors." + default_value: OFF + field: general.flags.landing_bump_detection + type: bool - name: nav_use_midthr_for_althold description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude" default_value: OFF diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index 3a164e6cc6..f719ccb0d6 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -36,7 +36,7 @@ static uint32_t arm_distance_cm; static uint32_t arm_mWhDrawn; static uint32_t flyingEnergy; // energy drawn during flying up to last disarm (ARMED) mWh -uint32_t getFlyingEnergy() { +uint32_t getFlyingEnergy(void) { return flyingEnergy; } #endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index cee5355bd0..b46224063f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -277,7 +277,7 @@ static void imuResetOrientationQuaternion(const fpVector3_t * accBF) static bool imuValidateQuaternion(const fpQuaternion_t * quat) { - const float check = fabs(quat->q0) + fabs(quat->q1) + fabs(quat->q2) + fabs(quat->q3); + const float check = fabsf(quat->q0) + fabsf(quat->q1) + fabsf(quat->q2) + fabsf(quat->q3); if (!isnan(check) && !isinf(check)) { return true; @@ -480,7 +480,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe const float thetaMagnitudeSq = vectorNormSquared(&vTheta); // If calculated rotation is zero - don't update quaternion - if (thetaMagnitudeSq >= 1e-20) { + if (thetaMagnitudeSq >= 1e-20f) { // Calculate quaternion delta: // Theta is a axis/angle rotation. Direction of a vector is axis, magnitude is angle/2. // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero. diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 00142fea55..765c2f1637 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -461,7 +461,7 @@ static int getReversibleMotorsThrottleDeadband(void) return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue; } -void FAST_CODE mixTable() +void FAST_CODE mixTable(void) { #ifdef USE_DSHOT if (FLIGHT_MODE(TURTLE_MODE)) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b240f7a5cb..20333071b0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM float iTermAntigravityGain; #endif static EXTENDED_FASTRAM uint8_t usedPidControllerType; -typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT); +typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv); static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; @@ -477,7 +477,7 @@ void schedulePidGainsUpdate(void) pidGainsUpdateRequired = true; } -void updatePIDCoefficients() +void updatePIDCoefficients(void) { STATIC_FASTRAM uint16_t prevThrottle = 0; @@ -625,7 +625,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam } } - float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); + float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); // Apply simple LPF to angleRateTarget to make response less jerky // Ideas behind this: @@ -681,13 +681,13 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) { } #ifdef USE_D_BOOST -static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT) { +static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) { float dBoost = 1.0f; - const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; + const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv; const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); - const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) / dT); + const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv); if (dBoostGyroAcceleration >= dBoostRateAcceleration) { //Gyro is accelerating faster than setpoint, we want to smooth out @@ -710,7 +710,7 @@ static float applyDBoost(pidState_t *pidState, float dT) { } #endif -static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT) { +static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) { // Calculate new D-term float newDTerm = 0; if (pidState->kD == 0) { @@ -722,7 +722,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); // Calculate derivative - newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, currentRateTarget, dT); + newDTerm = delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv); } return(newDTerm); } @@ -736,19 +736,20 @@ static void applyItermLimiting(pidState_t *pidState) { } } -static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { +static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { UNUSED(pidState); UNUSED(axis); UNUSED(dT); + UNUSED(dT_inv); } -static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) +static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); - const float newDTerm = dTermProcess(pidState, rateTarget, dT); + const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv); const float newFFTerm = rateTarget * pidState->kFF; /* @@ -806,14 +807,14 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, return itermErrorRate; } -static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) +static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); - const float newDTerm = dTermProcess(pidState, rateTarget, dT); + const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv); const float rateTargetDelta = rateTarget - pidState->previousRateTarget; const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta); @@ -864,7 +865,7 @@ void resetHeadingHoldTarget(int16_t heading) pt1FilterReset(&headingHoldRateFilter, 0.0f); } -int16_t getHeadingHoldTarget() { +int16_t getHeadingHoldTarget(void) { return headingHoldTarget; } @@ -1062,6 +1063,9 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis void FAST_CODE pidController(float dT) { + + const float dT_inv = 1.0f / dT; + if (!pidFiltersConfigured) { return; } @@ -1135,7 +1139,7 @@ void FAST_CODE pidController(float dT) } // Prevent strong Iterm accumulation during stick inputs - antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); + antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits @@ -1145,7 +1149,7 @@ void FAST_CODE pidController(float dT) checkItermLimitingActive(&pidState[axis]); checkItermFreezingActive(&pidState[axis], axis); - pidControllerApplyFn(&pidState[axis], axis, dT); + pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv); } } @@ -1177,7 +1181,7 @@ void pidInit(void) itermRelax = pidProfile()->iterm_relax; yawLpfHz = pidProfile()->yaw_lpf_hz; - motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); + motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f)); #ifdef USE_D_BOOST dBoostMin = pidProfile()->dBoostMin; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e7d3981a07..de0e3126b7 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -47,7 +47,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13 #define FP_PID_RATE_I_MULTIPLIER 4.0f #define FP_PID_RATE_D_MULTIPLIER 1905.0f #define FP_PID_RATE_D_FF_MULTIPLIER 7270.0f -#define FP_PID_LEVEL_P_MULTIPLIER 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s] +#define FP_PID_LEVEL_P_MULTIPLIER 1.0f / 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s] #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f #define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index d9688f1e82..2eaef1175c 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -187,7 +187,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa // Use different max rate in ANLGE mode if (FLIGHT_MODE(ANGLE_MODE)) { - float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER; + float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode); } diff --git a/src/main/flight/power_limits.c b/src/main/flight/power_limits.c index ce742df162..1fe13240fe 100644 --- a/src/main/flight/power_limits.c +++ b/src/main/flight/power_limits.c @@ -162,14 +162,14 @@ void powerLimiterApply(int16_t *throttleCommand) { int32_t overCurrent = current - activeCurrentLimit; if (lastCallTimestamp) { - currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7, 0, PWM_RANGE_MAX - PWM_RANGE_MIN); + currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN); } - float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3; + float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3f; - uint16_t currentThrAttn = lrintf(pt1FilterApply3(¤tThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6)); + uint16_t currentThrAttn = lrintf(pt1FilterApply3(¤tThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6f)); - throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(¤tThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand; + throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(¤tThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6f)) : *throttleCommand; uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn); if (activeCurrentLimit && currentThrAttned < *throttleCommand) { @@ -191,12 +191,12 @@ void powerLimiterApply(int16_t *throttleCommand) { int32_t overPower = power - activePowerLimit; if (lastCallTimestamp) { - powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5, 0, PWM_RANGE_MAX - PWM_RANGE_MIN); + powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN); } - float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1; + float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1f; - uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6)); + uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6f)); throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand; uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn); @@ -244,11 +244,11 @@ bool powerLimiterIsLimitingPower(void) { // returns seconds float powerLimiterGetRemainingBurstTime(void) { uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent; - float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7; + float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7f; #ifdef USE_ADC uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower; - float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7; + float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7f; if (!currentBatteryProfile->powerLimits.continuousCurrent) { return remainingPowerBurstTime; diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 1597737a34..eed9adbae3 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -150,7 +150,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { float RTH_heading; // degrees #ifdef USE_WIND_ESTIMATOR - uint16_t windHeading; // centidegrees + uint16_t windHeading = 0; // centidegrees const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading); const float verticalWindSpeed = -getEstimatedWindSpeed(Z) / 100; //from NED to NEU diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c index f32e6aeb61..6a363c4f23 100644 --- a/src/main/flight/smith_predictor.c +++ b/src/main/flight/smith_predictor.c @@ -54,7 +54,7 @@ float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sampl } void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) { - if (delay > 0.1) { + if (delay > 0.1f) { predictor->enabled = true; predictor->samples = (delay * 1000) / looptime; predictor->idx = 0; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 987e3acdf0..af0f826c88 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -218,7 +218,7 @@ static const beeperTableEntry_t *currentBeeperEntry = NULL; */ void beeper(beeperMode_e mode) { - if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (getBatteryCellCount() < 2))) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)) { + if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (getBatteryState() == BATTERY_NOT_PRESENT))) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)) { beeperSilence(); return; } diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index a6a103ab4e..444d91378a 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -400,7 +400,7 @@ static void showStatusPage(void) #ifdef USE_MAG if (sensors(SENSOR_MAG)) { - tfp_sprintf(lineBuffer, "HDG: %d", DECIDEGREES_TO_DEGREES(attitude.values.yaw)); + tfp_sprintf(lineBuffer, "HDG: %d", (int)DECIDEGREES_TO_DEGREES(attitude.values.yaw)); padHalfLineBuffer(); i2c_OLED_set_line(rowIndex); i2c_OLED_send_string(lineBuffer); diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 2b0d05f022..17f49c8b78 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -515,4 +515,13 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) } } +mspPort_t *getMspOsdPort() +{ + if (mspPort.port) { + return &mspPort; + } + + return NULL; +} + #endif // USE_MSP_OSD diff --git a/src/main/io/displayport_msp_osd.h b/src/main/io/displayport_msp_osd.h index cea787538d..0a2f64c48a 100644 --- a/src/main/io/displayport_msp_osd.h +++ b/src/main/io/displayport_msp_osd.h @@ -31,6 +31,7 @@ typedef struct displayPort_s displayPort_t; displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem); void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); +mspPort_t *getMspOsdPort(void); // MSP displayport V2 attribute byte bit functions #define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e diff --git a/src/main/io/frsky_osd.c b/src/main/io/frsky_osd.c index 7ba9b75e36..a69df4e104 100644 --- a/src/main/io/frsky_osd.c +++ b/src/main/io/frsky_osd.c @@ -365,7 +365,7 @@ static void frskyOSDUpdateReceiveBuffer(void) // Full uvarint decoded. Check against buffer size. if (state.recvBuffer.expected > sizeof(state.recvBuffer.data)) { FRSKY_OSD_ERROR("Can't handle payload of size %u with a buffer of size %u", - state.recvBuffer.expected, sizeof(state.recvBuffer.data)); + state.recvBuffer.expected, (unsigned int)sizeof(state.recvBuffer.data)); frskyOSDResetReceiveBuffer(); break; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 5af00c8dcd..965b89531a 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -55,6 +55,7 @@ #include "io/serial.h" #include "io/gps.h" #include "io/gps_private.h" +#include "io/gps_ublox.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -91,7 +92,7 @@ gpsSolutionData_t gpsSolDRV; //filled by driver gpsSolutionData_t gpsSol; //used in the rest of the code // Map gpsBaudRate_e index to baudRate_e -baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 }; +baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 }; static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { /* NMEA GPS */ @@ -130,7 +131,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { }; -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = SETTING_GPS_PROVIDER_DEFAULT, @@ -139,9 +140,69 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT, .dynModel = SETTING_GPS_DYN_MODEL_DEFAULT, .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT, - .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT + .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT, + .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT, + .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT, + .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT ); + +int getGpsBaudrate(void) +{ + switch(gpsState.baudrateIndex) + { + case GPS_BAUDRATE_115200: + return 115200; + case GPS_BAUDRATE_57600: + return 57600; + case GPS_BAUDRATE_38400: + return 38400; + case GPS_BAUDRATE_19200: + return 19200; + case GPS_BAUDRATE_9600: + return 9600; + case GPS_BAUDRATE_230400: + return 230400; + case GPS_BAUDRATE_460800: + return 460800; + case GPS_BAUDRATE_921600: + return 921600; + default: + return 0; + } +} + +const char *getGpsHwVersion(void) +{ + switch(gpsState.hwVersion) + { + case UBX_HW_VERSION_UBLOX5: + return "UBLOX5"; + case UBX_HW_VERSION_UBLOX6: + return "UBLOX6"; + case UBX_HW_VERSION_UBLOX7: + return "UBLOX7"; + case UBX_HW_VERSION_UBLOX8: + return "UBLOX8"; + case UBX_HW_VERSION_UBLOX9: + return "UBLOX9"; + case UBX_HW_VERSION_UBLOX10: + return "UBLOX10"; + default: + return "Unknown"; + } +} + +uint8_t getGpsProtoMajorVersion(void) +{ + return gpsState.swVersionMajor; +} + +uint8_t getGpsProtoMinorVersion(void) +{ + return gpsState.swVersionMinor; +} + void gpsSetState(gpsState_e state) { gpsState.state = state; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index a83662588c..31fa2c56bb 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -59,6 +59,8 @@ typedef enum { GPS_BAUDRATE_19200, GPS_BAUDRATE_9600, GPS_BAUDRATE_230400, + GPS_BAUDRATE_460800, + GPS_BAUDRATE_921600, GPS_BAUDRATE_COUNT } gpsBaudRate_e; @@ -93,7 +95,10 @@ typedef struct gpsConfig_s { gpsAutoBaud_e autoBaud; gpsDynModel_e dynModel; bool ubloxUseGalileo; + bool ubloxUseBeidou; + bool ubloxUseGlonass; uint8_t gpsMinSats; + uint8_t ubloxNavHz; } gpsConfig_t; PG_DECLARE(gpsConfig_t, gpsConfig); @@ -163,6 +168,12 @@ struct serialPort_s; void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); void mspGPSReceiveNewData(const uint8_t * bufferPtr); +const char *getGpsHwVersion(void); +uint8_t getGpsProtoMajorVersion(void); +uint8_t getGpsProtoMinorVersion(void); + +int getGpsBaudrate(void); + #if defined(USE_GPS_FAKE) void gpsFakeSet( gpsFixType_e fixType, diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 8c0d8a3ec4..ca2c15db14 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -43,6 +43,8 @@ typedef struct { serialPort_t * gpsPort; // Serial GPS only uint32_t hwVersion; + uint8_t swVersionMajor; + uint8_t swVersionMinor; gpsState_e state; gpsBaudRate_e baudrateIndex; @@ -54,6 +56,8 @@ typedef struct { timeMs_t lastMessageMs; timeMs_t timeoutMs; timeMs_t baseTimeoutMs; + timeMs_t lastCapaPoolMs; + timeMs_t lastCapaUpdMs; } gpsReceiverData_t; extern gpsReceiverData_t gpsState; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 8f5be8186f..10f1e6d947 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -42,6 +42,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/serial.h" #include "io/gps.h" @@ -49,31 +50,9 @@ #include "scheduler/protothreads.h" -#define GPS_CFG_CMD_TIMEOUT_MS 200 -#define GPS_VERSION_RETRY_TIMES 2 -#define MAX_UBLOX_PAYLOAD_SIZE 256 -#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE -#define UBLOX_SBAS_MESSAGE_LENGTH 16 +#include "gps_ublox.h" +#include "gps_ublox_utils.h" -#define UBX_DYNMODEL_PEDESTRIAN 3 -#define UBX_DYNMODEL_AIR_1G 6 -#define UBX_DYNMODEL_AIR_4G 8 - -#define UBX_FIXMODE_2D_ONLY 1 -#define UBX_FIXMODE_3D_ONLY 2 -#define UBX_FIXMODE_AUTO 3 - -#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0) -#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1) -#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) - -#define UBX_HW_VERSION_UNKNOWN 0 -#define UBX_HW_VERSION_UBLOX5 500 -#define UBX_HW_VERSION_UBLOX6 600 -#define UBX_HW_VERSION_UBLOX7 700 -#define UBX_HW_VERSION_UBLOX8 800 -#define UBX_HW_VERSION_UBLOX9 900 -#define UBX_HW_VERSION_UBLOX10 1000 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE // note PRNs last upadted 2020-12-18 @@ -97,255 +76,10 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200 "$PUBX,41,1,0003,0001,9600,0*16\r\n", // GPS_BAUDRATE_9600 "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400 + "$PUBX,41,1,0003,0001,460800,0*1C\r\n", // GPS_BAUDRATE_460800 + "$PUBX,41,1,0003,0001,921600,0*1C\r\n" // GPS_BAUDRATE_921600 }; -// payload types -typedef struct { - uint8_t mode; - uint8_t usage; - uint8_t maxSBAS; - uint8_t scanmode2; - uint32_t scanmode1; -} ubx_sbas; - -typedef struct { - uint8_t class; - uint8_t id; - uint8_t rate; -} ubx_msg; - -typedef struct { - uint16_t meas; - uint16_t nav; - uint16_t time; -} ubx_rate; - -typedef struct { - uint8_t gnssId; - uint8_t resTrkCh; - uint8_t maxTrkCh; - uint8_t reserved1; -// flags - uint8_t enabled; - uint8_t undefined0; - uint8_t sigCfgMask; - uint8_t undefined1; -} ubx_gnss_element_t; - -typedef struct { - uint8_t msgVer; - uint8_t numTrkChHw; - uint8_t numTrkChUse; - uint8_t numConfigBlocks; - ubx_gnss_element_t config[0]; -} ubx_gnss_msg_t; - -#define MAX_GNSS 7 -#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) - -typedef union { - uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder - ubx_sbas sbas; - ubx_msg msg; - ubx_rate rate; - ubx_gnss_msg_t gnss; -} ubx_payload; - -// UBX support -typedef struct { - uint8_t preamble1; - uint8_t preamble2; - uint8_t msg_class; - uint8_t msg_id; - uint16_t length; -} ubx_header; - -typedef struct { - ubx_header header; - ubx_payload payload; -} __attribute__((packed)) ubx_message; - -typedef struct { - char swVersion[30]; // Zero-terminated Software Version String - char hwVersion[10]; // Zero-terminated Hardware Version String -} ubx_mon_ver; - -typedef struct { - uint32_t time; // GPS msToW - int32_t longitude; - int32_t latitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; -} ubx_nav_posllh; - -typedef struct { - uint32_t time; // GPS msToW - uint8_t fix_type; - uint8_t fix_status; - uint8_t differential_status; - uint8_t res; - uint32_t time_to_first_fix; - uint32_t uptime; // milliseconds -} ubx_nav_status; - -typedef struct { - uint32_t time; - int32_t time_nsec; - int16_t week; - uint8_t fix_type; - uint8_t fix_status; - int32_t ecef_x; - int32_t ecef_y; - int32_t ecef_z; - uint32_t position_accuracy_3d; - int32_t ecef_x_velocity; - int32_t ecef_y_velocity; - int32_t ecef_z_velocity; - uint32_t speed_accuracy; - uint16_t position_DOP; - uint8_t res; - uint8_t satellites; - uint32_t res2; -} ubx_nav_solution; - -typedef struct { - uint32_t time; // GPS msToW - int32_t ned_north; - int32_t ned_east; - int32_t ned_down; - uint32_t speed_3d; - uint32_t speed_2d; - int32_t heading_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; -} ubx_nav_velned; - -typedef struct { - uint8_t chn; // Channel number, 255 for SVx not assigned to channel - uint8_t svid; // Satellite ID - uint8_t flags; // Bitmask - uint8_t quality; // Bitfield - uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55. - uint8_t elev; // Elevation in integer degrees - int16_t azim; // Azimuth in integer degrees - int32_t prRes; // Pseudo range residual in centimetres -} ubx_nav_svinfo_channel; - -typedef struct { - uint32_t time; // GPS Millisecond time of week - uint8_t numCh; // Number of channels - uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6 - uint16_t reserved2; // Reserved - ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte -} ubx_nav_svinfo; - -typedef struct { - uint32_t time; // GPS msToW - uint32_t tAcc; - int32_t nano; - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; -} ubx_nav_timeutc; - -typedef struct { - uint32_t time; // GPS msToW - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; - uint32_t tAcc; - int32_t nano; - uint8_t fix_type; - uint8_t fix_status; - uint8_t reserved1; - uint8_t satellites; - int32_t longitude; - int32_t latitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; - int32_t ned_north; - int32_t ned_east; - int32_t ned_down; - int32_t speed_2d; - int32_t heading_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; - uint16_t position_DOP; - uint16_t reserved2; - uint16_t reserved3; -} ubx_nav_pvt; - -typedef struct { - uint8_t class; - uint8_t msg; -} ubx_ack_ack; - -enum { - PREAMBLE1 = 0xB5, - PREAMBLE2 = 0x62, - CLASS_NAV = 0x01, - CLASS_ACK = 0x05, - CLASS_CFG = 0x06, - CLASS_MON = 0x0A, - MSG_CLASS_UBX = 0x01, - MSG_CLASS_NMEA = 0xF0, - MSG_VER = 0x04, - MSG_ACK_NACK = 0x00, - MSG_ACK_ACK = 0x01, - MSG_NMEA_GGA = 0x0, - MSG_NMEA_GLL = 0x1, - MSG_NMEA_GSA = 0x2, - MSG_NMEA_GSV = 0x3, - MSG_NMEA_RMC = 0x4, - MSG_NMEA_VGS = 0x5, - MSG_POSLLH = 0x2, - MSG_STATUS = 0x3, - MSG_SOL = 0x6, - MSG_PVT = 0x7, - MSG_VELNED = 0x12, - MSG_TIMEUTC = 0x21, - MSG_SVINFO = 0x30, - MSG_NAV_SAT = 0x35, - MSG_NAV_SIG = 0x35, - MSG_CFG_PRT = 0x00, - MSG_CFG_RATE = 0x08, - MSG_CFG_SET_RATE = 0x01, - MSG_CFG_NAV_SETTINGS = 0x24, - MSG_CFG_SBAS = 0x16, - MSG_CFG_GNSS = 0x3e -} ubx_protocol_bytes; - -enum { - FIX_NONE = 0, - FIX_DEAD_RECKONING = 1, - FIX_2D = 2, - FIX_3D = 3, - FIX_GPS_DEAD_RECKONING = 4, - FIX_TIME = 5 -} ubs_nav_fix_type; - -enum { - NAV_STATUS_FIX_VALID = 1 -} ubx_nav_status_bits; - -enum { - UBX_ACK_WAITING = 0, - UBX_ACK_GOT_ACK = 1, - UBX_ACK_GOT_NAK = 2 -} ubx_ack_state; - // Packet checksum accumulators static uint8_t _ck_a; static uint8_t _ck_b; @@ -369,7 +103,12 @@ static bool _new_position; static bool _new_speed; // Need this to determine if Galileo capable only -static bool capGalileo; +static struct { + uint8_t supported; + int capMaxGnss; + uint8_t defaultGnss; + uint8_t enabledGnss; +} ubx_capabilities = { }; // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver. //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status' @@ -401,16 +140,63 @@ static union { ubx_mon_ver ver; ubx_nav_timeutc timeutc; ubx_ack_ack ack; + ubx_mon_gnss gnss; uint8_t bytes[UBLOX_BUFFER_SIZE]; } _buffer; -void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) +bool gpsUbloxHasGalileo(void) { - while (len--) { - *ck_a += *data; - *ck_b += *ck_a; - data++; + return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK); } + +bool gpsUbloxHasBeidou(void) +{ + return ubx_capabilities.supported & UBX_MON_GNSS_BEIDOU_MASK; +} + +bool gpsUbloxHasGlonass(void) +{ + return ubx_capabilities.supported & UBX_MON_GNSS_GLONASS_MASK; +} + +bool gpsUbloxGalileoDefault(void) +{ + return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GALILEO_MASK; +} + +bool gpsUbloxBeidouDefault(void) +{ + return ubx_capabilities.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK; +} + +bool gpsUbloxGlonassDefault(void) +{ + return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GLONASS_MASK; +} + +bool gpsUbloxGalileoEnabled(void) +{ + return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GALILEO_MASK; +} + +bool gpsUbloxBeidouEnabled(void) +{ + return ubx_capabilities.enabledGnss & UBX_MON_GNSS_BEIDOU_MASK; +} + +bool gpsUbloxGlonassEnabled(void) +{ + return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GLONASS_MASK; +} + +uint8_t gpsUbloxMaxGnss(void) +{ + return ubx_capabilities.capMaxGnss; +} + +timeMs_t gpsUbloxCapLastUpdate(void) +{ + return gpsState.lastCapaUpdMs; } static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType) @@ -427,7 +213,7 @@ static void sendConfigMessageUBLOX(void) uint8_t ck_a=0, ck_b=0; send_buffer.message.header.preamble1=PREAMBLE1; send_buffer.message.header.preamble2=PREAMBLE2; - _update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b); + ublox_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b); send_buffer.bytes[send_buffer.message.header.length+6] = ck_a; send_buffer.bytes[send_buffer.message.header.length+7] = ck_b; serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8); @@ -445,6 +231,15 @@ static void pollVersion(void) sendConfigMessageUBLOX(); } +static void pollGnssCapabilities(void) +{ + send_buffer.message.header.msg_class = CLASS_MON; + send_buffer.message.header.msg_id = MSG_MON_GNSS; + send_buffer.message.header.length = 0; + sendConfigMessageUBLOX(); +} + + static const uint8_t default_payload[] = { 0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code) 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and @@ -454,7 +249,26 @@ static const uint8_t default_payload[] = { #define GNSSID_SBAS 1 #define GNSSID_GALILEO 2 +#define GNSSID_BEIDOU 3 +#define GNSSID_GZSS 5 +#define GNSSID_GLONASS 6 +// M10 ublox protocol info: +// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf +static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) +{ + ubx_config_data8_t cfg = {}; + + ubloxCfgFillBytes(&cfg, kvPairs, count); + + serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8); + _ack_waiting_msg = cfg.header.msg_id; + _ack_state = UBX_ACK_WAITING; +} + +// Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration +// https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf +// https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) { gnss_block->gnssId = GNSSID_SBAS; @@ -473,13 +287,17 @@ static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) { - if (!capGalileo) { + if (!gpsUbloxHasGalileo()) { return 0; } gnss_block->gnssId = GNSSID_GALILEO; gnss_block->maxTrkCh = 8; - gnss_block->sigCfgMask = 1; + // sigCfgMask + // 0x01 = Galileo E1 (not supported for protocol versions less than 18.00) + // 0x10 = Galileo E5a // off by default + // 0x20 = Galileo E5b // off by default + gnss_block->sigCfgMask = 0x01; if (gpsState.gpsConfig->ubloxUseGalileo) { gnss_block->enabled = 1; gnss_block->resTrkCh = 4; @@ -491,15 +309,109 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) return 1; } +static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) +{ + if (!gpsUbloxHasBeidou()) { + return 0; + } + + gnss_block->gnssId = GNSSID_BEIDOU; + gnss_block->maxTrkCh = 8; + // sigCfgMask + // 0x01 = BeiDou B1I + // 0x10 = BeiDou B2I // off by default + // 0x80 = BeiDou B2A // off by default + gnss_block->sigCfgMask = 0x01; + if (gpsState.gpsConfig->ubloxUseBeidou) { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + } else { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } + + return 1; +} + +/* +static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) +{ + if (!ubx_capabilities.capGzss) { + return 0; + } + + gnss_block->gnssId = GNSSID_GZSS; + gnss_block->maxTrkCh = 8; + // L1C = 0x01 + // L1S = 0x04 + // L2C = 0x10 + gnss_block->sigCfgMask = 0x01 | 0x04; + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + + return 1; +} +*/ + +static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) +{ + if(!gpsUbloxHasGlonass()) { + return 0; + } + + gnss_block->gnssId = GNSSID_GLONASS; + gnss_block->maxTrkCh = 8; + // 0x01 = GLONASS L1 + // 0x10 = GLONASS L2 // off by default + gnss_block->sigCfgMask = 0x01; + if (gpsState.gpsConfig->ubloxUseGlonass) { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + } else { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } + + return 1; +} + +static void configureGNSS10(void) +{ + ubx_config_data8_payload_t gnssConfigValues[] = { + // SBAS + {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, + {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, + + // Galileo + {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + + // Beidou + {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, + + // Should be enabled with GPS + {UBLOX_CFG_QZSS_ENA, 1}, + {UBLOX_CFG_QZSS_L1CA_ENA, 1}, + {UBLOX_CFG_QZSS_L1S_ENA, 1}, + + // Glonass + {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, + {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} + }; + + ubloxSendSetCfgBytes(gnssConfigValues, 12); +} + static void configureGNSS(void) { int blocksUsed = 0; - send_buffer.message.header.msg_class = CLASS_CFG; - send_buffer.message.header.msg_id = MSG_CFG_GNSS; + send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET send_buffer.message.payload.gnss.msgVer = 0; send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset - send_buffer.message.payload.gnss.numTrkChUse = 32; + send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max /* SBAS, always generated */ blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); @@ -507,6 +419,12 @@ static void configureGNSS(void) /* Galileo */ blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* BeiDou */ + blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); + + /* GLONASS */ + blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); + send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed); sendConfigMessageUBLOX(); @@ -523,12 +441,12 @@ static void configureNAV5(uint8_t dynModel, uint8_t fixMode) sendConfigMessageUBLOX(); } -static void configureMSG(uint8_t class, uint8_t id, uint8_t rate) +static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate) { send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_SET_RATE; send_buffer.message.header.length = 3; - send_buffer.message.payload.msg.class = class; + send_buffer.message.payload.msg.msg_class = msg_class; send_buffer.message.payload.msg.id = id; send_buffer.message.payload.msg.rate = rate; sendConfigMessageUBLOX(); @@ -565,6 +483,18 @@ static void configureSBAS(void) sendConfigMessageUBLOX(); } +static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) +{ + if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) { + proto+=8; + + float ver = atof(proto); + + gpsState.swVersionMajor = (uint8_t)ver; + gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f); + } +} + static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) { // ublox_5 hwVersion 00040005 @@ -600,7 +530,7 @@ static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) return UBX_HW_VERSION_UNKNOWN; } -static bool gpsParceFrameUBLOX(void) +static bool gpsParseFrameUBLOX(void) { switch (_msg_id) { case MSG_POSLLH: @@ -690,18 +620,51 @@ static bool gpsParceFrameUBLOX(void) case MSG_VER: if (_class == CLASS_MON) { gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); - if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) && (_buffer.ver.swVersion[9] > '2')) { + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { + if (_buffer.ver.swVersion[9] > '2' || true) { // check extensions; // after hw + sw vers; each is 30 bytes + bool found = false; + for (int j = 40; j < _payload_length && !found; j += 30) + { + // Example content: GPS;GAL;BDS;GLO + if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30)) + { + ubx_capabilities.supported |= UBX_MON_GNSS_GALILEO_MASK; + found = true; + } + if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30)) + { + ubx_capabilities.supported |= UBX_MON_GNSS_BEIDOU_MASK; + found = true; + } + if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30)) + { + ubx_capabilities.supported |= UBX_MON_GNSS_GLONASS_MASK; + found = true; + } + } + } for(int j = 40; j < _payload_length; j += 30) { - if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) { - capGalileo = true; + if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) { + gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30); break; } } } } break; + case MSG_MON_GNSS: + if(_class == CLASS_MON) { + if (_buffer.gnss.version == 0) { + ubx_capabilities.supported = _buffer.gnss.supported; + ubx_capabilities.defaultGnss = _buffer.gnss.defaultGnss; + ubx_capabilities.enabledGnss = _buffer.gnss.enabled; + ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent; + gpsState.lastCapaUpdMs = millis(); + } + } + break; case MSG_ACK_ACK: if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) { _ack_state = UBX_ACK_GOT_ACK; @@ -808,7 +771,7 @@ static bool gpsNewFrameUBLOX(uint8_t data) break; } - if (gpsParceFrameUBLOX()) { + if (gpsParseFrameUBLOX()) { parsed = true; } } @@ -816,6 +779,11 @@ static bool gpsNewFrameUBLOX(uint8_t data) return parsed; } +static uint16_t hz2rate(uint8_t hz) +{ + return 1000 / hz; +} + STATIC_PROTOTHREAD(gpsConfigure) { ptBegin(gpsConfigure); @@ -885,11 +853,19 @@ STATIC_PROTOTHREAD(gpsConfigure) configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); - // u-Blox 9 receivers such as M9N can do 10Hz as well, but the number of used satellites will be restricted to 16. - // Not mentioned in the datasheet - configureRATE(200); + if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { + configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz + } else { + configureRATE(hz2rate(5)); // 5Hz + gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT; + } + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + + if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error + configureRATE(hz2rate(5)); // 5Hz ptWait(_ack_state == UBX_ACK_GOT_ACK); } + } else { // u-Blox 5/6/7/8 or unknown // u-Blox 7-8 support PVT @@ -916,13 +892,18 @@ STATIC_PROTOTHREAD(gpsConfigure) ptWait(_ack_state == UBX_ACK_GOT_ACK); if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { - configureRATE(100); // 10Hz + configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz } else { - configureRATE(200); // 5Hz + configureRATE(hz2rate(5)); // 5Hz } + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + + if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error + configureRATE(hz2rate(5)); // 5Hz ptWait(_ack_state == UBX_ACK_GOT_ACK); } + } // u-Blox 5/6 doesn't support PVT, use legacy config // UNKNOWN also falls here, use as a last resort else { @@ -962,10 +943,20 @@ STATIC_PROTOTHREAD(gpsConfigure) ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); // Configure GNSS for M8N and later - if (gpsState.hwVersion >= 80000) { + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { + configureGNSS10(); + } else { configureGNSS(); + } ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + + if(_ack_state == UBX_ACK_GOT_NAK) { + gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; + gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT; + gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT; + } } ptEnd(0); @@ -1028,8 +1019,6 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); } - // Configure GPS module if enabled - if (gpsState.gpsConfig->autoConfig) { // Reset protocol timeout gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); @@ -1043,6 +1032,16 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + gpsState.autoConfigStep = 0; + ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; + do { + pollGnssCapabilities(); + gpsState.autoConfigStep++; + ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); + + // Configure GPS module if enabled + if (gpsState.gpsConfig->autoConfig) { // Configure GPS ptSpawn(gpsConfigure); } @@ -1054,6 +1053,21 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) while (1) { ptSemaphoreWait(semNewDataReady); gpsProcessNewSolutionData(false); + + if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) { + if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { + gpsState.lastCapaPoolMs = millis(); + + if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) + { + pollVersion(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } + + pollGnssCapabilities(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } + } } ptEnd(0); diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h new file mode 100644 index 0000000000..00b42eeb2b --- /dev/null +++ b/src/main/io/gps_ublox.h @@ -0,0 +1,429 @@ +/* + * This file is part of INAV + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "common/time.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define GPS_CFG_CMD_TIMEOUT_MS 500 +#define GPS_VERSION_RETRY_TIMES 3 +#define MAX_UBLOX_PAYLOAD_SIZE 256 +#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE +#define UBLOX_SBAS_MESSAGE_LENGTH 16 +#define GPS_CAPA_INTERVAL 5000 + +#define UBX_DYNMODEL_PEDESTRIAN 3 +#define UBX_DYNMODEL_AIR_1G 6 +#define UBX_DYNMODEL_AIR_4G 8 + +#define UBX_FIXMODE_2D_ONLY 1 +#define UBX_FIXMODE_3D_ONLY 2 +#define UBX_FIXMODE_AUTO 3 + +#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0) +#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1) +#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) + +#define UBX_HW_VERSION_UNKNOWN 0 +#define UBX_HW_VERSION_UBLOX5 500 +#define UBX_HW_VERSION_UBLOX6 600 +#define UBX_HW_VERSION_UBLOX7 700 +#define UBX_HW_VERSION_UBLOX8 800 +#define UBX_HW_VERSION_UBLOX9 900 +#define UBX_HW_VERSION_UBLOX10 1000 + + +#define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1 +#define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1 + +#define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 // U1 +#define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 // U1 + +#define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 // U1 +#define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d // U1 +#define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // U1 default off + +#define UBLOX_CFG_QZSS_ENA 0x10310024 // U1 +#define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 // U1 +#define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 // U1 + +#define UBLOX_CFG_GLO_ENA 0x10310025 // U1 default off - may conflict with other constelations +#define UBLOX_CFG_GLO_L1_ENA 0x10310018 // U1 default off + +#define UBLOX_CFG_SBAS_PRNSCANMASK 0x50360006 // 0 = auto // X8 +#define UBLOX_SBAS_ALL 0x0000000000000000 //Enable search for all SBAS PRNs +#define UBLOX_SBAS_PRN120 0x0000000000000001 //Enable search for SBAS PRN120 +#define UBLOX_SBAS_PRN121 0x0000000000000002 //Enable search for SBAS PRN121 +#define UBLOX_SBAS_PRN122 0x0000000000000004 //Enable search for SBAS PRN122 +#define UBLOX_SBAS_PRN123 0x0000000000000008 //Enable search for SBAS PRN123 +#define UBLOX_SBAS_PRN124 0x0000000000000010 //Enable search for SBAS PRN124 +#define UBLOX_SBAS_PRN125 0x0000000000000020 //Enable search for SBAS PRN125 +#define UBLOX_SBAS_PRN126 0x0000000000000040 //Enable search for SBAS PRN126 +#define UBLOX_SBAS_PRN127 0x0000000000000080 //Enable search for SBAS PRN127 +#define UBLOX_SBAS_PRN128 0x0000000000000100 //Enable search for SBAS PRN128 +#define UBLOX_SBAS_PRN129 0x0000000000000200 //Enable search for SBAS PRN129 +#define UBLOX_SBAS_PRN130 0x0000000000000400 //Enable search for SBAS PRN130 +#define UBLOX_SBAS_PRN131 0x0000000000000800 //Enable search for SBAS PRN131 +#define UBLOX_SBAS_PRN132 0x0000000000001000 //Enable search for SBAS PRN132 +#define UBLOX_SBAS_PRN133 0x0000000000002000 //Enable search for SBAS PRN133 +#define UBLOX_SBAS_PRN134 0x0000000000004000 //Enable search for SBAS PRN134 +#define UBLOX_SBAS_PRN135 0x0000000000008000 //Enable search for SBAS PRN135 +#define UBLOX_SBAS_PRN136 0x0000000000010000 //Enable search for SBAS PRN136 +#define UBLOX_SBAS_PRN137 0x0000000000020000 //Enable search for SBAS PRN137 +#define UBLOX_SBAS_PRN138 0x0000000000040000 //Enable search for SBAS PRN138 +#define UBLOX_SBAS_PRN139 0x0000000000080000 //Enable search for SBAS PRN139 +#define UBLOX_SBAS_PRN140 0x0000000000100000 //Enable search for SBAS PRN140 +#define UBLOX_SBAS_PRN141 0x0000000000200000 //Enable search for SBAS PRN141 +#define UBLOX_SBAS_PRN142 0x0000000000400000 //Enable search for SBAS PRN142 +#define UBLOX_SBAS_PRN143 0x0000000000800000 //Enable search for SBAS PRN143 +#define UBLOX_SBAS_PRN144 0x0000000001000000 //Enable search for SBAS PRN144 +#define UBLOX_SBAS_PRN145 0x0000000002000000 //Enable search for SBAS PRN145 +#define UBLOX_SBAS_PRN146 0x0000000004000000 //Enable search for SBAS PRN146 +#define UBLOX_SBAS_PRN147 0x0000000008000000 //Enable search for SBAS PRN147 +#define UBLOX_SBAS_PRN148 0x0000000010000000 //Enable search for SBAS PRN148 +#define UBLOX_SBAS_PRN149 0x0000000020000000 //Enable search for SBAS PRN149 +#define UBLOX_SBAS_PRN150 0x0000000040000000 //Enable search for SBAS PRN150 +#define UBLOX_SBAS_PRN151 0x0000000080000000 //Enable search for SBAS PRN151 +#define UBLOX_SBAS_PRN152 0x0000000100000000 //Enable search for SBAS PRN152 +#define UBLOX_SBAS_PRN153 0x0000000200000000 //Enable search for SBAS PRN153 +#define UBLOX_SBAS_PRN154 0x0000000400000000 //Enable search for SBAS PRN154 +#define UBLOX_SBAS_PRN155 0x0000000800000000 //Enable search for SBAS PRN155 +#define UBLOX_SBAS_PRN156 0x0000001000000000 //Enable search for SBAS PRN156 +#define UBLOX_SBAS_PRN157 0x0000002000000000 //Enable search for SBAS PRN157 +#define UBLOX_SBAS_PRN158 0x0000004000000000 //Enable search for SBAS PRN158 + +// payload types +typedef struct { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} ubx_sbas; + +typedef struct { + uint8_t msg_class; + uint8_t id; + uint8_t rate; +} ubx_msg; + +typedef struct { + uint16_t meas; + uint16_t nav; + uint16_t time; +} ubx_rate; + +typedef struct { + uint8_t gnssId; + uint8_t resTrkCh; + uint8_t maxTrkCh; + uint8_t reserved1; +// flags + uint8_t enabled; + uint8_t undefined0; + uint8_t sigCfgMask; + uint8_t undefined1; +} ubx_gnss_element_t; + +typedef struct { + uint8_t msgVer; + uint8_t numTrkChHw; + uint8_t numTrkChUse; + uint8_t numConfigBlocks; + ubx_gnss_element_t config[0]; +} ubx_gnss_msg_t; + +typedef struct { + uint8_t version; + uint8_t layers; + uint8_t reserved; +} __attribute__((packed)) ubx_config_data_header_v0_t; + +typedef struct { + uint8_t version; + uint8_t layers; + uint8_t transaction; + uint8_t reserved; +} __attribute__((packed)) ubx_config_data_header_v1_t; + + +#define MAX_GNSS 7 +#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) + +typedef union { + uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder + ubx_sbas sbas; + ubx_msg msg; + ubx_rate rate; + ubx_gnss_msg_t gnss; +} ubx_payload; + +// UBX support +typedef struct { + uint8_t preamble1; + uint8_t preamble2; + uint8_t msg_class; + uint8_t msg_id; + uint16_t length; +} ubx_header; + +typedef struct { + uint32_t key; + uint8_t value; +} __attribute__((packed)) ubx_config_data8_payload_t; + + +#define MAX_CONFIG_SET_VAL_VALUES 32 + +typedef struct { + ubx_header header; + ubx_config_data_header_v1_t configHeader; + union { + ubx_config_data8_payload_t payload[0]; + uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES * sizeof(ubx_config_data8_payload_t)) + 2]; // 12 key/value pairs + 2 checksum bytes + } data; +} __attribute__((packed)) ubx_config_data8_t; + +typedef struct { + ubx_header header; + ubx_payload payload; +} __attribute__((packed)) ubx_message; + +typedef struct { + char swVersion[30]; // Zero-terminated Software Version String + char hwVersion[10]; // Zero-terminated Hardware Version String +} ubx_mon_ver; + +typedef struct { + uint32_t time; // GPS msToW + int32_t longitude; + int32_t latitude; + int32_t altitude_ellipsoid; + int32_t altitude_msl; + uint32_t horizontal_accuracy; + uint32_t vertical_accuracy; +} ubx_nav_posllh; + +typedef struct { + uint32_t time; // GPS msToW + uint8_t fix_type; + uint8_t fix_status; + uint8_t differential_status; + uint8_t res; + uint32_t time_to_first_fix; + uint32_t uptime; // milliseconds +} ubx_nav_status; + +typedef struct { + uint32_t time; + int32_t time_nsec; + int16_t week; + uint8_t fix_type; + uint8_t fix_status; + int32_t ecef_x; + int32_t ecef_y; + int32_t ecef_z; + uint32_t position_accuracy_3d; + int32_t ecef_x_velocity; + int32_t ecef_y_velocity; + int32_t ecef_z_velocity; + uint32_t speed_accuracy; + uint16_t position_DOP; + uint8_t res; + uint8_t satellites; + uint32_t res2; +} ubx_nav_solution; + +typedef struct { + uint32_t time; // GPS msToW + int32_t ned_north; + int32_t ned_east; + int32_t ned_down; + uint32_t speed_3d; + uint32_t speed_2d; + int32_t heading_2d; + uint32_t speed_accuracy; + uint32_t heading_accuracy; +} ubx_nav_velned; + +typedef struct { + uint8_t chn; // Channel number, 255 for SVx not assigned to channel + uint8_t svid; // Satellite ID + uint8_t flags; // Bitmask + uint8_t quality; // Bitfield + uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55. + uint8_t elev; // Elevation in integer degrees + int16_t azim; // Azimuth in integer degrees + int32_t prRes; // Pseudo range residual in centimetres +} ubx_nav_svinfo_channel; + +typedef struct { + uint32_t time; // GPS Millisecond time of week + uint8_t numCh; // Number of channels + uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6 + uint16_t reserved2; // Reserved + ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte +} ubx_nav_svinfo; + +typedef struct { + uint32_t time; // GPS msToW + uint32_t tAcc; + int32_t nano; + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; +} ubx_nav_timeutc; + +typedef struct { + uint32_t time; // GPS msToW + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; + uint32_t tAcc; + int32_t nano; + uint8_t fix_type; + uint8_t fix_status; + uint8_t reserved1; + uint8_t satellites; + int32_t longitude; + int32_t latitude; + int32_t altitude_ellipsoid; + int32_t altitude_msl; + uint32_t horizontal_accuracy; + uint32_t vertical_accuracy; + int32_t ned_north; + int32_t ned_east; + int32_t ned_down; + int32_t speed_2d; + int32_t heading_2d; + uint32_t speed_accuracy; + uint32_t heading_accuracy; + uint16_t position_DOP; + uint16_t reserved2; + uint16_t reserved3; +} ubx_nav_pvt; + +#define UBX_MON_GNSS_GPS_MASK (1 << 0) +#define UBX_MON_GNSS_GLONASS_MASK (1 << 1) +#define UBX_MON_GNSS_BEIDOU_MASK (1 << 2) +#define UBX_MON_GNSS_GALILEO_MASK (1 << 3) + +typedef struct { + uint8_t version; + uint8_t supported; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo + uint8_t defaultGnss; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo + uint8_t enabled; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo + uint8_t maxConcurrent; + uint8_t reserverd1; + uint8_t reserverd2; + uint8_t reserverd3; +} ubx_mon_gnss; + +typedef struct { + uint8_t msg_class; + uint8_t msg; +} ubx_ack_ack; + +typedef enum { + UBX_ACK_WAITING = 0, + UBX_ACK_GOT_ACK = 1, + UBX_ACK_GOT_NAK = 2 +} ubx_ack_state_t; + +typedef enum { + PREAMBLE1 = 0xB5, + PREAMBLE2 = 0x62, + CLASS_NAV = 0x01, + CLASS_ACK = 0x05, + CLASS_CFG = 0x06, + CLASS_MON = 0x0A, + MSG_CLASS_UBX = 0x01, + MSG_CLASS_NMEA = 0xF0, + MSG_VER = 0x04, + MSG_ACK_NACK = 0x00, + MSG_ACK_ACK = 0x01, + MSG_NMEA_GGA = 0x0, + MSG_NMEA_GLL = 0x1, + MSG_NMEA_GSA = 0x2, + MSG_NMEA_GSV = 0x3, + MSG_NMEA_RMC = 0x4, + MSG_NMEA_VGS = 0x5, + MSG_POSLLH = 0x2, + MSG_STATUS = 0x3, + MSG_SOL = 0x6, + MSG_PVT = 0x7, + MSG_VELNED = 0x12, + MSG_TIMEUTC = 0x21, + MSG_SVINFO = 0x30, + MSG_NAV_SAT = 0x35, + MSG_NAV_SIG = 0x35, + MSG_CFG_PRT = 0x00, + MSG_CFG_RATE = 0x08, + MSG_CFG_SET_RATE = 0x01, + MSG_CFG_NAV_SETTINGS = 0x24, + MSG_CFG_SBAS = 0x16, + MSG_CFG_GNSS = 0x3e, + MSG_MON_GNSS = 0x28 +} ubx_protocol_bytes_t; + +typedef enum { + FIX_NONE = 0, + FIX_DEAD_RECKONING = 1, + FIX_2D = 2, + FIX_3D = 3, + FIX_GPS_DEAD_RECKONING = 4, + FIX_TIME = 5 +} ubs_nav_fix_type_t; + +typedef enum { + NAV_STATUS_FIX_VALID = 1 +} ubx_nav_status_bits_t; + +uint8_t gpsUbloxMaxGnss(void); +timeMs_t gpsUbloxCapLastUpdate(void); + +bool gpsUbloxHasGalileo(void); +bool gpsUbloxHasBeidou(void); +bool gpsUbloxHasGlonass(void); + +bool gpsUbloxGalileoDefault(void); +bool gpsUbloxBeidouDefault(void); +bool gpsUbloxGlonassDefault(void); + +bool gpsUbloxGalileoEnabled(void); +bool gpsUbloxBeidouEnabled(void); +bool gpsUbloxGlonassEnabled(void); + + +#ifdef __cplusplus +} +#endif diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c new file mode 100644 index 0000000000..97c5bf5cd9 --- /dev/null +++ b/src/main/io/gps_ublox_utils.c @@ -0,0 +1,62 @@ +/* + * This file is part of INAV + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + + +#include + +#include "gps_ublox_utils.h" + +void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) +{ + *ck_a = *ck_b = 0; + while (len--) { + *ck_a += *data; + *ck_b += *ck_a; + data++; + } +} + +int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count) +{ + if (count > MAX_CONFIG_SET_VAL_VALUES) + count = MAX_CONFIG_SET_VAL_VALUES; + + cfg->header.preamble1 = 0xb5; + cfg->header.preamble2 = 0x62; + cfg->header.msg_class = 0x06; + cfg->header.msg_id = 0x8A; + cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data8_payload_t) * count)); + cfg->configHeader.layers = 0x1; + cfg->configHeader.transaction = 0; + cfg->configHeader.reserved = 0; + cfg->configHeader.version = 1; + + + for (int i = 0; i < count; ++i) { + cfg->data.payload[i].key = kvPairs[i].key; //htonl(kvPairs[i].key); + cfg->data.payload[i].value = kvPairs[i].value; + } + + uint8_t *buf = (uint8_t *)cfg; + uint8_t ck_a, ck_b; + ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b); + buf[cfg->header.length + 6] = ck_a; + buf[cfg->header.length + 7] = ck_b; + + return count; +} + diff --git a/src/main/io/gps_ublox_utils.h b/src/main/io/gps_ublox_utils.h new file mode 100644 index 0000000000..e7ae0caac9 --- /dev/null +++ b/src/main/io/gps_ublox_utils.h @@ -0,0 +1,34 @@ +/* + * This file is part of INAV + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#include + +#include "gps_ublox.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count); + +void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b); + +#ifdef __cplusplus +} +#endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 664bdb0ac4..518cdc61aa 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -207,11 +207,11 @@ static bool osdDisplayHasCanvas; PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); -void osdStartedSaveProcess() { +void osdStartedSaveProcess(void) { savingSettings = true; } -void osdShowEEPROMSavedNotification() { +void osdShowEEPROMSavedNotification(void) { savingSettings = false; notify_settings_saved = millis() + 5000; } @@ -329,7 +329,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) buff[sym_index + 1] = '\0'; break; case OSD_UNIT_GA: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_NAUTICALMILE, decimals, 3, digits)) { + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) { buff[sym_index] = symbol_nm; } else { buff[sym_index] = symbol_ft; @@ -532,6 +532,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ void osdFormatAltitudeSymbol(char *buff, int32_t alt) { + uint8_t totalDigits = 4U; uint8_t digits = 4U; uint8_t symbolIndex = 4U; uint8_t symbolKFt = SYM_ALT_KFT; @@ -543,6 +544,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values if (isBfCompatibleVideoSystem(osdConfig())) { + totalDigits++; digits++; symbolIndex++; symbolKFt = SYM_ALT_FT; @@ -555,7 +557,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) case OSD_UNIT_GA: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + 4 - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { + if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { // Scaled to kft buff[symbolIndex++] = symbolKFt; } else { @@ -568,7 +570,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) FALLTHROUGH; case OSD_UNIT_METRIC: // alt is alredy in cm - if (osdFormatCentiNumber(buff + 4 - digits, alt, 1000, 0, 2, digits)) { + if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) { // Scaled to km buff[symbolIndex++] = SYM_ALT_KM; } else { @@ -1092,7 +1094,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) * Check if this OSD layout is using scaled or unscaled throttle. * If both are used, it will default to scaled. */ -bool osdUsingScaledThrottle() +bool osdUsingScaledThrottle(void) { bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]); bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]); @@ -1209,7 +1211,7 @@ uint16_t osdGetRemainingGlideTime(void) { value = 0; } - return (uint16_t)round(value); + return (uint16_t)roundf(value); } static bool osdIsHeadingValid(void) @@ -1434,7 +1436,7 @@ static void osdDisplayTelemetry(void) trk_bearing %= 360; int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); float at = atan2(alt, GPS_distanceToHome); - trk_elevation = (float)at * 57.2957795; // 57.2957795 = 1 rad + trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 if (trk_elevation < 0) { trk_elevation = 0; @@ -1751,7 +1753,7 @@ static bool osdDrawSingleElement(uint8_t item) else if (!batteryWasFullWhenPluggedIn()) tfp_sprintf(buff, " NF"); else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) - tfp_sprintf(buff, "%4lu", getBatteryRemainingCapacity()); + tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity()); else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); @@ -3097,9 +3099,9 @@ static bool osdDrawSingleElement(uint8_t item) buff, "[%u]=%8ld [%u]=%8ld", bufferIndex, - constrain(debug[bufferIndex], -9999999, 99999999), + (long)constrain(debug[bufferIndex], -9999999, 99999999), bufferIndex+1, - constrain(debug[bufferIndex+1], -9999999, 99999999) + (long)constrain(debug[bufferIndex+1], -9999999, 99999999) ); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); } @@ -4239,12 +4241,19 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) bool efficiencyValid = totalDistance >= 10000; if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); + uint8_t digits = 3U; // Total number of digits (including decimal point) + #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so no switch to scaled decimal occurs above 99 + digits = 4U; + } + #endif switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, 3); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); } else { @@ -4257,7 +4266,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, 3); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4266,7 +4275,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) break; case OSD_UNIT_GA: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, 3); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); } else { @@ -4279,7 +4288,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, 3); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4290,7 +4299,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) FALLTHROUGH; case OSD_UNIT_METRIC: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); } else { @@ -4303,7 +4312,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, 3); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index d9ac344c7b..d5c84aa88c 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -279,7 +279,7 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA itoa(absLevel, buf, 10); int pos = level * pixelsPerDegreeLevel; int charY = 9 - pos * 2; - int cx = (absLevel >= 100 ? -1.5f : -1.0) * canvas->gridElementWidth; + int cx = (absLevel >= 100 ? -1.5f : -1.0f) * canvas->gridElementWidth; int px = cx + (pitchOffset + pos) * sx * 2; int py = -charY - (pitchOffset + pos) * (1 - sy) * 2; displayCanvasDrawString(canvas, px, py, buf, 0); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index c6b0b85e2c..b8ddd058cb 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -721,7 +721,7 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) thr = rcCommand[THROTTLE]; } - tfp_sprintf(buff, "%3ld%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR"); + tfp_sprintf(buff, "%3ld%s", (unsigned long)((constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)), "%THR"); } /** diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index ee352cde78..2e209fd282 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -265,9 +265,9 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu case OSD_UNIT_GA: { if (poiType == 1) { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_NAUTICALMILE, 0, 4, 4); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4); } else { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_NAUTICALMILE, 0, 3, 3); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3); } } break; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 9cd0e7e1d7..7766679106 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -59,6 +59,8 @@ typedef enum { FUNCTION_MSP_OSD = (1 << 25), // 33554432 } serialPortFunction_e; +#define FUNCTION_VTX_MSP FUNCTION_MSP_OSD + typedef enum { BAUD_AUTO = 0, BAUD_1200, diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 02fc8010d0..58651444f9 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -18,8 +18,10 @@ // Get target build configuration #include "platform.h" +#include "build/debug.h" #include "common/maths.h" +#include "common/log.h" #include "config/config_eeprom.h" #include "config/parameter_group.h" @@ -37,7 +39,6 @@ #include "io/osd.h" #include "io/vtx_control.h" - #if defined(USE_VTX_CONTROL) PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 4); diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c new file mode 100644 index 0000000000..696918e570 --- /dev/null +++ b/src/main/io/vtx_msp.c @@ -0,0 +1,701 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* Created by phobos- */ + +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) + +#include "build/debug.h" + +//#include "cms/cms_menu_vtx_msp.h" +#include "common/crc.h" +#include "common/log.h" +#include "config/feature.h" + +#include "drivers/vtx_common.h" +//#include "drivers/vtx_table.h" + +#include "fc/runtime_config.h" +#include "flight/failsafe.h" + +#include "io/serial.h" +#include "io/vtx_msp.h" +#include "io/vtx_control.h" +#include "io/vtx_string.h" +#include "io/vtx_smartaudio.h" +#include "io/vtx.h" +#include "io/displayport_msp_osd.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" +#include "msp/msp.h" + +//#include "pg/vtx_table.h" +#include "fc/settings.h" + +#include "rx/crsf.h" +//#include "rx/crsf_protocol.h" +#include "rx/rx.h" + +#include "telemetry/msp_shared.h" + +//static uint16_t mspConfFreq = 0; +static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT; +static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT; +//static uint16_t mspConfPower = 0; +static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT; +static uint8_t mspConfPitMode = 0; +static bool mspVtxConfigChanged = false; +static timeUs_t mspVtxLastTimeUs = 0; +static bool prevLowPowerDisarmedState = false; + +static const vtxVTable_t mspVTable; // forward +static vtxDevice_t vtxMsp = { + .vTable = &mspVTable, + .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS, + .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS, + .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS, + .capability.bandNames = (char **)vtx58BandNames, + .capability.channelNames = (char **)vtx58ChannelNames, + .capability.powerNames = (char**)saPowerNames + +}; + +STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE; +static uint8_t mspVtxPortIdentifier = 255; + +#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms + +static bool isCrsfPortConfig(const serialPortConfig_t *portConfig) +{ + return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF; +} + +static bool isLowPowerDisarmed(void) +{ + return (!ARMING_FLAG(ARMED) && !failsafeIsActive() && + (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS || + (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))); +} + +bool isVtxConfigValid(const vtxConfig_t *cfg) +{ + LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n"); + for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) { + + if (cfg->vtxChannelActivationConditions[i].band || + (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) || + cfg->vtxChannelActivationConditions[i].auxChannelIndex || + cfg->vtxChannelActivationConditions[i].channel) { + return true; + } + } + + LOG_DEBUG(VTX, "msp Invalid Config!\r\n"); + return false; +} + + +void setMspVtxDeviceStatusReady(const int descriptor) +{ + LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n"); + UNUSED(descriptor); + + mspVtxStatus = MSP_VTX_STATUS_READY; + mspVtxConfigChanged = true; +} + + +void prepareMspFrame(uint8_t *mspFrame) +{ + LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n"); +/* +HDZERO parsing + fc_band_rx = msp_rx_buf[1]; + fc_channel_rx = msp_rx_buf[2]; + fc_pwr_rx = msp_rx_buf[3]; + fc_pit_rx = msp_rx_buf[4]; + fc_lp_rx = msp_rx_buf[8]; +*/ + + uint8_t pitmode = 0; + vtxCommonGetPitMode(&vtxMsp, &pitmode); + + mspFrame[0] = VTXDEV_MSP, + mspFrame[1] = vtxSettingsConfig()->band; + mspFrame[2] = vtxSettingsConfig()->channel; + mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based + mspFrame[4] = pitmode; + mspFrame[5] = 0; // Freq_L + mspFrame[6] = 0; // Freq_H + mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0; + mspFrame[8] = isLowPowerDisarmed(); + mspFrame[9] = 0; // Pitmode freq Low + mspFrame[10] = 0; // pitmod freq High + mspFrame[11] = 0; // 1 if using vtx table + mspFrame[12] = 0; // vtx table bands or 0 + mspFrame[13] = 0; // vtx table channels or 0 + mspFrame[14] = 0; // vtx table power levels or 0 +} + +static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) +{ + + LOG_DEBUG(VTX, "msp CrsfPush\r\n"); +#ifndef USE_TELEMETRY_CRSF + UNUSED(mspCommand); + UNUSED(mspFrame); + UNUSED(mspFrameSize); +#else + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + + uint8_t mspHeader[6] = {0x50, 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 over CRSF header + uint8_t mspChecksum; + + mspChecksum = crc8_dvb_s2_update(0, &mspHeader[1], 5); // first character is not checksummable + mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize); + + uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1; // add 1 for msp checksum + uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize; + + uint8_t crsfFrame[crsfFrameSize]; + + dst->ptr = crsfFrame; + dst->end = ARRAYEND(crsfFrame); + + sbufWriteU8(dst, CRSF_SYNC_BYTE); + sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself) + sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type + sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection + sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device + sbufWriteData(dst, mspHeader, sizeof(mspHeader)); + sbufWriteData(dst, mspFrame, mspFrameSize); + sbufWriteU8(dst, mspChecksum); + crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length + sbufSwitchToReader(dst, crsfFrame); + + crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry + crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); + crsfRxSendTelemetryData(); +#endif +} + +static uint16_t packetCounter = 0; + +static bool isVtxConfigChanged(void) +{ + if(mspVtxStatus == MSP_VTX_STATUS_READY) { + if (mspVtxConfigChanged == true) + return true; + + if (isLowPowerDisarmed() != prevLowPowerDisarmedState) { + LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n"); + mspVtxConfigChanged = true; + prevLowPowerDisarmedState = isLowPowerDisarmed(); + } + + if (mspConfPowerIndex != vtxSettingsConfig()->power) { + LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n"); + mspVtxConfigChanged = true; + } + + if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) { + LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n"); + mspVtxConfigChanged = true; + } + + return mspVtxConfigChanged; + } + + return false; +} + +static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + UNUSED(vtxDevice); + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); + uint8_t frame[15]; + + switch (mspVtxStatus) { + case MSP_VTX_STATUS_OFFLINE: + LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n"); + // wait for MSP communication from the VTX +#ifdef USE_CMS + //mspCmsUpdateStatusString(); +#endif + break; + case MSP_VTX_STATUS_READY: + LOG_DEBUG(VTX, "msp MspProcess: READY\r\n"); + // send an update if stuff has changed with 200ms period + if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) { + + LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n"); + prepareMspFrame(frame); + + if (isCrsfPortConfig(portConfig)) { + mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame)); + } else { + mspPort_t *port = getMspOsdPort(); + if(port != NULL && port->port) { + LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n"); + int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE); + if (sent <= 0) { + break; + } + } + } + packetCounter++; + mspVtxLastTimeUs = currentTimeUs; + mspVtxConfigChanged = false; + +#ifdef USE_CMS + //mspCmsUpdateStatusString(); +#endif + } + break; + default: + mspVtxStatus = MSP_VTX_STATUS_OFFLINE; + break; + } + +#if 0 + DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter); + DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig)); + DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed()); +#if defined(USE_MSP_OVER_TELEMETRY) + DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier)); +#else + DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier)); +#endif +#endif +} + +static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) +{ + //LOG_DEBUG(VTX, "msp GetDeviceType\r\n"); + UNUSED(vtxDevice); + return VTXDEV_MSP; +} + +static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) +{ + //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N"); + return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY; +} + +static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) +{ + LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n"); + UNUSED(vtxDevice); + if (band != mspConfBand || channel != mspConfChannel) { + LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n"); + mspVtxConfigChanged = true; + } + mspConfBand = band; + mspConfChannel = channel; +} + +static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) +{ + LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n"); + UNUSED(vtxDevice); + + if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS)) + { + if (index != mspConfPowerIndex) + { + LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n"); + mspVtxConfigChanged = true; + } + mspConfPowerIndex = index; + } +} + +static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +{ + LOG_DEBUG(VTX, "msp SetPitMode\r\n"); + UNUSED(vtxDevice); + if (onoff != mspConfPitMode) { + LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n"); + mspVtxConfigChanged = true; + } + mspConfPitMode = onoff; +} + +#if 0 +static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) +{ + UNUSED(vtxDevice); + if (freq != mspConfFreq) { + mspVtxConfigChanged = true; + } + mspConfFreq = freq; +} +#endif + + + + +static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + *pBand = vtxSettingsConfig()->band; + *pChannel = vtxSettingsConfig()->channel; + + //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + + return true; +} + +static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; + // Special case, power not set + if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) { + *pIndex = 0; + //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); + return true; + } + + *pIndex = power; + + //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); + return true; +} + +static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + LOG_DEBUG(VTX, "msp GetFreq\r\n"); + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + *pFreq = 5800; + return true; +} + +static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) +{ + LOG_DEBUG(VTX, "msp GetPower\r\n"); + uint8_t powerIndex; + + if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) { + return false; + } + + + *pIndex = powerIndex; + *pPowerMw = *pIndex; + return true; +} + +static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo) +{ + LOG_DEBUG(VTX, "msp GetOsdInfo\r\n"); + uint8_t powerIndex; + uint16_t powerMw; + uint16_t freq; + uint8_t band, channel; + + if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) { + return false; + } + + if (!vtxMspGetFreq(vtxDevice, &freq)) { + return false; + } + + if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) { + return false; + } + + pOsdInfo->band = band; + pOsdInfo->channel = channel; + pOsdInfo->frequency = freq; + pOsdInfo->powerIndex = powerIndex; + pOsdInfo->powerMilliwatt = powerMw; + pOsdInfo->bandLetter = vtx58BandNames[band][0]; + pOsdInfo->bandName = vtx58BandNames[band]; + pOsdInfo->channelName = vtx58ChannelNames[channel]; + pOsdInfo->powerIndexLetter = '0' + powerIndex; + return true; +} + + +bool vtxMspInit(void) +{ + LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); + // don't bother setting up this device if we don't have MSP vtx enabled + // Port is shared with MSP_OSD + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); + if (!portConfig) { + return false; + } + + mspVtxPortIdentifier = portConfig->identifier; + + // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called. +#if defined(USE_VTX_COMMON) + vtxCommonSetDevice(&vtxMsp); +#endif + + mspConfBand = vtxSettingsConfig()->band; + mspConfChannel = vtxSettingsConfig()->channel; + mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based + vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode); + + vtxInit(); + + return true; +} + +static const vtxVTable_t mspVTable = { + .process = vtxMspProcess, + .getDeviceType = vtxMspGetDeviceType, + .isReady = vtxMspIsReady, + .setBandAndChannel = vtxMspSetBandAndChannel, + .setPowerByIndex = vtxMspSetPowerByIndex, + .setPitMode = vtxMspSetPitMode, + //.setFrequency = vtxMspSetFreq, + .getBandAndChannel = vtxMspGetBandAndChannel, + .getPowerIndex = vtxMspGetPowerIndex, + .getFrequency = vtxMspGetFreq, + //.getStatus = vtxMspGetStatus, + .getPower = vtxMspGetPower, + //.serializeCustomDeviceStatus = NULL, + .getOsdInfo = vtxMspGetOsdInfo, +}; + +static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n"); + UNUSED(mspPostProcessFn); + + sbuf_t *dst = &reply->buf; + sbuf_t *src = &cmd->buf; + + const unsigned int dataSize = sbufBytesRemaining(src); + UNUSED(dst); + UNUSED(src); + + // Start initializing the reply message + reply->cmd = cmd->cmd; + reply->result = MSP_RESULT_ACK; + + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) { + LOG_DEBUG(VTX, "msp wrong vtx\r\n"); + return MSP_RESULT_ERROR; + } + + switch (cmd->cmd) + { + case MSP_VTXTABLE_BAND: + { + LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n"); + uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice); + if (deviceType == VTXDEV_MSP) + { + /* + char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1]; + memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1); + uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS]; + const uint8_t band = sbufReadU8(src); + const uint8_t bandNameLength = sbufReadU8(src); + for (int i = 0; i < bandNameLength; i++) { + const char nameChar = sbufReadU8(src); + if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) { + bandName[i] = toupper(nameChar); + } + } + const char bandLetter = toupper(sbufReadU8(src)); + const bool isFactoryBand = (bool)sbufReadU8(src); + const uint8_t channelCount = sbufReadU8(src); + for (int i = 0; i < channelCount; i++) + { + const uint16_t frequency = sbufReadU16(src); + if (i < vtxTableConfig()->channels) + { + frequencies[i] = frequency; + } + } + */ + + setMspVtxDeviceStatusReady(1); + } + break; + } + case MSP_VTXTABLE_POWERLEVEL: + { + LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n"); + + /* + char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1]; + memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1); + const uint8_t powerLevel = sbufReadU8(src); + const uint16_t powerValue = sbufReadU16(src); + const uint8_t powerLevelLabelLength = sbufReadU8(src); + for (int i = 0; i < powerLevelLabelLength; i++) + { + const char labelChar = sbufReadU8(src); + if (i < VTX_TABLE_POWER_LABEL_LENGTH) + { + powerLevelLabel[i] = toupper(labelChar); + } + } + */ + setMspVtxDeviceStatusReady(1); + } + break; + case MSP_VTX_CONFIG: + { + LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n"); + LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n"); + uint8_t pitmode = 0; + vtxCommonGetPitMode(vtxDevice, &pitmode); + + // VTXDEV_MSP, + sbufWriteU8(dst, VTXDEV_MSP); + // band; + sbufWriteU8(dst, vtxSettingsConfig()->band); + // channel; + sbufWriteU8(dst, vtxSettingsConfig()->channel); + // power; // index based + sbufWriteU8(dst, vtxSettingsConfig()->power); + // pit mode; + // Freq_L + sbufWriteU8(dst, 0); + // Freq_H + sbufWriteU8(dst, 0); + // vtx status + sbufWriteU8(dst, 1); + // lowPowerDisarm + + sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); + // Pitmode freq Low + sbufWriteU8(dst, 0); + // pitmod freq High + sbufWriteU8(dst, 0); + // 1 if using vtx table + sbufWriteU8(dst, 0); + // vtx table bands or 0 + sbufWriteU8(dst, 0); + // vtx table channels or 0 + sbufWriteU8(dst, 0); + + setMspVtxDeviceStatusReady(1); + break; + } + case MSP_SET_VTX_CONFIG: + LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n"); + if (dataSize == 15) + { + if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) + { + for (int i = 0; i < 15; ++i) + { + uint8_t data = sbufReadU8(src); + switch (i) + { + case 1: + vtxSettingsConfigMutable()->band = data; + break; + case 2: + vtxSettingsConfigMutable()->channel = data; + break; + case 3: + vtxSettingsConfigMutable()->power = data; + break; + case 4: + vtxCommonSetPitMode(vtxDevice, data); + break; + case 7: + // vtx ready + break; + case 8: + vtxSettingsConfigMutable()->lowPowerDisarm = data; + break; + } + } + } + + setMspVtxDeviceStatusReady(1); + break; + } + LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n"); + return MSP_RESULT_ERROR; + default: + // debug[1]++; + // debug[2] = cmd->cmd; + if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) { + LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd); + } + reply->result = MSP_RESULT_ERROR; + break; + } + + // Process DONT_REPLY flag + if (cmd->flags & MSP_FLAG_DONT_REPLY) + { + reply->result = MSP_RESULT_NO_REPLY; + } + + return reply->result; +} + +void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) +{ + UNUSED(mspProcessCommandFn); + // Check if VTX is ready + /* + if (mspVtxStatus != MSP_VTX_STATUS_READY) { + LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n"); + return; + } + */ + + mspPort_t *port = getMspOsdPort(); + + if(port) { + mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand); + } + +} + + +#endif \ No newline at end of file diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h new file mode 100644 index 0000000000..30ca245fed --- /dev/null +++ b/src/main/io/vtx_msp.h @@ -0,0 +1,53 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#pragma once + +#include + +#include "build/build_config.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +typedef enum { + // Offline - device hasn't responded yet + MSP_VTX_STATUS_OFFLINE = 0, + MSP_VTX_STATUS_READY, +} mspVtxStatus_e; + +typedef struct mspPowerTable_s { + int mW; // rfpower + int16_t dbi; // valueV1 +} mspPowerTable_t; + +#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands +#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels +#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT +#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1 +#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8 +#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3 + + +bool vtxMspInit(void); +void setMspVtxDeviceStatusReady(const int descriptor); +void prepareMspFrame(uint8_t *mspFrame); + +void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index f94b04fe1e..b44037f2f8 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -217,6 +217,9 @@ #define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility #define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility +#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data - needed by msp vtx +#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data - neede by msp vtx + // // OSD specific // diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 5fa10e819e..0aaecba2d7 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -530,7 +530,7 @@ int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t return mspSerialEncode(mspPort, &push, version); } -int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) +int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version) { int ret = 0; @@ -545,11 +545,16 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) continue; } - ret = mspSerialPushPort(cmd, data, datalen, mspPort, MSP_V1); + ret = mspSerialPushPort(cmd, data, datalen, mspPort, version); } return ret; // return the number of bytes written } +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) +{ + return mspSerialPushVersion(cmd, data, datalen, MSP_V1); +} + uint32_t mspSerialTxBytesFree(serialPort_t *port) { return serialTxBytesFree(port); diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 8132f077a0..67487606da 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -107,5 +107,6 @@ void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version); int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen); +int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version); uint32_t mspSerialTxBytesFree(serialPort_t *port); mspPort_t * mspSerialPortFind(const struct serialPort_s *serialPort); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f5223605ce..47d644b70b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -101,7 +101,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -125,6 +125,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled .rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT, // RTH trackback useage mode .rth_use_linear_descent = SETTING_NAV_RTH_USE_LINEAR_DESCENT_DEFAULT, // Use linear descent during RTH + .landing_bump_detection = SETTING_NAV_LANDING_BUMP_DETECTION_DEFAULT, // Detect landing based on touchdown G bump }, // General navigation parameters @@ -1247,7 +1248,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n } const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT; - const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); + const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0f) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) { @@ -1722,6 +1723,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi timeMs_t currentTime = millis(); if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 || + posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || (posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) { return NAV_FSM_EVENT_SUCCESS; } @@ -1877,7 +1879,7 @@ static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) { const timeMs_t currentMillis = millis(); - navigationFSMState_t previousState; + navigationFSMState_t previousState = NAV_STATE_UNDEFINED; static timeMs_t lastStateProcessTime = 0; /* Process new injected event if event defined, @@ -2635,7 +2637,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) * transiton in to turn. * Limited to fixed wing only. * --------------------------------------------------- */ - bool rthClimbStageActiveAndComplete() { + bool rthClimbStageActiveAndComplete(void) { if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) { if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) { return true; @@ -3229,7 +3231,7 @@ void loadSelectedMultiMission(uint8_t missionIndex) posControl.geoWaypointCount = 0; for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { - if ((missionCount == missionIndex)) { + if (missionCount == missionIndex) { /* store details of selected mission: start wp index, mission wp count, geo wp count */ if (!(posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD || @@ -4397,7 +4399,7 @@ int32_t navigationGetHomeHeading(void) } // returns m/s -float calculateAverageSpeed() { +float calculateAverageSpeed(void) { float flightTime = getFlightTime(); if (flightTime == 0.0f) return 0; return (float)getTotalTravelDistance() / (flightTime * 100); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5533e4a4df..5b1abf0f88 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -237,6 +237,7 @@ typedef struct navConfig_s { uint8_t waypoint_mission_restart; // Waypoint mission restart action uint8_t rth_trackback_mode; // Useage mode setting for RTH trackback uint8_t rth_use_linear_descent; // Use linear descent in the RTH head home leg + uint8_t landing_bump_detection; // Allow landing detection based on G bump at touchdown } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) @@ -623,6 +624,8 @@ bool isAdjustingHeading(void); float getEstimatedAglPosition(void); bool isEstimatedAglTrusted(void); +float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue); + /* Returns the heading recorded when home position was acquired. * Note that the navigation system uses deg*100 as unit and angles * are in the [0, 360 * 100) interval. diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 5e40fd56e7..51aa03b5d4 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -670,7 +670,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat #endif } -bool isFixedWingAutoThrottleManuallyIncreased() +bool isFixedWingAutoThrottleManuallyIncreased(void) { return isAutoThrottleManuallyIncreased; } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2c5eaf4a34..864cde8b33 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -720,10 +720,56 @@ bool isMulticopterFlying(void) /*----------------------------------------------------------- * Multicopter land detector *-----------------------------------------------------------*/ + #if defined(USE_BARO) +float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue) +{ + static float baroAltRate; + if (updateValue) { + baroAltRate = newBaroAltRate; + } + + return baroAltRate; +} + +static bool isLandingGbumpDetected(timeMs_t currentTimeMs) +{ + /* Detection based on G bump at touchdown, falling Baro altitude and throttle below hover. + * G bump trigger: > 2g then falling back below 1g in < 0.1s. + * Baro trigger: rate must be -ve at initial trigger g and < -2 m/s when g falls back below 1g + * Throttle trigger: must be below hover throttle with lower threshold for manual throttle control */ + + static timeMs_t gSpikeDetectTimeMs = 0; + float baroAltRate = updateBaroAltitudeRate(0, false); + + if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) { + gSpikeDetectTimeMs = currentTimeMs; + } else if (gSpikeDetectTimeMs) { + if (currentTimeMs < gSpikeDetectTimeMs + 100) { + if (acc.accADCf[Z] < 1.0f && baroAltRate < -200.0f) { + const uint16_t idleThrottle = getThrottleIdleValue(); + const uint16_t hoverThrottleRange = currentBatteryProfile->nav.mc.hover_throttle - idleThrottle; + return rcCommand[THROTTLE] < idleThrottle + ((navigationInAutomaticThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + } + } else if (acc.accADCf[Z] <= 1.0f) { + gSpikeDetectTimeMs = 0; + } + } + + return false; +} +#endif bool isMulticopterLandingDetected(void) { DEBUG_SET(DEBUG_LANDING, 4, 0); - static timeMs_t landingDetectorStartedAt; + DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100); + + const timeMs_t currentTimeMs = millis(); + +#if defined(USE_BARO) + if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && isLandingGbumpDetected(currentTimeMs)) { + return true; // Landing flagged immediately if landing bump detected + } +#endif bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue())); @@ -735,6 +781,8 @@ bool isMulticopterLandingDetected(void) || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && throttleIsBelowMidHover) || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow()); + static timeMs_t landingDetectorStartedAt; + if (!startCondition || posControl.flags.resetLandingDetector) { landingDetectorStartedAt = 0; return posControl.flags.resetLandingDetector = false; @@ -751,7 +799,6 @@ bool isMulticopterLandingDetected(void) DEBUG_SET(DEBUG_LANDING, 3, gyroCondition); bool possibleLandingDetected = false; - const timeMs_t currentTimeMs = millis(); if (navGetCurrentStateFlags() & NAV_CTL_LAND) { // We have likely landed if throttle is 40 units below average descend throttle diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 8487c234ca..9fed0f6978 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -331,12 +331,17 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs)); + + // baro altitude rate + static float baroAltPrevious = 0; + posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / US2S(baroDtUs); + baroAltPrevious = posEstimator.baro.alt; + updateBaroAltitudeRate(posEstimator.baro.baroAltRate, true); } } else { posEstimator.baro.alt = 0; posEstimator.baro.lastUpdateTime = 0; - posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv; } } #endif @@ -564,8 +569,15 @@ static void estimationPredict(estimationContext_t * ctx) } static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) -{ - bool correctionCalculated = false; +{ + DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate + DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude + DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude + DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level + DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate + DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame + DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed + DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count if (ctx->newFlags & EST_BARO_VALID) { timeUs_t currentTimeUs = micros(); @@ -611,12 +623,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } - correctionCalculated = true; - } else { - pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f); + return true; } - - if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) { + else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) { // If baro is not available - use GPS Z for correction on a plane // Reset current estimate to GPS altitude if estimate not valid if (!(ctx->newFlags & EST_Z_VALID)) { @@ -637,20 +646,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); } - correctionCalculated = true; + return true; } - // DEBUG_ALTITUDE will be always available - DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate - DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude - DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude - DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level - DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate - DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame - DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed - DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count - - return correctionCalculated; + return false; } static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) @@ -837,7 +836,6 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) navEPH = posEstimator.est.eph; navEPV = posEstimator.est.epv; - DEBUG_SET(DEBUG_POS_EST, 0, (int32_t) posEstimator.est.pos.x*1000.0F); // Position estimate X DEBUG_SET(DEBUG_POS_EST, 1, (int32_t) posEstimator.est.pos.y*1000.0F); // Position estimate Y if (IS_RC_MODE_ACTIVE(BOXSURFACE) && posEstimator.est.aglQual!=SURFACE_QUAL_LOW){ diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index dd171fa8e6..40129f844b 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -81,6 +81,7 @@ typedef struct { pt1Filter_t avgFilter; float alt; // Raw barometric altitude (cm) float epv; + float baroAltRate; // Baro altitude rate of change (cm/s) } navPositionEstimatorBARO_t; typedef struct { diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index d88fa6fee9..9857038765 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -69,8 +69,6 @@ typedef struct sbusFrameData_s { // Receive ISR callback static void sbusDataReceive(uint16_t c, void *data) { - static uint16_t sbusDesyncCounter = 0; - sbusFrameData_t *sbusFrameData = data; const timeUs_t currentTimeUs = micros(); const timeDelta_t timeSinceLastByteUs = cmpTimeUs(currentTimeUs, sbusFrameData->lastActivityTimeUs); @@ -110,7 +108,6 @@ static void sbusDataReceive(uint16_t c, void *data) default: // Failed end marker sbusFrameData->state = STATE_SBUS_WAIT_SYNC; - sbusDesyncCounter++; break; } diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c index 8a615039f3..95ab941027 100644 --- a/src/main/rx/srxl2.c +++ b/src/main/rx/srxl2.c @@ -157,7 +157,7 @@ bool srxl2ProcessHandshake(const Srxl2Header* header) /* priority */ 10, /* baudSupported*/ baudRate, /* info */ 0, - // U_ID_2 + 0 // U_ID_2 } }; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index fc917c4354..f897d7e580 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -70,7 +70,7 @@ FASTRAM acc_t acc; // acc access functions STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration; -STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT]; +STATIC_FASTRAM float accADC[XYZ_AXIS_COUNT]; STATIC_FASTRAM filter_t accFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM filterApplyFnPtr accSoftLpfFilterApplyFn; @@ -82,6 +82,9 @@ static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn; static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT]; +static EXTENDED_FASTRAM float fAccZero[XYZ_AXIS_COUNT]; +static EXTENDED_FASTRAM float fAccGain[XYZ_AXIS_COUNT]; + PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5); void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) @@ -105,6 +108,17 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) ); } +static void updateAccCoefficients(void) { + + for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) { + //Float zero + fAccZero[i] = (float)accelerometerConfig()->accZero.raw[i]; + //Float gain + fAccGain[i] = (float)accelerometerConfig()->accGain.raw[i] / 4096.0f; + } + +} + static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware = ACC_NONE; @@ -280,6 +294,7 @@ bool accInit(uint32_t targetLooptime) acc.accTargetLooptime = targetLooptime; acc.accClipCount = 0; accInitFilters(); + updateAccCoefficients(); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { acc.extremes[axis].min = 100; @@ -290,7 +305,7 @@ bool accInit(uint32_t targetLooptime) } static bool calibratedPosition[6]; -static int32_t accSamples[6][3]; +static float accSamples[6][3]; uint8_t accGetCalibrationAxisFlags(void) { @@ -309,10 +324,10 @@ uint8_t accGetCalibrationAxisFlags(void) return flags; } -static int getPrimaryAxisIndex(int32_t accADCData[3]) +static int getPrimaryAxisIndex(float accADCData[3]) { // Work on a copy so we don't mess with accADC data - int32_t sample[3]; + float sample[3]; applySensorAlignment(sample, accADCData, acc.dev.accAlign); @@ -447,7 +462,7 @@ static void performAcclerationCalibration(void) sensorCalibrationResetState(&calState); for (int axis = 0; axis < 6; axis++) { - int32_t accSample[3]; + float accSample[3]; accSample[X] = accSamples[axis][X] - accelerometerConfig()->accZero.raw[X]; accSample[Y] = accSamples[axis][Y] - accelerometerConfig()->accZero.raw[Y]; @@ -477,15 +492,20 @@ static void performAcclerationCalibration(void) // saveConfigAndNotify will trigger eepromREAD and in turn call back the accelerometer gain validation // that will set ENABLE_STATE(ACCELEROMETER_CALIBRATED) if all is good saveConfigAndNotify(); + //Recompute all coeffs + updateAccCoefficients(); } } } -static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain) +static void applyAccelerationZero(void) { - accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096; - accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096; - accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096; + float tmp[XYZ_AXIS_COUNT]; + + //Apply zero + arm_sub_f32(accADC, fAccZero, tmp, XYZ_AXIS_COUNT); + //Apply gain + arm_mult_f32(tmp, fAccGain, accADC, XYZ_AXIS_COUNT); } /* @@ -493,9 +513,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f */ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS; - } + arm_scale_f32(acc.accADCf, GRAVITY_CMSS, measuredAcc->v, XYZ_AXIS_COUNT); } /* @@ -530,7 +548,7 @@ void accUpdate(void) if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) { performAcclerationCalibration(); - applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain); + applyAccelerationZero(); } applySensorAlignment(accADC, accADC, acc.dev.accAlign); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index a521e83c83..6604ccc1c8 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -254,6 +254,12 @@ uint32_t baroUpdate(void) { static barometerState_e state = BAROMETER_NEEDS_SAMPLES; +#ifdef USE_SIMULATOR + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { + return 0; + } +#endif + switch (state) { default: case BAROMETER_NEEDS_SAMPLES: @@ -274,14 +280,8 @@ uint32_t baroUpdate(void) if (baro.dev.start_ut) { baro.dev.start_ut(&baro.dev); } -#ifdef USE_SIMULATOR - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { - //output: baro.baroPressure, baro.baroTemperature - baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature); - } -#else + //output: baro.baroPressure, baro.baroTemperature baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature); -#endif state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; break; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 5149a1a258..0afc15b5be 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -199,7 +199,7 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, void batteryInit(void) { batteryState = BATTERY_NOT_PRESENT; - batteryCellCount = 1; + batteryCellCount = 0; batteryFullVoltage = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; @@ -716,11 +716,11 @@ uint16_t getPowerSupplyImpedance(void) { } // returns cW (0.01W) -int32_t calculateAveragePower() { +int32_t calculateAveragePower(void) { return (int64_t)mWhDrawn * 360 / getFlightTime(); } // returns mWh / meter -int32_t calculateAverageEfficiency() { +int32_t calculateAverageEfficiency(void) { return getFlyingEnergy() * 100 / getTotalTravelDistance(); } diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index ffe4210f5c..86e41880f8 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -85,7 +85,7 @@ void updateBoardAlignment(int16_t roll, int16_t pitch) initBoardAlignment(); } -void applyBoardAlignment(int32_t *vec) +void applyBoardAlignment(float *vec) { if (standardBoardAlignment) { return; @@ -99,12 +99,12 @@ void applyBoardAlignment(int32_t *vec) vec[Z] = lrintf(fpVec.z); } -void FAST_CODE applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation) +void FAST_CODE applySensorAlignment(float * dest, float * src, uint8_t rotation) { // Create a copy so we could use the same buffer for src & dest - const int32_t x = src[X]; - const int32_t y = src[Y]; - const int32_t z = src[Z]; + const float x = src[X]; + const float y = src[Y]; + const float z = src[Z]; switch (rotation) { default: diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index 1d84965ed2..6bf0127265 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -29,5 +29,5 @@ PG_DECLARE(boardAlignment_t, boardAlignment); void initBoardAlignment(void); void updateBoardAlignment(int16_t roll, int16_t pitch); -void applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation); -void applyBoardAlignment(int32_t *vec); \ No newline at end of file +void applySensorAlignment(float * dest, float * src, uint8_t rotation); +void applyBoardAlignment(float *vec); \ No newline at end of file diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 27827649bb..c6dd78f7db 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -49,7 +49,7 @@ typedef enum { typedef struct mag_s { magDev_t dev; - int32_t magADC[XYZ_AXIS_COUNT]; + float magADC[XYZ_AXIS_COUNT]; } mag_t; extern mag_t mag; diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 97981df92c..cc874b6bb8 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -95,6 +95,17 @@ hardwareSensorStatus_e getHwCompassStatus(void) hardwareSensorStatus_e getHwBarometerStatus(void) { #if defined(USE_BARO) +#ifdef USE_SIMULATOR + if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { + if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) { + return HW_SENSOR_NONE; + } else if (baroIsHealthy()) { + return HW_SENSOR_OK; + } else { + return HW_SENSOR_UNHEALTHY; + } + } +#endif if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) { if (baroIsHealthy()) { return HW_SENSOR_OK; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 11e3dd2ef5..a586ce6df6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -372,7 +372,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe setGyroCalibration(dev->gyroZero); #endif - LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); + LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", (int16_t) dev->gyroZero[X], (int16_t) dev->gyroZero[Y], (int16_t) dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics } else { dev->gyroZero[X] = 0; @@ -409,21 +409,17 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC #endif if (zeroCalibrationIsCompleteV(gyroCal)) { - int32_t gyroADCtmp[XYZ_AXIS_COUNT]; + float gyroADCtmp[XYZ_AXIS_COUNT]; - // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment - gyroADCtmp[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X]; - gyroADCtmp[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y]; - gyroADCtmp[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z]; + //Apply zero calibration with CMSIS DSP + arm_sub_f32(gyroDev->gyroADCRaw, gyroDev->gyroZero, gyroADCtmp, 3); // Apply sensor alignment applySensorAlignment(gyroADCtmp, gyroADCtmp, gyroDev->gyroAlign); applyBoardAlignment(gyroADCtmp); // Convert to deg/s and store in unified data - gyroADCf[X] = (float)gyroADCtmp[X] * gyroDev->scale; - gyroADCf[Y] = (float)gyroADCtmp[Y] * gyroDev->scale; - gyroADCf[Z] = (float)gyroADCtmp[Z] * gyroDev->scale; + arm_scale_f32(gyroADCtmp, gyroDev->scale, gyroADCf, 3); return true; } else { @@ -442,7 +438,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC } } -void FAST_CODE NOINLINE gyroFilter() +void FAST_CODE NOINLINE gyroFilter(void) { if (!gyro.initialized) { return; @@ -504,7 +500,7 @@ void FAST_CODE NOINLINE gyroFilter() } -void FAST_CODE NOINLINE gyroUpdate() +void FAST_CODE NOINLINE gyroUpdate(void) { #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 66666d80e0..669f8fbee4 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -25,6 +25,11 @@ #include "drivers/sensor.h" #include "flight/dynamic_gyro_notch.h" #include "flight/secondary_dynamic_gyro_notch.h" +#if !defined(SITL_BUILD) +#include "arm_math.h" +#else +#include +#endif typedef enum { GYRO_NONE = 0, diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index 649f24a045..f82d8f16a5 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -204,7 +204,7 @@ void opflowUpdate(timeUs_t currentTimeUs) // If quality of the flow from the sensor is good - process further if (opflow.flowQuality == OPFLOW_QUALITY_VALID) { - const float integralToRateScaler = (opticalFlowConfig()->opflow_scale > 0.01f) ? (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale : 0.0f; + const float integralToRateScaler = (opticalFlowConfig()->opflow_scale > 0.01f) ? (1.0e6f / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale : 0.0f; // Apply sensor alignment applySensorAlignment(opflow.dev.rawData.flowRateRaw, opflow.dev.rawData.flowRateRaw, opticalFlowConfig()->opflow_align); diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index eb694be20d..2fae0a1692 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -255,7 +255,7 @@ STATIC_PROTOTHREAD(pitotThread) #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); - } + } #endif } diff --git a/src/main/sensors/temperature.c b/src/main/sensors/temperature.c index 5cb146cf16..3d5c05f179 100644 --- a/src/main/sensors/temperature.c +++ b/src/main/sensors/temperature.c @@ -163,7 +163,7 @@ void tempSensorAddressToString(uint64_t address, char *hex_address) tfp_sprintf(hex_address, "%d", (int)address); else { uint32_t *address32 = (uint32_t *)&address; - tfp_sprintf(hex_address, "%08lx%08lx", address32[1], address32[0]); + tfp_sprintf(hex_address, "%08lx%08lx", (unsigned long)address32[1], (unsigned long)address32[0]); } } diff --git a/src/main/target/AOCODARCF405AIO/CMakeLists.txt b/src/main/target/AOCODARCF405AIO/CMakeLists.txt new file mode 100644 index 0000000000..4c76bc974b --- /dev/null +++ b/src/main/target/AOCODARCF405AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(AOCODARCF405AIO) diff --git a/src/main/target/AOCODARCF405AIO/config.c b/src/main/target/AOCODARCF405AIO/config.c new file mode 100644 index 0000000000..aa919d8551 --- /dev/null +++ b/src/main/target/AOCODARCF405AIO/config.c @@ -0,0 +1,30 @@ +/* + * 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" +#include "sensors/boardalignment.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" + +void targetConfiguration(void) +{ + //boardAlignmentMutable()->yawDeciDegrees = 450; +} diff --git a/src/main/target/AOCODARCF405AIO/target.c b/src/main/target/AOCODARCF405AIO/target.c new file mode 100644 index 0000000000..9548cb179a --- /dev/null +++ b/src/main/target/AOCODARCF405AIO/target.c @@ -0,0 +1,41 @@ +/* + * 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/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); +timerHardware_t timerHardware[] = { + DEF_TIM(TIM2, CH2, PA10, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AOCODARCF405AIO/target.h b/src/main/target/AOCODARCF405AIO/target.h new file mode 100644 index 0000000000..859caa51f4 --- /dev/null +++ b/src/main/target/AOCODARCF405AIO/target.h @@ -0,0 +1,171 @@ +/* + * 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 "F405AIO" +#define USBD_PRODUCT_STRING "AocodaRCF405AIO" + +#define LED0 PC13 + +#define USE_BEEPER +#define BEEPER PB8 +#define BEEPER_INVERTED + +/*** UART ***/ +#define USB_IO +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PC12 +#define UART5_TX_PIN PD2 + +#define SERIAL_PORT_COUNT 6 + +/*** Gyro & Acc ***/ +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6500 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_EXTI_PIN PC5 +#define IMU_MPU6500_ALIGN CW90_DEG + +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_EXTI_PIN PC5 +#define IMU_MPU6000_ALIGN CW90_DEG + +#define USE_IMU_BMI270 +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_EXTI_PIN PC5 +#define IMU_BMI270_ALIGN CW90_DEG + +/*** I2C (Baro & Mag) ***/ +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +// Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 +#define BARO_I2C_BUS BUS_I2C1 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_MLX90393 + +/*** Onboard Flash ***/ +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB5 +#define SPI3_MOSI_PIN PB4 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PC0 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +/*** OSD ***/ +#define USE_OSD +#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 PA13 + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC3 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +/*** LED ***/ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + +/*** Default settings ***/ +#define SERIALRX_UART SERIAL_PORT_USART2 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define CURRENT_METER_SCALE 250 + +/*** Optical Flow & Lidar ***/ +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT +#define USE_ESC_SENSOR + +/*** Misc ***/ +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/AOCODARCF4V2/target.h b/src/main/target/AOCODARCF4V2/target.h index 273a6d0691..5ad27e4657 100644 --- a/src/main/target/AOCODARCF4V2/target.h +++ b/src/main/target/AOCODARCF4V2/target.h @@ -81,16 +81,16 @@ #define UART2_RX_PIN PA3 #define USE_UART3 -#define UART3_TX_PIN PC11 -#define UART3_RX_PIN PC10 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 #define USE_UART4 #define UART4_TX_PIN PA0 #define UART4_RX_PIN PA1 #define USE_UART5 -#define UART5_TX_PIN PC12 -#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PD2 +#define UART5_RX_PIN PC12 #define SERIAL_PORT_COUNT 6 diff --git a/src/main/target/AOCODARCF7MINI/target.c b/src/main/target/AOCODARCF7MINI/target.c index 26594a6751..7b34bb05df 100644 --- a/src/main/target/AOCODARCF7MINI/target.c +++ b/src/main/target/AOCODARCF7MINI/target.c @@ -1,3 +1,10 @@ +/* + * @Author: g05047 + * @Date: 2023-03-24 17:43:23 + * @LastEditors: g05047 + * @LastEditTime: 2023-05-11 15:49:30 + * @Description: file content + */ /* * This file is part of INAV. * @@ -26,7 +33,8 @@ #include "drivers/pinio.h" #include "drivers/sensor.h" -BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1 diff --git a/src/main/target/AOCODARCF7MINI/target.h b/src/main/target/AOCODARCF7MINI/target.h index 1c1c36e6fb..ff52565f58 100644 --- a/src/main/target/AOCODARCF7MINI/target.h +++ b/src/main/target/AOCODARCF7MINI/target.h @@ -45,10 +45,10 @@ #define MPU6500_CS_PIN PB2 #define MPU6500_SPI_BUS BUS_SPI1 -#define USE_EXTI -#define MPU6500_EXTI_PIN PC4 - -#define USE_MPU_DATA_READY_SIGNAL +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN PB2 +#define MPU6000_SPI_BUS BUS_SPI1 // *************** I2C /Baro/Mag ********************* #define USE_I2C diff --git a/src/main/target/AOCODARCH7DUAL/CMakeLists.txt b/src/main/target/AOCODARCH7DUAL/CMakeLists.txt new file mode 100644 index 0000000000..bc7c2e86fd --- /dev/null +++ b/src/main/target/AOCODARCH7DUAL/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(AOCODARCH7DUAL) diff --git a/src/main/target/AOCODARCH7DUAL/config.c b/src/main/target/AOCODARCH7DUAL/config.c new file mode 100644 index 0000000000..1065971614 --- /dev/null +++ b/src/main/target/AOCODARCH7DUAL/config.c @@ -0,0 +1,32 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/AOCODARCH7DUAL/target.c b/src/main/target/AOCODARCH7DUAL/target.c new file mode 100644 index 0000000000..3bc551f5c4 --- /dev/null +++ b/src/main/target/AOCODARCH7DUAL/target.c @@ -0,0 +1,55 @@ +/* + * 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" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, DEVFLAGS_NONE, 0, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_1, DEVHW_BMI270, BMI270_SPI_BUS_1, BMI270_CS_PIN_1, BMI270_EXTI_PIN_1, DEVFLAGS_NONE, 0, IMU_BMI270_ALIGN_1); +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, BMI270_SPI_BUS_2, BMI270_CS_PIN_2, BMI270_EXTI_PIN_2, DEVFLAGS_NONE, 1, IMU_BMI270_ALIGN_2); + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 + + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AOCODARCH7DUAL/target.h b/src/main/target/AOCODARCH7DUAL/target.h new file mode 100644 index 0000000000..58d9c4893e --- /dev/null +++ b/src/main/target/AOCODARCH7DUAL/target.h @@ -0,0 +1,211 @@ +/* + * 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 "AOH7" +#define USBD_PRODUCT_STRING "AocodaRCH7Dual" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +// *************** SPI1 IMU0 MPU6000 && BMI270 **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_IMU_MPU6000 + +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PC15 +#define MPU6000_EXTI_PIN PB2 + +#define USE_IMU_BMI270 + +#define IMU_BMI270_ALIGN_1 CW90_DEG +#define BMI270_SPI_BUS_1 BUS_SPI1 +#define BMI270_CS_PIN_1 PC15 +#define BMI270_EXTI_PIN_1 PB2 + +// *************** SPI4 IMU1 BMI270 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_IMU_BMI270 + +#define IMU_BMI270_ALIGN_2 CW180_DEG +#define BMI270_SPI_BUS_2 BUS_SPI4 +#define BMI270_CS_PIN_2 PE11 +#define BMI270_EXTI_PIN_2 PE15 + +// *************** SPI2 OSD *********************** +#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 *********************** +#define USE_SPI_DEVICE_3 + +#define SPI3_NSS_PIN PD3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define SPI3_SCK_AF GPIO_AF6_SPI3 +#define SPI3_MISO_AF GPIO_AF6_SPI3 +#define SPI3_MOSI_AF GPIO_AF7_SPI3 + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN SPI3_NSS_PIN + +#define USE_BLACKBOX +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_VCM5883 + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + + +// *************** UART ***************************** +#define USE_VCP +#define VBUS_SENSING_PIN PE2 +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#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 PB9 +#define UART4_RX_PIN PB8 + +#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 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI +#define ADC_CHANNEL_4_PIN PC4 //ADC12 AirS +#define ADC_CHANNEL_5_PIN PA4 //ADC12 VB2 +#define ADC_CHANNEL_6_PIN PA7 //ADC12 CU2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 // VTX power switcher +#define PINIO2_PIN PD11 // 2xCamera switcher + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#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 + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/AXISFLYINGF7PRO/target.c b/src/main/target/AXISFLYINGF7PRO/target.c index 532e668b3a..fc46a9c0b8 100644 --- a/src/main/target/AXISFLYINGF7PRO/target.c +++ b/src/main/target/AXISFLYINGF7PRO/target.c @@ -25,8 +25,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 diff --git a/src/main/target/AXISFLYINGF7PRO/target.h b/src/main/target/AXISFLYINGF7PRO/target.h index 7128dc9cf7..4a4b95657e 100644 --- a/src/main/target/AXISFLYINGF7PRO/target.h +++ b/src/main/target/AXISFLYINGF7PRO/target.h @@ -52,19 +52,16 @@ #define IMU_MPU6000_ALIGN CW0_DEG #define MPU6000_SPI_BUS BUS_SPI1 #define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_EXTI_PIN GYRO_INT_EXTI #define USE_IMU_ICM42605 #define IMU_ICM42605_ALIGN CW0_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN SPI1_NSS_PIN -#define ICM42605_EXTI_PIN GYRO_INT_EXTI #define USE_IMU_BMI270 #define IMU_BMI270_ALIGN CW0_DEG #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN SPI1_NSS_PIN -#define BMI270_EXTI_PIN GYRO_INT_EXTI // *************** SPI2 OSD ***************// #define USE_MAX7456 @@ -173,6 +170,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 4 +#define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT #define USE_ESC_SENSOR diff --git a/src/main/target/BETAFPVF722/target.h b/src/main/target/BETAFPVF722/target.h index 8384cb8f1d..9d1258385d 100755 --- a/src/main/target/BETAFPVF722/target.h +++ b/src/main/target/BETAFPVF722/target.h @@ -61,7 +61,7 @@ #define I2C_DEVICE_2_SHARES_UART3 #define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 +#define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_BMP280 #define USE_BARO_MS5611 #define USE_BARO_BMP085 diff --git a/src/main/target/FLYCOLORF7MINI/CMakeLists.txt b/src/main/target/FLYCOLORF7MINI/CMakeLists.txt new file mode 100644 index 0000000000..ca394a1ac1 --- /dev/null +++ b/src/main/target/FLYCOLORF7MINI/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(FLYCOLORF7MINI) diff --git a/src/main/target/FLYCOLORF7MINI/config.c b/src/main/target/FLYCOLORF7MINI/config.c new file mode 100644 index 0000000000..30bf875373 --- /dev/null +++ b/src/main/target/FLYCOLORF7MINI/config.c @@ -0,0 +1,30 @@ +/* + * 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" +#include "io/serial.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +} diff --git a/src/main/target/FLYCOLORF7MINI/target.c b/src/main/target/FLYCOLORF7MINI/target.c new file mode 100644 index 0000000000..146aa9fe1c --- /dev/null +++ b/src/main/target/FLYCOLORF7MINI/target.c @@ -0,0 +1,42 @@ +/* + * 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" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // WS2812B +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYCOLORF7MINI/target.h b/src/main/target/FLYCOLORF7MINI/target.h new file mode 100644 index 0000000000..ece951b3c4 --- /dev/null +++ b/src/main/target/FLYCOLORF7MINI/target.h @@ -0,0 +1,164 @@ +/* + * 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 "FL7M" +#define USBD_PRODUCT_STRING "Flycolor F7 Mini" + +#define LED0 PC15 + +#define BEEPER PC14 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW180_DEG_FLIP +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#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_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +// *************** SPI2 OSD *********************** +#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 SD BLACKBOX******************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PC13 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART4 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB8 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 +#define CURRENT_METER_OFFSET 0 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 6 +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR + diff --git a/src/main/target/FLYWOOF405PRO/CMakeLists.txt b/src/main/target/FLYWOOF405PRO/CMakeLists.txt new file mode 100644 index 0000000000..8447a117c0 --- /dev/null +++ b/src/main/target/FLYWOOF405PRO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(FLYWOOF405PRO) diff --git a/src/main/target/FLYWOOF405PRO/target.c b/src/main/target/FLYWOOF405PRO/target.c new file mode 100644 index 0000000000..753f904613 --- /dev/null +++ b/src/main/target/FLYWOOF405PRO/target.c @@ -0,0 +1,40 @@ +/* + * 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 +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,0,2) + + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(1,7,5) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h new file mode 100644 index 0000000000..39a3339c10 --- /dev/null +++ b/src/main/target/FLYWOOF405PRO/target.h @@ -0,0 +1,170 @@ +/* + * 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 "F4PR" +#define USBD_PRODUCT_STRING "FLYWOOF405PRO" + + +#define LED0 PC14 //Green +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW90_DEG +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG_FLIP +#define BMI270_CS_PIN PB12 +#define BMI270_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP +#define ICM42605_CS_PIN PB12 +#define ICM42605_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PB13 +#define USE_MPU_DATA_READY_SIGNAL + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + + + +// *************** SPI2 OSD *************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PB14// + +// *************** Onboard flash ******************** + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_BUS BUS_SPI3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + + +// *************** UART ***************************** +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_PIN PA8 +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PA10 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN NONE +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART3 + + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA9 + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#define CURRENT_METER_SCALE 250 + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 diff --git a/src/main/target/FOXEERF722V4/target.h b/src/main/target/FOXEERF722V4/target.h index aeee2dc913..3c6ab0b2a7 100644 --- a/src/main/target/FOXEERF722V4/target.h +++ b/src/main/target/FOXEERF722V4/target.h @@ -26,15 +26,21 @@ #define BEEPER_INVERTED /*** IMU sensors ***/ -#define USE_EXTI - -#define USE_MPU_DATA_READY_SIGNAL #define USE_IMU_ICM42605 #define IMU_ICM42605_ALIGN CW270_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN PB2 -#define ICM42605_EXTI_PIN PC4 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_CS_PIN PB2 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW270_DEG +#define MPU6500_CS_PIN PB2 +#define MPU6500_SPI_BUS BUS_SPI1 /*** SPI/I2C bus ***/ #define USE_SPI diff --git a/src/main/target/FOXEERF745AIO/target.h b/src/main/target/FOXEERF745AIO/target.h index 498fc7e3a5..60666b7611 100644 --- a/src/main/target/FOXEERF745AIO/target.h +++ b/src/main/target/FOXEERF745AIO/target.h @@ -26,11 +26,6 @@ #define BEEPER_INVERTED /*** IMU sensors ***/ -#define USE_EXTI - - -#define USE_MPU_DATA_READY_SIGNAL - #ifdef FOXEERF745AIO_V3 #define USE_IMU_ICM42605 diff --git a/src/main/target/FOXEERH743/CMakeLists.txt b/src/main/target/FOXEERH743/CMakeLists.txt new file mode 100644 index 0000000000..42b04df8ab --- /dev/null +++ b/src/main/target/FOXEERH743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(FOXEERH743) \ No newline at end of file diff --git a/src/main/target/FOXEERH743/config.c b/src/main/target/FOXEERH743/config.c new file mode 100644 index 0000000000..b83a0a0d03 --- /dev/null +++ b/src/main/target/FOXEERH743/config.c @@ -0,0 +1,57 @@ +/* + * 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 + +#include + +#include "common/axis.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + /* + * UART1 is SerialRX + */ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; + +} diff --git a/src/main/target/FOXEERH743/target.c b/src/main/target/FOXEERH743/target.c new file mode 100644 index 0000000000..c8bc9e7ada --- /dev/null +++ b/src/main/target/FOXEERH743/target.c @@ -0,0 +1,47 @@ +/* + * 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" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 6), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 7), // S8 + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FOXEERH743/target.h b/src/main/target/FOXEERH743/target.h new file mode 100644 index 0000000000..72fef026b1 --- /dev/null +++ b/src/main/target/FOXEERH743/target.h @@ -0,0 +1,165 @@ +/* + * 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 "F743" +#define USBD_PRODUCT_STRING "FOXEERH743" + +#define USE_TARGET_CONFIG + +#define LED0 PC13 + +#define BEEPER PD2 +#define BEEPER_INVERTED + +// *************** SPI1 OSD **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PA4 + +// *************** SPI2 Gyro *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_SPI_BUS BUS_SPI2 +#define MPU6000_CS_PIN PB12 + +// *************** SPI3 FLASH *********************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 +#define SPI3_NSS_PIN PA15 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN SPI3_NSS_PIN + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN SPI3_NSS_PIN + +#define USE_BLACKBOX +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define USE_FLASH_W25N01G +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_VCM5883 + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#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 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL) +#define CURRENT_METER_SCALE 100 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#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 + +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT +#define USE_ESC_SENSOR \ No newline at end of file diff --git a/src/main/target/GEPRCF722_BT_HD/target.h b/src/main/target/GEPRCF722_BT_HD/target.h index efa2211e89..ca32c210c6 100644 --- a/src/main/target/GEPRCF722_BT_HD/target.h +++ b/src/main/target/GEPRCF722_BT_HD/target.h @@ -40,22 +40,16 @@ #define IMU_MPU6000_ALIGN CW0_DEG #define MPU6000_CS_PIN PA15 #define MPU6000_SPI_BUS BUS_SPI1 -#define MPU6000_EXTI_PIN PA8 #define USE_IMU_BMI270 #define IMU_BMI270_ALIGN CW0_DEG #define BMI270_CS_PIN PA15 #define BMI270_SPI_BUS BUS_SPI1 -#define BMI270_EXTI_PIN PA8 #define USE_IMU_ICM42605 #define IMU_ICM42605_ALIGN CW0_DEG #define ICM42605_CS_PIN PA15 #define ICM42605_SPI_BUS BUS_SPI1 -#define ICM42605_EXTI_PIN PA8 - -#define USE_EXTI -#define USE_MPU_DATA_READY_SIGNAL // *************** I2C/Baro/Mag ********************* #define USE_I2C diff --git a/src/main/target/GEPRC_F722_AIO/target.h b/src/main/target/GEPRC_F722_AIO/target.h index ba933c3dc8..ce2ebde6a5 100644 --- a/src/main/target/GEPRC_F722_AIO/target.h +++ b/src/main/target/GEPRC_F722_AIO/target.h @@ -40,22 +40,16 @@ #define IMU_MPU6000_ALIGN CW90_DEG #define MPU6000_CS_PIN PA15 #define MPU6000_SPI_BUS BUS_SPI1 -#define MPU6000_EXTI_PIN PA8 #define USE_IMU_BMI270 #define IMU_BMI270_ALIGN CW90_DEG #define BMI270_CS_PIN PA15 #define BMI270_SPI_BUS BUS_SPI1 -#define BMI270_EXTI_PIN PA8 #define USE_IMU_ICM42605 #define IMU_ICM42605_ALIGN CW90_DEG #define ICM42605_CS_PIN PA15 #define ICM42605_SPI_BUS BUS_SPI1 -#define ICM42605_EXTI_PIN PA8 - -#define USE_EXTI -#define USE_MPU_DATA_READY_SIGNAL // *************** I2C/Baro/Mag ********************* #define USE_I2C diff --git a/src/main/target/HAKRCF722V2/target.h b/src/main/target/HAKRCF722V2/target.h index 3f56a4418a..809f6ddb10 100644 --- a/src/main/target/HAKRCF722V2/target.h +++ b/src/main/target/HAKRCF722V2/target.h @@ -57,8 +57,6 @@ #define I2C1_SDA PB9 // *** IMU sensors *** -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS #define USE_DUAL_GYRO diff --git a/src/main/target/KAKUTEH7WING/CMakeLists.txt b/src/main/target/KAKUTEH7WING/CMakeLists.txt new file mode 100644 index 0000000000..9b23444de9 --- /dev/null +++ b/src/main/target/KAKUTEH7WING/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(KAKUTEH7WING HSE_MHZ 16) diff --git a/src/main/target/KAKUTEH7WING/config.c b/src/main/target/KAKUTEH7WING/config.c new file mode 100644 index 0000000000..1b2670c952 --- /dev/null +++ b/src/main/target/KAKUTEH7WING/config.c @@ -0,0 +1,48 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "rx/rx.h" + +#include "io/piniobox.h" +#include "drivers/serial.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[1].gps_baudrateIndex = BAUD_115200; + + serialConfigMutable()->portConfigs[5].functionMask = FUNCTION_RX_SERIAL; + rxConfigMutable()->receiverType = RX_TYPE_SERIAL; + rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + + serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[6].msp_baudrateIndex = BAUD_115200; + + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3; + pinioBoxConfigMutable()->permanentId[3] = BOX_PERMANENT_ID_USER4; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/KAKUTEH7WING/hardware_setup.c b/src/main/target/KAKUTEH7WING/hardware_setup.c new file mode 100644 index 0000000000..855c80d05e --- /dev/null +++ b/src/main/target/KAKUTEH7WING/hardware_setup.c @@ -0,0 +1,59 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" + +#include "drivers/time.h" +#include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" + +void initialisePreBootHardware(void) +{ + // VDD_3V3_SENSORS_EN + IOInit(DEFIO_IO(PB2), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PB2), IOCFG_OUT_PP); + // IOLo(DEFIO_IO(PB2)); + // delay(100); + IOHi(DEFIO_IO(PB2)); + + // CAM Switch / User3 + IOInit(DEFIO_IO(PC13), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PC13), IOCFG_OUT_PP); + IOLo(DEFIO_IO(PC13)); + + // User1 + IOInit(DEFIO_IO(PD4), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PD4), IOCFG_OUT_PP); + IOLo(DEFIO_IO(PD4)); + + // VTx 9V Switch / User4 + IOInit(DEFIO_IO(PE3), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PE3), IOCFG_OUT_PP); + IOHi(DEFIO_IO(PE3)); + + // User2 + IOInit(DEFIO_IO(PE4), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PE4), IOCFG_OUT_PP); + IOLo(DEFIO_IO(PE4)); +} diff --git a/src/main/target/KAKUTEH7WING/target.c b/src/main/target/KAKUTEH7WING/target.c new file mode 100644 index 0000000000..2b880fc3b7 --- /dev/null +++ b/src/main/target/KAKUTEH7WING/target.c @@ -0,0 +1,57 @@ +/* + * 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" + +BUSDEV_REGISTER_SPI_TAG(busdev_bmi088_gyro, DEVHW_BMI088_GYRO, BMI088_SPI_BUS, BMI088_GYRO_CS_PIN, BMI088_GYRO_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_BMI088_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_bmi088_acc, DEVHW_BMI088_ACC, BMI088_SPI_BUS, BMI088_ACC_CS_PIN, BMI088_ACC_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_BMI088_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +// BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 + DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA_NONE + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S7 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S8 + + DEF_TIM(TIM15,CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S9 + DEF_TIM(TIM15,CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S13 + //DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S14 / LED_2812 + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // S14 / LED_2812 + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h new file mode 100644 index 0000000000..72cf6b6806 --- /dev/null +++ b/src/main/target/KAKUTEH7WING/target.h @@ -0,0 +1,204 @@ +/* + * 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 "KH7W" +#define USBD_PRODUCT_STRING "KAKUTEH7WING" + +#define USE_HARDWARE_PREBOOT_SETUP + +#define USE_TARGET_CONFIG + +#define LED0 PC15 +#define LED1 PC14 + +#define BEEPER PB9 +#define BEEPER_INVERTED + +// *************** IMU generic *********************** +// #define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +// *************** SPI1 IMU0 BMI088 ****************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_BMI088 + +#define IMU_BMI088_ALIGN CW0_DEG +#define BMI088_SPI_BUS BUS_SPI1 + +#define BMI088_GYRO_CS_PIN PC9 +#define BMI088_GYRO_EXTI_PIN PD10 +#define BMI088_ACC_CS_PIN PC8 +#define BMI088_ACC_EXTI_PIN PD11 + +// *************** SPI3 IMU1 ICM42688 ************ +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI3 +#define ICM42605_CS_PIN PE12 +#define ICM42605_EXTI_PIN PE15 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PD3 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_2 +#define SDCARD_SDIO_4BIT +#define SDCARD_SDIO_NORMAL_SPEED +#define SDCARD_SDIO2_CMD_ALT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_I2C_DEVICE_4 +#define I2C4_SCL PD12 +#define I2C4_SDA PD13 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C4 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_VCM5883 + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP +#define VBUS_SENSING_PIN PA9 +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PA10 + +#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_UART5 +#define UART5_TX_PIN PB13 +#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 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC5 //VBAT1 +#define ADC_CHANNEL_2_PIN PC4 //CURR1 +#define ADC_CHANNEL_3_PIN PC0 //RSSI +#define ADC_CHANNEL_5_PIN PA3 //VB2 +#define ADC_CHANNEL_6_PIN PA2 //CU2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC13 // VTX power switcher +#define PINIO2_PIN PE3 // 2xCamera switcher +#define PINIO3_PIN PD4 // User1 +#define PINIO4_PIN PE4 // User2 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA15 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_GPS) +#define CURRENT_METER_SCALE 3660 // 36.6 +#define VBAT_SCALE_DEFAULT 1818 // 18.18 +#define VBAT_SCALE_DEFAULT2 1100 // 11.0 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#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 + +#define MAX_PWM_OUTPUT_PORTS 14 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h index 92c9f351a1..42cddf5681 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.h +++ b/src/main/target/NEUTRONRCF435MINI/target.h @@ -56,10 +56,6 @@ #define SPI1_MOSI_PIN PA7 #define SPI1_NSS_PIN PA4 -// #define USE_EXTI -// #define GYRO_INT_EXTI PA15 -// #define USE_MPU_DATA_READY_SIGNAL - // MPU6500 #define USE_IMU_MPU6500 #define IMU_MPU6500_ALIGN CW0_DEG @@ -71,7 +67,6 @@ #define IMU_ICM42605_ALIGN CW0_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN SPI1_NSS_PIN -// #define ICM42605_EXTI_PIN GYRO_INT_EXTI // BMI270 #define USE_IMU_BMI270 diff --git a/src/main/target/NEUTRONRCF435SE/target.h b/src/main/target/NEUTRONRCF435SE/target.h index 4a151ca290..5d00a09911 100644 --- a/src/main/target/NEUTRONRCF435SE/target.h +++ b/src/main/target/NEUTRONRCF435SE/target.h @@ -54,10 +54,6 @@ #define SPI1_MOSI_PIN PA7 #define SPI1_NSS_PIN PA15 -// #define USE_EXTI -// #define GYRO_INT_EXTI PA8 -// #define USE_MPU_DATA_READY_SIGNAL - // MPU6500 #define USE_IMU_MPU6500 #define IMU_MPU6500_ALIGN CW180_DEG @@ -69,7 +65,6 @@ #define IMU_ICM42605_ALIGN CW180_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN SPI1_NSS_PIN -// #define ICM42605_EXTI_PIN GYRO_INT_EXTI // BMI270 #define USE_IMU_BMI270 diff --git a/src/main/target/NEUTRONRCF435WING/target.h b/src/main/target/NEUTRONRCF435WING/target.h index 6b6a288b51..4f56d86778 100644 --- a/src/main/target/NEUTRONRCF435WING/target.h +++ b/src/main/target/NEUTRONRCF435WING/target.h @@ -56,10 +56,6 @@ #define SPI1_MOSI_PIN PA7 #define SPI1_NSS_PIN PA4 -// #define USE_EXTI -// #define GYRO_INT_EXTI PA15 -// #define USE_MPU_DATA_READY_SIGNAL - // MPU6500 #define USE_IMU_MPU6500 #define IMU_MPU6500_ALIGN CW0_DEG @@ -71,7 +67,6 @@ #define IMU_ICM42605_ALIGN CW0_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN SPI1_NSS_PIN -// #define ICM42605_EXTI_PIN GYRO_INT_EXTI // BMI270 #define USE_IMU_BMI270 diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h index 1c25d0cfa5..ba5915892b 100644 --- a/src/main/target/RUSH_BLADE_F7/target.h +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -53,13 +53,11 @@ #define IMU_MPU6000_ALIGN CW270_DEG #define MPU6000_SPI_BUS BUS_SPI1 #define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_EXTI_PIN GYRO_INT_EXTI #define USE_IMU_ICM42605 #define IMU_ICM42605_ALIGN CW270_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN SPI1_NSS_PIN -#define ICM42605_EXTI_PIN GYRO_INT_EXTI // *************** I2C/Baro/Mag ********************* #define USE_I2C diff --git a/src/main/target/SAGEATF4/target.h b/src/main/target/SAGEATF4/target.h index a0e734e5fd..3ec00a37b3 100644 --- a/src/main/target/SAGEATF4/target.h +++ b/src/main/target/SAGEATF4/target.h @@ -55,16 +55,11 @@ #define SPI1_MOSI_PIN PA7 #define SPI1_NSS_PIN PA4 -#define USE_EXTI -#define GYRO_INT_EXTI PA15 -#define USE_MPU_DATA_READY_SIGNAL - // MPU6000 #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW0_DEG #define MPU6000_SPI_BUS BUS_SPI1 #define MPU6000_CS_PIN SPI1_NSS_PIN -#define MPU6000_EXTI_PIN GYRO_INT_EXTI // MPU6500 #define USE_IMU_MPU6500 #define IMU_MPU6500_ALIGN CW0_DEG @@ -76,7 +71,6 @@ #define IMU_ICM42605_ALIGN CW0_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN SPI1_NSS_PIN -#define ICM42605_EXTI_PIN GYRO_INT_EXTI // MPU9250 #define USE_IMU_MPU9250 diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index 4c3f2fcfe9..eb164c4293 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -80,46 +80,46 @@ static bool useImu = false; typedef struct { - double m_channelValues[RF_MAX_PWM_OUTS]; - double m_currentPhysicsSpeedMultiplier; - double m_currentPhysicsTime_SEC; - double m_airspeed_MPS; - double m_altitudeASL_MTR; - double m_altitudeAGL_MTR; - double m_groundspeed_MPS; - double m_pitchRate_DEGpSEC; - double m_rollRate_DEGpSEC; - double m_yawRate_DEGpSEC; - double m_azimuth_DEG; - double m_inclination_DEG; - double m_roll_DEG; - double m_orientationQuaternion_X; - double m_orientationQuaternion_Y; - double m_orientationQuaternion_Z; - double m_orientationQuaternion_W; - double m_aircraftPositionX_MTR; - double m_aircraftPositionY_MTR; - double m_velocityWorldU_MPS; - double m_velocityWorldV_MPS; - double m_velocityWorldW_MPS; - double m_velocityBodyU_MPS; - double m_velocityBodyV_MPS; - double m_velocityBodyW_MPS; - double m_accelerationWorldAX_MPS2; - double m_accelerationWorldAY_MPS2; - double m_accelerationWorldAZ_MPS2; - double m_accelerationBodyAX_MPS2; - double m_accelerationBodyAY_MPS2; - double m_accelerationBodyAZ_MPS2; - double m_windX_MPS; - double m_windY_MPS; - double m_windZ_MPSPS; - double m_propRPM; - double m_heliMainRotorRPM; - double m_batteryVoltage_VOLTS; - double m_batteryCurrentDraw_AMPS; - double m_batteryRemainingCapacity_MAH; - double m_fuelRemaining_OZ; + float m_channelValues[RF_MAX_PWM_OUTS]; + float m_currentPhysicsSpeedMultiplier; + float m_currentPhysicsTime_SEC; + float m_airspeed_MPS; + float m_altitudeASL_MTR; + float m_altitudeAGL_MTR; + float m_groundspeed_MPS; + float m_pitchRate_DEGpSEC; + float m_rollRate_DEGpSEC; + float m_yawRate_DEGpSEC; + float m_azimuth_DEG; + float m_inclination_DEG; + float m_roll_DEG; + float m_orientationQuaternion_X; + float m_orientationQuaternion_Y; + float m_orientationQuaternion_Z; + float m_orientationQuaternion_W; + float m_aircraftPositionX_MTR; + float m_aircraftPositionY_MTR; + float m_velocityWorldU_MPS; + float m_velocityWorldV_MPS; + float m_velocityWorldW_MPS; + float m_velocityBodyU_MPS; + float m_velocityBodyV_MPS; + float m_velocityBodyW_MPS; + float m_accelerationWorldAX_MPS2; + float m_accelerationWorldAY_MPS2; + float m_accelerationWorldAZ_MPS2; + float m_accelerationBodyAX_MPS2; + float m_accelerationBodyAY_MPS2; + float m_accelerationBodyAZ_MPS2; + float m_windX_MPS; + float m_windY_MPS; + float m_windZ_MPSPS; + float m_propRPM; + float m_heliMainRotorRPM; + float m_batteryVoltage_VOLTS; + float m_batteryCurrentDraw_AMPS; + float m_batteryRemainingCapacity_MAH; + float m_fuelRemaining_OZ; bool m_isLocked; bool m_hasLostComponents; bool m_anEngineIsRunning; @@ -244,34 +244,38 @@ static bool getChannelValues(const char* response, uint16_t* channelValues) } -static void fakeCoords(double posX, double posY, double distanceX, double distanceY, double *lat, double *lon) +static void fakeCoords(float posX, float posY, float distanceX, float distanceY, float *lat, float *lon) { - double m = 1 / (2 * (double)M_PIf / 360 * EARTH_RADIUS) / 1000; - *lat = (double)(posX + (distanceX * m)); - *lon = (double)(posY + (distanceY * m) / cos(posX * ((double)M_PIf / 180))); + float m = 1 / (2 * M_PIf / 360 * EARTH_RADIUS) / 1000; + *lat = (posX + (distanceX * m)); + *lon = (posY + (distanceY * m) / cosf(posX * (M_PIf / 180))); } -static double convertAzimuth(double azimuth) +static float convertAzimuth(float azimuth) { if (azimuth < 0) { azimuth += 360; } - return 360 - fmod(azimuth + 90, 360.0f); + return 360 - fmodf(azimuth + 90, 360.0f); } static void exchangeData(void) { - double servoValues[RF_MAX_PWM_OUTS] = { 0 }; + double servoValues[RF_MAX_PWM_OUTS] = { }; for (int i = 0; i < mappingCount; i++) { - if (pwmMapping[i] & 0x80){ // Motor - servoValues[i] = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]); + if (pwmMapping[i] & 0x80) { // Motor + servoValues[i] = (double)PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]); } else { - servoValues[i] = PWM_TO_FLOAT_0_1(servo[pwmMapping[i]]); + servoValues[i] = (double)PWM_TO_FLOAT_0_1(servo[pwmMapping[i]]); } } - startRequest("ExchangeData", "%u%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f", - 0xFFF, servoValues[0], servoValues[1], servoValues[2], servoValues[3], servoValues[4], servoValues[5], servoValues[6], servoValues[7], servoValues[8], servoValues[9], servoValues[10], servoValues[11]); + startRequest("ExchangeData", "%u" + "%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f" + "%.4f%.4f%.4f%.4f", + 0xFFF, + servoValues[0], servoValues[1], servoValues[2], servoValues[3], servoValues[4], servoValues[5], servoValues[6], servoValues[7], + servoValues[8], servoValues[9], servoValues[10], servoValues[11]); char* response = endRequest(); //rfValues.m_currentPhysicsTime_SEC = getDoubleFromResponse(response, "m-currentPhysicsTime-SEC"); @@ -325,18 +329,18 @@ static void exchangeData(void) getChannelValues(response, channelValues); rxSimSetChannelValue(channelValues, RF_MAX_CHANNEL_COUNT); - double lat, lon; + float lat, lon; fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon); - int16_t course = (int16_t)round(convertAzimuth(rfValues.m_azimuth_DEG) * 10); - int32_t altitude = (int32_t)round(rfValues.m_altitudeASL_MTR * 100); + int16_t course = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10); + int32_t altitude = (int32_t)roundf(rfValues.m_altitudeASL_MTR * 100); gpsFakeSet( GPS_FIX_3D, 16, - (int32_t)round(lat * 10000000), - (int32_t)round(lon * 10000000), + (int32_t)roundf(lat * 10000000), + (int32_t)roundf(lon * 10000000), altitude, - (int16_t)round(rfValues.m_groundspeed_MPS * 100), + (int16_t)roundf(rfValues.m_groundspeed_MPS * 100), course, 0, 0, @@ -344,15 +348,15 @@ static void exchangeData(void) 0 ); - int32_t altitudeOverGround = (int32_t)round(rfValues.m_altitudeAGL_MTR * 100); + int32_t altitudeOverGround = (int32_t)roundf(rfValues.m_altitudeAGL_MTR * 100); if (altitudeOverGround > 0 && altitudeOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) { fakeRangefindersSetData(altitudeOverGround); } else { fakeRangefindersSetData(-1); } - const int16_t roll_inav = (int16_t)round(rfValues.m_roll_DEG * 10); - const int16_t pitch_inav = (int16_t)round(-rfValues.m_inclination_DEG * 10); + const int16_t roll_inav = (int16_t)roundf(rfValues.m_roll_DEG * 10); + const int16_t pitch_inav = (int16_t)roundf(-rfValues.m_inclination_DEG * 10); const int16_t yaw_inav = course; if (!useImu) { imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav); @@ -376,16 +380,16 @@ static void exchangeData(void) fakeAccSet(accX, accY, accZ); fakeGyroSet( - constrainToInt16(rfValues.m_rollRate_DEGpSEC * (double)16.0), - constrainToInt16(-rfValues.m_pitchRate_DEGpSEC * (double)16.0), - constrainToInt16(rfValues.m_yawRate_DEGpSEC * (double)16.0) + constrainToInt16(rfValues.m_rollRate_DEGpSEC * 16.0f), + constrainToInt16(-rfValues.m_pitchRate_DEGpSEC * 16.0f), + constrainToInt16(rfValues.m_yawRate_DEGpSEC * 16.0f) ); fakeBaroSet(altitudeToPressure(altitude), DEGREES_TO_CENTIDEGREES(21)); fakePitotSetAirspeed(rfValues.m_airspeed_MPS * 100); - fakeBattSensorSetVbat((uint16_t)round(rfValues.m_batteryVoltage_VOLTS * 100)); - fakeBattSensorSetAmperage((uint16_t)round(rfValues.m_batteryCurrentDraw_AMPS * 100)); + fakeBattSensorSetVbat((uint16_t)roundf(rfValues.m_batteryVoltage_VOLTS * 100)); + fakeBattSensorSetAmperage((uint16_t)roundf(rfValues.m_batteryCurrentDraw_AMPS * 100)); fpQuaternion_t quat; fpVector3_t north; diff --git a/src/main/target/SITL/sim/simHelper.c b/src/main/target/SITL/sim/simHelper.c index 576060199a..65622ed5f0 100644 --- a/src/main/target/SITL/sim/simHelper.c +++ b/src/main/target/SITL/sim/simHelper.c @@ -30,7 +30,7 @@ #include "target/SITL/sim/simHelper.h" -inline int16_t constrainToInt16(double value) +inline int16_t constrainToInt16(float value) { return (int16_t)round(constrain(value, INT16_MIN, INT16_MAX)); } diff --git a/src/main/target/SITL/sim/simHelper.h b/src/main/target/SITL/sim/simHelper.h index 22eaf0ab2e..9137b4fa0c 100644 --- a/src/main/target/SITL/sim/simHelper.h +++ b/src/main/target/SITL/sim/simHelper.h @@ -26,12 +26,12 @@ #include "common/maths.h" #include "common/quaternion.h" -#define EARTH_RADIUS ((double)6378.137) -#define PWM_TO_FLOAT_0_1(x) (((int)x - 1000) / 1000.0f) -#define PWM_TO_FLOAT_MINUS_1_1(x) (((int)x - 1500) / 500.0f) +#define EARTH_RADIUS (6378.137f) +#define PWM_TO_FLOAT_0_1(x) ((float)(((int)x - 1000) / 1000.0f)) +#define PWM_TO_FLOAT_MINUS_1_1(x) ((float)(((int)x - 1500) / 500.0f)) #define FLOAT_0_1_TO_PWM(x) ((uint16_t)(x * 1000.0f) + 1000.0f) -#define FLOAT_MINUS_1_1_TO_PWM(x) ((uint16_t)((x + 1.0f) / 2.0f * 1000.0f) + 1000.0f) +#define FLOAT_MINUS_1_1_TO_PWM(x) ((float)((uint16_t)((x + 1.0f) / 2.0f * 1000.0f) + 1000.0f)) -int16_t constrainToInt16(double value); +int16_t constrainToInt16(float value); void transformVectorEarthToBody(fpVector3_t *v, const fpQuaternion_t *quat); void computeQuaternionFromRPY(fpQuaternion_t *quat, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw); \ No newline at end of file diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index d98d9aa28f..44089117d2 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/src/main/target/SITL/sim/xplane.c @@ -132,18 +132,18 @@ typedef enum 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) { - 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) @@ -369,18 +369,18 @@ static void* listenWorker(void* arg) gpsFakeSet( GPS_FIX_3D, 16, - (int32_t)round(lattitude * 10000000), - (int32_t)round(longitude * 10000000), - (int32_t)round(elevation * 100), - (int16_t)round(groundspeed * 100), - (int16_t)round(hpath * 10), - 0, //(int16_t)round(-local_vz * 100), - 0, //(int16_t)round(local_vx * 100), - 0, //(int16_t)round(-local_vy * 100), + (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 ); - const int32_t altitideOverGround = (int32_t)round(agl * 100); + const int32_t altitideOverGround = (int32_t)roundf(agl * 100); if (altitideOverGround > 0 && altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) { fakeRangefindersSetData(altitideOverGround); } else { @@ -397,9 +397,9 @@ static void* listenWorker(void* arg) } fakeAccSet( - constrainToInt16(-accel_x * GRAVITY_MSS * 1000), - constrainToInt16(accel_y * GRAVITY_MSS * 1000), - constrainToInt16(accel_z * GRAVITY_MSS * 1000) + constrainToInt16(-accel_x * GRAVITY_MSS * 1000.0f), + constrainToInt16(accel_y * GRAVITY_MSS * 1000.0f), + constrainToInt16(accel_z * GRAVITY_MSS * 1000.0f) ); fakeGyroSet( @@ -408,16 +408,16 @@ static void* listenWorker(void* arg) constrainToInt16(-gyro_z * 16.0f) ); - fakeBaroSet((int32_t)round(barometer * 3386.39f), DEGREES_TO_CENTIDEGREES(21)); + fakeBaroSet((int32_t)roundf(barometer * 3386.39f), DEGREES_TO_CENTIDEGREES(21)); fakePitotSetAirspeed(airspeed * 100.0f); - fakeBattSensorSetVbat(16.8 * 100); + fakeBattSensorSetVbat(16.8f * 100); fpQuaternion_t quat; fpVector3_t north; north.x = 1.0f; - north.y = 0; - north.z = 0; + north.y = 0.0f; + north.z = 0.0f; computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav); transformVectorEarthToBody(&north, &quat); fakeMagSet( @@ -439,86 +439,6 @@ static void* listenWorker(void* arg) return NULL; } -static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len ) -{ - struct addrinfo *servinfo, *p; - struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG}; - if (name == NULL) { - hints.ai_flags |= AI_PASSIVE; - } - /* - This nonsense is to uniformly deliver the same sa_family regardless of whether - name is NULL or non-NULL ** ON LINUX ** - Otherwise, at least on Linux, we get - - V6,V4 for the non-null case and - - V4,V6 for the null case, regardless of gai.conf - Which may confuse consumers - FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks - unless a quad dotted address is specified (or a name resolveds to V4, - or system policy enforces IPv4 over V6 - */ - struct addrinfo *p4 = NULL; - struct addrinfo *p6 = NULL; - - int result; - char aport[16]; - snprintf(aport, sizeof(aport), "%d", port); - - if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) { - fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result)); - return result; - } else { - for(p = servinfo; p != NULL; p = p->ai_next) { - if(p->ai_family == AF_INET6) - p6 = p; - else if(p->ai_family == AF_INET) - p4 = p; - } - - if (p6 != NULL) - p = p6; - else if (p4 != NULL) - p = p4; - else - return -1; - memcpy(addr, p->ai_addr, p->ai_addrlen); - *len = p->ai_addrlen; - freeaddrinfo(servinfo); - } - return 0; -} - -static char * pretty_print_address(struct sockaddr* p) -{ - char straddr[INET6_ADDRSTRLEN]; - void *addr; - uint16_t port; - if (p->sa_family == AF_INET6) { - struct sockaddr_in6 * ip = (struct sockaddr_in6*)p; - addr = &ip->sin6_addr; - port = ntohs(ip->sin6_port); - } else { - struct sockaddr_in * ip = (struct sockaddr_in*)p; - port = ntohs(ip->sin_port); - addr = &ip->sin_addr; - } - const char *res = inet_ntop(p->sa_family, addr, straddr, sizeof straddr); - if (res != NULL) { - int nb = strlen(res)+16; - char *buf = calloc(nb,1); - char *ptr = buf; - if (p->sa_family == AF_INET6) { - *ptr++='['; - } - ptr = stpcpy(ptr, res); - if (p->sa_family == AF_INET6) { - *ptr++=']'; - } - sprintf(ptr, ":%d", port); - return buf; - } - return NULL; -} bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu) { @@ -527,10 +447,10 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool useImu = imu; if (port == 0) { - port = XP_PORT; // use default port + port = XP_PORT; // use default port } - if(lookup_address(ip, port, SOCK_DGRAM, (struct sockaddr*)&serverAddr, &serverAddrLen) != 0) { + if(lookupAddress(ip, port, SOCK_DGRAM, (struct sockaddr*)&serverAddr, &serverAddrLen) != 0) { return false; } @@ -538,17 +458,17 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool if (sockFd < 0) { return false; } else { - char *nptr = pretty_print_address((struct sockaddr *)&serverAddr); - if (nptr != NULL) { - fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd); - free(nptr); - } + 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; } diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 957af2541f..4bf53a791a 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -34,6 +34,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include "target.h" @@ -364,3 +369,84 @@ char * strnstr(const char *s, const char *find, size_t slen) } return ((char *)s); } + +int lookupAddress (char *name, int port, int type, struct sockaddr *addr, socklen_t* len ) +{ + struct addrinfo *servinfo, *p; + struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG}; + if (name == NULL) { + hints.ai_flags |= AI_PASSIVE; + } + /* + This nonsense is to uniformly deliver the same sa_family regardless of whether + name is NULL or non-NULL ** ON LINUX ** + Otherwise, at least on Linux, we get + - V6,V4 for the non-null case and + - V4,V6 for the null case, regardless of gai.conf + Which may confuse consumers + FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks + unless a quad dotted address is specified (or a name resolveds to V4, + or system policy enforces IPv4 over V6 + */ + struct addrinfo *p4 = NULL; + struct addrinfo *p6 = NULL; + + int result; + char aport[16]; + snprintf(aport, sizeof(aport), "%d", port); + + if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) { + fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result)); + return result; + } else { + for(p = servinfo; p != NULL; p = p->ai_next) { + if(p->ai_family == AF_INET6) + p6 = p; + else if(p->ai_family == AF_INET) + p4 = p; + } + + if (p6 != NULL) + p = p6; + else if (p4 != NULL) + p = p4; + else + return -1; + memcpy(addr, p->ai_addr, p->ai_addrlen); + *len = p->ai_addrlen; + freeaddrinfo(servinfo); + } + return 0; +} + +char *prettyPrintAddress(struct sockaddr* p, char *outbuf, size_t buflen) +{ + if (buflen < IPADDRESS_PRINT_BUFLEN) { + return NULL; + } + char *bufp = outbuf; + void *addr; + uint16_t port; + if (p->sa_family == AF_INET6) { + struct sockaddr_in6 * ip = (struct sockaddr_in6*)p; + addr = &ip->sin6_addr; + port = ntohs(ip->sin6_port); + } else { + struct sockaddr_in * ip = (struct sockaddr_in*)p; + port = ntohs(ip->sin_port); + addr = &ip->sin_addr; + } + const char *res = inet_ntop(p->sa_family, addr, outbuf+1, buflen-1); + if (res != NULL) { + char *ptr = (char*)res+strlen(res); + if (p->sa_family == AF_INET6) { + *bufp ='['; + *ptr++ = ']'; + } else { + bufp++; + } + sprintf(ptr, ":%d", port); + return bufp; + } + return NULL; +} diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index f2c76497ad..78f5022a9e 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -27,6 +27,7 @@ #include #include #include +#include #include @@ -69,6 +70,9 @@ #define USE_RANGEFINDER_FAKE #define USE_RX_SIM +#define USE_MSP_OSD +#define USE_OSD + #undef USE_DASHBOARD #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? @@ -86,9 +90,6 @@ #undef USE_TELEMETRY_JETIEXBUS #undef USE_TELEMETRY_SRXL #undef USE_TELEMETRY_GHST -#undef USE_VTX_COMMON -#undef USE_VTX_CONTROL -#undef USE_VTX_SMARTAUDIO #undef USE_VTX_TRAMP #undef USE_CAMERA_CONTROL #undef USE_BRUSHED_ESC_AUTODETECT @@ -186,7 +187,13 @@ typedef enum SITL_SIM_XPLANE, } SitlSim_e; -bool lockMainPID(void); -void unlockMainPID(void); -void parseArguments(int argc, char *argv[]); -char *strnstr(const char *s, const char *find, size_t slen); + + +extern bool lockMainPID(void); +extern void unlockMainPID(void); +extern void parseArguments(int argc, char *argv[]); +extern char *strnstr(const char *s, const char *find, size_t slen); +extern int lookupAddress (char *, int, int, struct sockaddr *, socklen_t*); + +#define IPADDRESS_PRINT_BUFLEN (INET6_ADDRSTRLEN + 16) +extern char *prettyPrintAddress(struct sockaddr*, char*, size_t); diff --git a/src/main/target/SPEEDYBEEF405WING/target.c b/src/main/target/SPEEDYBEEF405WING/target.c index 4892321064..a06d783801 100644 --- a/src/main/target/SPEEDYBEEF405WING/target.c +++ b/src/main/target/SPEEDYBEEF405WING/target.c @@ -30,14 +30,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(2,4,7) DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(2,7,7) - DEF_TIM(TIM8, CH2N,PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S7 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S8 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S9 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S10 - DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S11 - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) + DEF_TIM(TIM8, CH2N,PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S7 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S8 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S9 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S10 + DEF_TIM(TIM1, CH3N,PB15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S11 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/common.h b/src/main/target/common.h index 4a60523548..d18bdfb1a2 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -166,6 +166,7 @@ #define USE_VTX_CONTROL #define USE_VTX_SMARTAUDIO #define USE_VTX_TRAMP +#define USE_VTX_MSP #define USE_PROGRAMMING_FRAMEWORK #define USE_CLI_BATCH diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 3f674d8df2..623fdcbbbc 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -364,10 +364,10 @@ /** OTHER HARDWARE **/ #if defined(USE_MAX7456) - BUSDEV_REGISTER_SPI(busdev_max7456, DEVHW_MAX7456, MAX7456_SPI_BUS, MAX7456_CS_PIN, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); + BUSDEV_REGISTER_SPI(busdev_max7456, DEVHW_MAX7456, MAX7456_SPI_BUS, MAX7456_CS_PIN, NONE, DEVFLAGS_USE_RAW_REGISTERS | DEVFLAGS_SPI_MODE_0, 0); #endif -#if defined(USE_FLASH_M25P16) +#if defined(USE_FLASH_M25P16) && defined(M25P16_SPI_BUS) BUSDEV_REGISTER_SPI(busdev_m25p16, DEVHW_M25P16, M25P16_SPI_BUS, M25P16_CS_PIN, NONE, DEVFLAGS_NONE, 0); #endif @@ -376,7 +376,7 @@ #endif #if defined(USE_SDCARD) && defined(USE_SDCARD_SPI) - BUSDEV_REGISTER_SPI(busdev_sdcard_spi, DEVHW_SDCARD, SDCARD_SPI_BUS, SDCARD_CS_PIN, NONE, DEVFLAGS_USE_MANUAL_DEVICE_SELECT | DEVFLAGS_SPI_MODE_0, 0); + BUSDEV_REGISTER_SPI(busdev_sdcard_spi, DEVHW_SDCARD, SDCARD_SPI_BUS, SDCARD_CS_PIN, NONE, DEVFLAGS_SPI_MODE_0, 0); #endif /* diff --git a/src/main/target/system_stm32h7xx.c b/src/main/target/system_stm32h7xx.c index ce16a546ff..cc580320b5 100644 --- a/src/main/target/system_stm32h7xx.c +++ b/src/main/target/system_stm32h7xx.c @@ -256,6 +256,8 @@ static void SystemClockHSE_Config(void) pllConfig_t *pll1Config = (HAL_GetREVID() == REV_ID_V) ? &pll1ConfigRevV : &pll1ConfigRevY; #endif + pll1Config->m = HSE_VALUE / 1000000 / 2; // correction for different HSE_VALUE + // Configure voltage scale. // It has been pre-configured at PWR_REGULATOR_VOLTAGE_SCALE1, // and it may stay or overridden by PWR_REGULATOR_VOLTAGE_SCALE0 depending on the clock config. @@ -509,6 +511,12 @@ void SystemClock_Config(void) HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit); #endif +#ifdef USE_QUADSPI + RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_QSPI; + RCC_PeriphClkInit.QspiClockSelection = RCC_QSPICLKSOURCE_D1HCLK; + HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit); +#endif + // Configure MCO clocks for clock test/verification // Possible sources for MCO1: diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 9be24ed767..a0537bfcd5 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -375,8 +375,8 @@ static void sendSMS(void) amps / 10, amps % 10, getAltitudeMeters(), groundSpeed, avgSpeed / 10, avgSpeed % 10, - GPS_distanceToHome, getTotalTravelDistance() / 100, - DECIDEGREES_TO_DEGREES(attitude.values.yaw), + (unsigned long)GPS_distanceToHome, getTotalTravelDistance() / 100ul, + (int)DECIDEGREES_TO_DEGREES(attitude.values.yaw), gpsSol.numSat, gpsFixIndicators[gpsSol.fixType], simRssi, getStateOfForcedRTH() == RTH_IDLE ? modeDescriptions[getFlightModeForTelemetry()] : "RTH", @@ -388,7 +388,7 @@ static void sendSMS(void) atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE; } -void handleSimTelemetry() +void handleSimTelemetry(void) { static uint16_t simResponseIndex = 0; uint32_t now = millis(); diff --git a/src/main/telemetry/sim.h b/src/main/telemetry/sim.h index 299981e623..40e40e6a8c 100644 --- a/src/main/telemetry/sim.h +++ b/src/main/telemetry/sim.h @@ -17,8 +17,8 @@ #pragma once -#define SIM_MIN_TRANSMIT_INTERVAL 10 -#define SIM_DEFAULT_TRANSMIT_INTERVAL 60 +#define SIM_MIN_TRANSMIT_INTERVAL 10u +#define SIM_DEFAULT_TRANSMIT_INTERVAL 60u #define SIM_N_TX_FLAGS 5 #define SIM_DEFAULT_TX_FLAGS "f" #define SIM_PIN "0000" diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index abb9389fbb..ebfd3b78d4 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -37,6 +37,9 @@ set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular set_property(SOURCE osd_unittest.cc PROPERTY depends "io/osd_utils.c" "io/displayport_msp_osd.c" "common/typeconversion.c") set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_DISPLAYPORT DISABLE_MSP_BF_COMPAT) +set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c") +set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST) + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc.txt similarity index 95% rename from src/test/unit/alignsensor_unittest.cc rename to src/test/unit/alignsensor_unittest.cc.txt index d060315112..5a17a6eebe 100644 --- a/src/test/unit/alignsensor_unittest.cc +++ b/src/test/unit/alignsensor_unittest.cc.txt @@ -40,9 +40,9 @@ extern "C" { #define DEG2RAD 0.01745329251 -static void rotateVector(int32_t mat[3][3], int32_t vec[3], int32_t *out) +static void rotateVector(float mat[3][3], float vec[3], float *out) { - int32_t tmp[3]; + float tmp[3]; for(int i=0; i<3; i++) { tmp[i] = 0; @@ -70,7 +70,7 @@ static void rotateVector(int32_t mat[3][3], int32_t vec[3], int32_t *out) // mat[2][2] = cos(angle*DEG2RAD); //} -static void initYAxisRotation(int32_t mat[][3], int32_t angle) +static void initYAxisRotation(float mat[][3], int32_t angle) { mat[0][0] = cos(angle*DEG2RAD); mat[0][1] = 0; @@ -83,7 +83,7 @@ static void initYAxisRotation(int32_t mat[][3], int32_t angle) mat[2][2] = cos(angle*DEG2RAD); } -static void initZAxisRotation(int32_t mat[][3], int32_t angle) +static void initZAxisRotation(float mat[][3], int32_t angle) { mat[0][0] = cos(angle*DEG2RAD); mat[0][1] = -sin(angle*DEG2RAD); @@ -98,15 +98,15 @@ static void initZAxisRotation(int32_t mat[][3], int32_t angle) static void testCW(sensor_align_e rotation, int32_t angle) { - int32_t src[XYZ_AXIS_COUNT]; - int32_t test[XYZ_AXIS_COUNT]; + float src[XYZ_AXIS_COUNT]; + float test[XYZ_AXIS_COUNT]; // unit vector along x-axis src[X] = 1; src[Y] = 0; src[Z] = 0; - int32_t matrix[3][3]; + float matrix[3][3]; initZAxisRotation(matrix, angle); rotateVector(matrix, src, test); @@ -155,15 +155,15 @@ static void testCW(sensor_align_e rotation, int32_t angle) */ static void testCWFlip(sensor_align_e rotation, int32_t angle) { - int32_t src[XYZ_AXIS_COUNT]; - int32_t test[XYZ_AXIS_COUNT]; + float src[XYZ_AXIS_COUNT]; + float test[XYZ_AXIS_COUNT]; // unit vector along x-axis src[X] = 1; src[Y] = 0; src[Z] = 0; - int32_t matrix[3][3]; + float matrix[3][3]; initYAxisRotation(matrix, 180); rotateVector(matrix, src, test); initZAxisRotation(matrix, angle); diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc.txt similarity index 100% rename from src/test/unit/flight_imu_unittest.cc rename to src/test/unit/flight_imu_unittest.cc.txt diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc new file mode 100644 index 0000000000..bc20b34025 --- /dev/null +++ b/src/test/unit/gps_ublox_unittest.cc @@ -0,0 +1,90 @@ +/* + * This file is part of INAV. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + + +#include "gtest/gtest.h" +#include "unittest_macros.h" + +#include +#include +#include + +#include "io/gps_ublox_utils.h" + +void dumpCfg(const ubx_config_data8_t *cfg, int valuesAdded) +{ + printf("%02x %02x %02x %02x %04x\n", cfg->header.preamble1, cfg->header.preamble2, cfg->header.msg_class, cfg->header.msg_id, cfg->header.length); + + printf("%02x %02x %02x %02x\n", cfg->configHeader.version, cfg->configHeader.layers, cfg->configHeader.transaction, cfg->configHeader.reserved); + + for(int i =0; i < valuesAdded; ++i) { + printf("%i: %08x %02x\n", i+1, cfg->data.payload[i].key, cfg->data.payload[i].value); + } + + uint8_t *chksums = (uint8_t *)&cfg->data.payload[valuesAdded]; + + printf("%02x %02x\n", chksums[0], chksums[1]); +} + +void dumpMemory(uint8_t *mem, int size) +{ + for(int i =0; i < size; ++i) { + printf("%02x ", mem[i]); + } + printf("\n"); +} + +TEST(GPSUbloxTest, TestUbloxCfgFillBytes) +{ + ubx_config_data8_t cfg = {}; + ubx_config_data8_payload_t kvPairs[] = { + { 0x10310025, 0x1}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69} + }; + + int valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 12); + + EXPECT_TRUE(valuesAdded == 12); + + dumpCfg(&cfg, valuesAdded); + + valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 1); + EXPECT_TRUE(1 == valuesAdded); + + // Set glonass enabled, from u-center 2 + const uint8_t expected[] = {0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x25, 0x00, 0x31, 0x10, 0x01, 0x02, 0xA7}; + EXPECT_FALSE(memcmp((void *)expected, (void *)&cfg, 17)); + + printf("Expected:\n"); + dumpMemory((uint8_t *)expected, 17); + printf("Actual:\n"); + dumpMemory((uint8_t *)&cfg, 17); + + // osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); + // std::cout << "'" << buf << "'" << std::endl; + //EXPECT_FALSE(strcmp(buf, " 123.45")); +} \ No newline at end of file diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc.txt similarity index 100% rename from src/test/unit/sensor_gyro_unittest.cc rename to src/test/unit/sensor_gyro_unittest.cc.txt diff --git a/src/test/unit/target.h b/src/test/unit/target.h index b95e1678c7..ac7617a63e 100755 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -17,6 +17,7 @@ #pragma once +#define SITL_BUILD #define USE_MAG #define USE_BARO #define USE_GPS diff --git a/src/utils/settings.rb b/src/utils/settings.rb index f10248cce2..bbc76f9ebd 100644 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -627,7 +627,7 @@ class Generator enc = @value_encoder.encode_values(min, max) buf << ", .config.minmax.indexes = #{enc}" end - buf << ", offsetof(#{group["type"]}, #{member["field"]}) },\n" + buf << ", (setting_offset_t)offsetof(#{group["type"]}, #{member["field"]}) },\n" end buf << "};\n" @@ -1002,7 +1002,7 @@ class Generator typ = "uint32_t" when "float" typ = "float" - when /^char \[(\d+)\]/ + when /^char\s*\[(\d+)\]/ # Substract 1 to show the maximum string size without the null terminator member["max"] = $1.to_i - 1; typ = "string"