Skip to content
Permalink

Comparing changes

This is a direct comparison between two commits made in this repository or its related repositories. View the default comparison for this range or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: tiiuae/px4-firmware
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: f463d3dc6532de1e00ecb591c3d3c47336b780c3
Choose a base ref
..
head repository: tiiuae/px4-firmware
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: 49c46c7b6fc5e0099401b650b8fb2b3ce3fda49e
Choose a head ref
Showing with 993 additions and 391 deletions.
  1. +3 −3 .github/workflows/tiiuae-coverity-scan-image.yaml
  2. +1 −1 .github/workflows/tiiuae-coverity-scan.yaml
  3. +164 −18 .github/workflows/tiiuae-pixhawk-and-saluki.yaml
  4. +3 −3 .github/workflows/tiiuae-sitl.yaml
  5. +2 −0 ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt
  6. +42 −29 ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x
  7. +12 −3 ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
  8. +1 −1 ROMFS/px4fmu_common/init.d-posix/rcS
  9. +10 −0 ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server
  10. +10 −0 ROMFS/px4fmu_common/init.d-posix/rcS.sim_udp
  11. +15 −10 ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor
  12. +4 −0 Tools/px_uploader.Dockerfile
  13. +1 −1 Tools/saluki-sec-scripts
  14. +1 −1 boards/ssrc/common
  15. +1 −1 boards/ssrc/saluki-pi
  16. +1 −1 boards/ssrc/saluki-v2
  17. +1 −1 boards/ssrc/saluki-v3
  18. +2 −0 msg/CMakeLists.txt
  19. +4 −0 msg/Guid.msg
  20. +4 −0 msg/HwInfo.msg
  21. +2 −0 msg/SystemVersion.msg
  22. +2 −0 packaging/entrypoint.sh
  23. +1 −1 platforms/common/uORB/IndexedStack.hpp
  24. +25 −54 platforms/common/uORB/SubscriptionCallback.hpp
  25. +43 −33 platforms/common/uORB/uORBDeviceNode.cpp
  26. +30 −14 platforms/common/uORB/uORBDeviceNode.hpp
  27. +105 −7 platforms/common/uORB/uORBManager.cpp
  28. +23 −51 platforms/common/uORB/uORBManager.hpp
  29. +1 −1 platforms/nuttx/NuttX/extern/pf_crypto
  30. +1 −1 platforms/nuttx/NuttX/nuttx
  31. +1 −0 platforms/nuttx/src/px4/common/CMakeLists.txt
  32. +47 −3 platforms/nuttx/src/px4/common/console_buffer.cpp
  33. +36 −8 platforms/nuttx/src/px4/common/console_buffer_usr.cpp
  34. +42 −12 platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c
  35. +6 −0 platforms/nuttx/src/px4/common/px4_init.cpp
  36. +6 −0 platforms/nuttx/src/px4/common/px4_userspace_init.cpp
  37. +107 −15 platforms/nuttx/src/px4/common/usr_mcu_version.cpp
  38. +1 −1 platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/device_info.h
  39. +32 −1 platforms/nuttx/src/px4/microchip/mpfs/version/board_mcu_version.c
  40. +8 −2 platforms/nuttx/toc/CMakeLists.txt
  41. +1 −0 src/include/image_toc.h
  42. +165 −100 src/lib/perf/perf_counter.cpp
  43. +2 −1 src/modules/mavlink/mavlink_ulog.cpp
  44. +24 −13 src/systemcmds/uorb/uORBDeviceMaster.cpp
6 changes: 3 additions & 3 deletions .github/workflows/tiiuae-coverity-scan-image.yaml
Original file line number Diff line number Diff line change
@@ -20,20 +20,20 @@ jobs:
fetch-depth: 0
- name: Docker meta
id: meta
uses: docker/metadata-action@v4
uses: docker/metadata-action@v5
with:
images: ghcr.io/tiiuae/px4-coverity-scan-image
tags: |
type=raw,value=latest
type=sha
- name: Login to GitHub Container Registry
uses: docker/login-action@v2
uses: docker/login-action@v3
with:
registry: ghcr.io
username: ${{ github.actor }}
password: ${{ secrets.GITHUB_TOKEN }}
- name: Build and push coverity scan image
uses: docker/build-push-action@v4
uses: docker/build-push-action@v5
with:
context: .
file: ./px4-firmware/packaging/Dockerfile.coverity
2 changes: 1 addition & 1 deletion .github/workflows/tiiuae-coverity-scan.yaml
Original file line number Diff line number Diff line change
@@ -18,7 +18,7 @@ jobs:
run: |
git submodule foreach --recursive git fetch --tags
- name: Login to GitHub Container Registry
uses: docker/login-action@v2
uses: docker/login-action@v3
with:
registry: ghcr.io
username: ${{ github.actor }}
182 changes: 164 additions & 18 deletions .github/workflows/tiiuae-pixhawk-and-saluki.yaml
Original file line number Diff line number Diff line change
@@ -15,22 +15,37 @@ on:
required: false
default: true
type: boolean
saluki-v2-manual-fpga-version:
description: 'saluki-v2 optional fpga version (e.g. sha-3b85ccc)'
required: false
default: ''
type: string
saluki-v3-manual-fpga-version:
description: 'saluki-v3 optional fpga version (e.g. sha-3b85ccc)'
required: false
default: ''
type: string
saluki-pi-manual-fpga-version:
description: 'saluki-pi optional fpga version (e.g. sha-cd7bb6b)'
required: false
default: ''
type: string

permissions:
contents: read
packages: write

env:
saluki_pi_fpga_version: "sha-d4ab4c3"
saluki_v2_fpga_version: "sha-d4ab4c3"
saluki_v3_fpga_version: "sha-d4ab4c3"
saluki_pi_fpga_version: "sha-b7e842d"
saluki_v2_fpga_version: "sha-b7e842d"
saluki_v3_fpga_version: "sha-b7e842d"

jobs:
fc_matrix:
strategy:
fail-fast: false
matrix:
product: [pixhawk, saluki-v2_default, saluki-v2_amp, saluki-v2_protected, saluki-v2_kernel, saluki-pi_default, saluki-pi_amp, saluki-pi_protected, saluki-v3_default, saluki-v3_amp]
product: [pixhawk, saluki-v2_default, saluki-v2_amp, saluki-v2_kernel, saluki-pi_default, saluki-pi_amp, saluki-v3_default, saluki-v3_amp]
include:
- product: saluki-v2_custom_keys
keys: Tools/saluki-sec-scripts/custom_keys/saluki-v2/px4_bin_ed25519_private.pem
@@ -47,6 +62,45 @@ jobs:
enabled: ${{ github.event.pull_request.head.repo.full_name == github.repository || github.event_name == 'push' || github.event_name == 'workflow_dispatch' }}
secrets: inherit

variables:
runs-on: ubuntu-latest
outputs:
jfrog_upload: ${{ steps.variables.outputs.jfrog_upload }}
saluki_v2_fpga_version: ${{ steps.variables.outputs.saluki_v2_fpga_version }}
saluki_v3_fpga_version: ${{ steps.variables.outputs.saluki_v3_fpga_version }}
saluki_pi_fpga_version: ${{ steps.variables.outputs.saluki_pi_fpga_version }}
steps:
- name: Print input variables
id: variables
run: |
# use saluki-v2-fpga default version if custom is not provided
saluki_v2_fpga_version=${{ env.saluki_v2_fpga_version }}
if [ -n "${{ github.event.inputs.saluki-v2-manual-fpga-version }}" ]; then
saluki_v2_fpga_version=${{ github.event.inputs.saluki-v2-manual-fpga-version }}
fi
# use saluki-v3-fpga default version if custom is not provided
saluki_v3_fpga_version=${{ env.saluki_v3_fpga_version }}
if [ -n "${{ github.event.inputs.saluki-v3-manual-fpga-version }}" ]; then
saluki_v3_fpga_version=${{ github.event.inputs.saluki-v3-manual-fpga-version }}
fi
# use saluki-pi-fpga default version if custom is not provided
saluki_pi_fpga_version=${{ env.saluki_pi_fpga_version }}
if [ -n "${{ github.event.inputs.saluki-pi-manual-fpga-version }}" ]; then
saluki_pi_fpga_version=${{ github.event.inputs.saluki-pi-manual-fpga-version }}
fi
echo "saluki_v2_fpga_version=${saluki_v2_fpga_version}" >> $GITHUB_OUTPUT
echo "saluki_v3_fpga_version=${saluki_v3_fpga_version}" >> $GITHUB_OUTPUT
echo "saluki_pi_fpga_version=${saluki_pi_fpga_version}" >> $GITHUB_OUTPUT
echo "jfrog_upload=${{ github.event.inputs.jfrog-upload }}" >> $GITHUB_OUTPUT
echo "saluki_v2_fpga_version: ${saluki_v2_fpga_version}"
echo "saluki_v3_fpga_version: ${saluki_v3_fpga_version}"
echo "saluki_pi_fpga_version: ${saluki_pi_fpga_version}"
echo "jfrog_upload: ${{ github.event.inputs.jfrog-upload }}"
px4fwupdater:
name: build px4fwupdater
runs-on: ubuntu-latest
@@ -83,6 +137,7 @@ jobs:
runs-on: ubuntu-latest
needs:
- px4fwupdater
- variables
steps:
- name: Checkout px4-firmware
uses: actions/checkout@v4
@@ -96,7 +151,7 @@ jobs:
path: bin
- name: Firmware flasher - Container metadata
id: containermeta # referenced from later step
uses: docker/metadata-action@v4
uses: docker/metadata-action@v5
with:
images: ghcr.io/tiiuae/px4-firmware
tags: |
@@ -105,29 +160,119 @@ jobs:
type=semver,pattern={{version}}
type=sha
- name: Login to GitHub Container Registry
uses: docker/login-action@v2
uses: docker/login-action@v3
with:
registry: ghcr.io
username: ${{ github.actor }}
password: ${{ secrets.GITHUB_TOKEN }}
- name: Firmware flasher - Build and push
uses: docker/build-push-action@v3
uses: docker/build-push-action@v5
with:
push: true
context: .
file: px4-firmware/Tools/px_uploader.Dockerfile
tags: ${{ steps.containermeta.outputs.tags }}
labels: ${{ steps.containermeta.outputs.labels }}
build-args: |
"saluki_pi_fpga_version=${{ env.saluki_pi_fpga_version }}"
"saluki_v2_fpga_version=${{ env.saluki_v2_fpga_version }}"
"saluki_v3_fpga_version=${{ env.saluki_v3_fpga_version }}"
"saluki_pi_fpga_version=${{ needs.variables.outputs.saluki_pi_fpga_version }}"
"saluki_v2_fpga_version=${{ needs.variables.outputs.saluki_v2_fpga_version }}"
"saluki_v3_fpga_version=${{ needs.variables.outputs.saluki_v3_fpga_version }}"
- name: Build overview
run: |
echo "### Build overview:" >> $GITHUB_STEP_SUMMARY
echo "Build version: ${{ steps.containermeta.outputs.tags }}"
echo "Build labels: ${{ steps.containermeta.outputs.labels }}"
echo "Build args:"
echo " saluki_pi_fpga_version: ${{ needs.variables.outputs.saluki_pi_fpga_version }}"
echo " saluki_v2_fpga_version: ${{ needs.variables.outputs.saluki_v2_fpga_version }}"
echo " saluki_v3_fpga_version: ${{ needs.variables.outputs.saluki_v3_fpga_version }}"
# in case more than one tag is generated, use the one which mentions commit sha
if (( $(echo "${{ steps.containermeta.outputs.tags }}" | wc -l) > 1 )); then
container_name=$(echo "${{ steps.containermeta.outputs.tags }}" | grep ':sha-' | head -n 1)
else
container_name="${{ steps.containermeta.outputs.tags }}"
fi
echo "Container name: $container_name"
# display mermaid flowchart
echo '```mermaid' >> $GITHUB_STEP_SUMMARY
echo "flowchart LR" >> $GITHUB_STEP_SUMMARY
# inputs
echo "FPGA-V2[(Saluki-v2 FPGA\n${{ needs.variables.outputs.saluki_v2_fpga_version }})]" >> $GITHUB_STEP_SUMMARY
echo "FPGA-V3[(Saluki-v3 FPGA\n${{ needs.variables.outputs.saluki_v3_fpga_version }})]" >> $GITHUB_STEP_SUMMARY
echo "FPGA-PI[(Saluki-pi FPGA\n${{ needs.variables.outputs.saluki_pi_fpga_version }})]" >> $GITHUB_STEP_SUMMARY
echo "PX4-SHA[${{ github.repository }}\n$GITHUB_REF]" >> $GITHUB_STEP_SUMMARY
# build
echo "BUILD[build PX4 fwupdater]" >> $GITHUB_STEP_SUMMARY
# outputs
echo "OUTPUT[(${container_name})]" >> $GITHUB_STEP_SUMMARY
# links
echo "FPGA-V2 --> BUILD" >> $GITHUB_STEP_SUMMARY
echo "FPGA-V3 --> BUILD" >> $GITHUB_STEP_SUMMARY
echo "FPGA-PI --> BUILD" >> $GITHUB_STEP_SUMMARY
echo "PX4-SHA --> BUILD" >> $GITHUB_STEP_SUMMARY
echo "BUILD --> OUTPUT" >> $GITHUB_STEP_SUMMARY
# end mermaid flowchart
echo '```' >> $GITHUB_STEP_SUMMARY
# set variables for container name and date
CONTAINER_DATE=$(date +%s)
PX4_TMP_CONTAINER_NAME="tmp_px4_container_$CONTAINER_DATE"
px4_github_sha=$(echo "${{ github.sha }}" | cut -c1-7)
echo "# Flashing this package to your Saluki" >> $GITHUB_STEP_SUMMARY
echo "## Get these px4 firmware files to your computer" >> $GITHUB_STEP_SUMMARY
echo "To get these files to you computer, you can use the following command" >> $GITHUB_STEP_SUMMARY
echo "The command will create a temporary \`$PX4_TMP_CONTAINER_NAME\` container, copy the firmware files to directory \`px4-firmware_${px4_github_sha}\` and remove the temporary container" >> $GITHUB_STEP_SUMMARY
# compose docker cp command
docker_cp_cmd='docker cp $(docker create --name '
docker_cp_cmd+="$PX4_TMP_CONTAINER_NAME $container_name"
docker_cp_cmd+='):/firmware '
docker_cp_cmd+=px4-firmware_${px4_github_sha}
docker_cp_cmd+=' && docker rm '
docker_cp_cmd+=$PX4_TMP_CONTAINER_NAME
# echo docker cp command to summary
echo '```shell' >> $GITHUB_STEP_SUMMARY
echo "${docker_cp_cmd}" >> $GITHUB_STEP_SUMMARY
echo '```' >> $GITHUB_STEP_SUMMARY
# This flashing is not currently working, just keeping it here for the future
# echo "## Flash this px4 firmware to your Saluki" >> $GITHUB_STEP_SUMMARY
# echo "To flash this firmware to your Saluki, you can use the following command:" >> $GITHUB_STEP_SUMMARY
# docker_flash_cmd='docker run --rm --network=host --entrypoint= --device=${dev}:/dev/px4serial '
# docker_flash_cmd+=$container_name
# docker_flash_cmd+=' sh -c "/bin/px_uploader.py ssrc_saluki-v2_default-*.px4"'
# echo '```shell' >> $GITHUB_STEP_SUMMARY
# echo "${docker_flash_cmd}" >> $GITHUB_STEP_SUMMARY
# echo '```' >> $GITHUB_STEP_SUMMARY
echo "## Flash this px4 firmware to your Saluki by using fpga-flashing tool" >> $GITHUB_STEP_SUMMARY
echo "fpga-flashing is separate tool: https://github.com/tiiuae/fpga-flashing/" >> $GITHUB_STEP_SUMMARY
echo "To flash this px4 firmware and FPGA to your Saluki with fpga-flashing, you can use the following command:" >> $GITHUB_STEP_SUMMARY
flash_tool_cmd='./flash.sh --update-package '
flash_tool_cmd+=$container_name
flash_tool_cmd+=' --fpga --px4'
echo '```shell' >> $GITHUB_STEP_SUMMARY
echo "${flash_tool_cmd}" >> $GITHUB_STEP_SUMMARY
echo '```' >> $GITHUB_STEP_SUMMARY
echo 'For more info please see: https://github.com/tiiuae/fpga-flashing/#usage' >> $GITHUB_STEP_SUMMARY
upload-px4fwupdater-uae:
name: upload px4fwupdater to UAE docker registry
# temporarily disabled until we get new token from UAR
if: false
runs-on: ubuntu-latest
needs:
- px4fwupdater
- variables
steps:
- name: Checkout px4-firmware
uses: actions/checkout@v4
@@ -141,7 +286,7 @@ jobs:
path: bin
- name: Firmware flasher - Container metadata
id: containermeta # referenced from later step
uses: docker/metadata-action@v4
uses: docker/metadata-action@v5
with:
images: artifactory.ssrcdevops.tii.ae/tiiuae/px4-firmware
tags: |
@@ -150,30 +295,30 @@ jobs:
type=semver,pattern={{version}}
type=sha
- name: Login to SSRC JFrog Container Registry
uses: docker/login-action@v2
uses: docker/login-action@v3
with:
registry: artifactory.ssrcdevops.tii.ae
username: ${{ secrets.UAE_RT_USER }}
password: ${{ secrets.UAE_RT_APIKEY }}
# have to login to ghcr as well to download fpga and BL
- name: Login to GitHub Container Registry
uses: docker/login-action@v2
uses: docker/login-action@v3
with:
registry: ghcr.io
username: ${{ github.actor }}
password: ${{ secrets.GITHUB_TOKEN }}
- name: Firmware flasher - Build and push
uses: docker/build-push-action@v3
uses: docker/build-push-action@v5
with:
push: true
context: .
file: px4-firmware/Tools/px_uploader.Dockerfile
tags: ${{ steps.containermeta.outputs.tags }}
labels: ${{ steps.containermeta.outputs.labels }}
build-args: |
"saluki_pi_fpga_version=${{ env.saluki_pi_fpga_version }}"
"saluki_v2_fpga_version=${{ env.saluki_v2_fpga_version }}"
"saluki_v3_fpga_version=${{ env.saluki_v3_fpga_version }}"
"saluki_pi_fpga_version=${{ needs.variables.outputs.saluki_pi_fpga_version }}"
"saluki_v2_fpga_version=${{ needs.variables.outputs.saluki_v2_fpga_version }}"
"saluki_v3_fpga_version=${{ needs.variables.outputs.saluki_v3_fpga_version }}"
artifactory:
name: upload builds to artifactory
@@ -256,7 +401,8 @@ jobs:
artifactory-uae:
name: upload builds to UAE artifactory
if: ${{ github.event_name != 'workflow_dispatch' || inputs.jfrog-upload == true }}
# temporarily disabled until we get new token from UAR
if: false
runs-on: ubuntu-latest
needs:
- px4fwupdater
6 changes: 3 additions & 3 deletions .github/workflows/tiiuae-sitl.yaml
Original file line number Diff line number Diff line change
@@ -44,7 +44,7 @@ jobs:
- name: Docker meta
id: meta
uses: docker/metadata-action@v4
uses: docker/metadata-action@v5
with:
images: ghcr.io/tiiuae/tii-px4-sitl
tags: |
@@ -55,14 +55,14 @@ jobs:
type=raw,value=latest
- name: Login to GitHub Container Registry
uses: docker/login-action@v2
uses: docker/login-action@v3
with:
registry: ghcr.io
username: ${{ github.actor }}
password: ${{ secrets.GITHUB_TOKEN }}

- name: Build tii-px4-sitl image and push
uses: docker/build-push-action@v3
uses: docker/build-push-action@v5
with:
context: .
file: ./px4-firmware/packaging/Dockerfile.sitl
2 changes: 2 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -38,5 +38,7 @@ px4_add_romfs_files(
px4-rc.params
px4-rc.simulator
rc.replay
rcS.sim_tcp_server
rcS.sim_udp
rcS
)
71 changes: 42 additions & 29 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x
Original file line number Diff line number Diff line change
@@ -13,32 +13,45 @@

. ${R}etc/init.d/rc.mc_defaults

set MIXER quad_x
set PWM_OUT 1234

if [ $AUTOCNF = yes ]
then
param set IMU_GYRO_CUTOFF 60
param set IMU_DGYRO_CUTOFF 30
param set MC_ROLLRATE_P 0.14
param set MC_PITCHRATE_P 0.14
param set MC_ROLLRATE_I 0.3
param set MC_PITCHRATE_I 0.3
param set MC_ROLLRATE_D 0.004
param set MC_PITCHRATE_D 0.004

param set BAT_N_CELLS 4

param set MAV_0_CONFIG 0
param set RTPS_MAV_CONFIG 101
param set SER_TEL1_BAUD 460800

# Disable Multi-EKF
param set EKF2_MULTI_IMU 0
param set SENS_IMU_MODE 1
param set EKF2_MULTI_MAG 0
param set SENS_MAG_MODE 1

# Logger used only while flying
param set SDLOG_MODE 0
fi
param set IMU_GYRO_CUTOFF 60
param set IMU_DGYRO_CUTOFF 30
param set MC_ROLLRATE_P 0.14
param set MC_PITCHRATE_P 0.14
param set MC_ROLLRATE_I 0.3
param set MC_PITCHRATE_I 0.3
param set MC_ROLLRATE_D 0.004
param set MC_PITCHRATE_D 0.004

param set BAT_N_CELLS 4

# Control allocator parameters
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 0.175
param set-default CA_ROTOR0_PY 0.175
param set-default CA_ROTOR1_PX -0.175
param set-default CA_ROTOR1_PY -0.175
param set-default CA_ROTOR2_PX 0.175
param set-default CA_ROTOR2_PY -0.175
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.175
param set-default CA_ROTOR3_PY 0.175
param set-default CA_ROTOR3_KM -0.05


param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104

param set MAV_0_CONFIG 0
param set RTPS_MAV_CONFIG 101
param set SER_TEL1_BAUD 3000000

# Disable Multi-EKF
param set EKF2_MULTI_IMU 0
param set SENS_IMU_MODE 1
param set EKF2_MULTI_MAG 0
param set SENS_MAG_MODE 1

# Logger used only while flying
param set SDLOG_MODE 0
15 changes: 12 additions & 3 deletions ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
Original file line number Diff line number Diff line change
@@ -36,7 +36,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
exit 1
fi

elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then

# allow starting of gz sim optionally
if [ "$(param show -q SIM_GZ_RUN_GZSIM)" -eq "1" ];
@@ -234,15 +234,24 @@ elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)"
else
# otherwise start simulator (mavlink) module
simulator_tcp_port=$((4560+px4_instance))
simulator_udp_port=$((14560+px4_instance))

# Check if PX4_SIM_HOSTNAME environment variable is empty
# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
# If both are empty use localhost for simulator
if [ -z "${PX4_SIM_HOSTNAME}" ]; then

if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
echo "INFO [init] PX4_SIM_HOSTNAME: localhost"
simulator_mavlink start -c $simulator_tcp_port
if [ ! -z "${PX4_SIM_USE_TCP_SERVER}" ]; then
echo "INFO [init] PX4_SIM_USE_TCP_SERVER"
simulator_mavlink start -s $simulator_tcp_port
elif [ ! -z "${PX4_SIM_USE_UDP}" ]; then
echo "INFO [init] PX4_SIM_USE_UDP"
simulator_mavlink start -u $simulator_udp_port
else
echo "INFO [init] PX4_SIM_HOSTNAME: localhost"
simulator_mavlink start -c $simulator_tcp_port
fi
else
echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}"
simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}"
2 changes: 1 addition & 1 deletion ROMFS/px4fmu_common/init.d-posix/rcS
Original file line number Diff line number Diff line change
@@ -29,7 +29,7 @@ fi
set VEHICLE_TYPE none
set LOGGER_ARGS ""
set LOGGER_BUF 1000

set SDCARD_AVAILABLE no
set RUN_MINIMAL_SHELL no

set SYS_AUTOSTART=0
10 changes: 10 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
#!/bin/sh

# search path for sourcing rcS
PATH="$PATH:${R}etc/init.d-posix"

# Define TCP server
export PX4_SIM_USE_TCP_SERVER=1

# Continue to the main startup script
. rcS
10 changes: 10 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/rcS.sim_udp
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
#!/bin/sh

# search path for sourcing rcS
PATH="$PATH:${R}etc/init.d-posix"

# Define UDP
export PX4_SIM_USE_UDP=1

# Continue to the main startup script
. rcS
25 changes: 15 additions & 10 deletions ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor
Original file line number Diff line number Diff line change
@@ -14,18 +14,23 @@
. /etc/init.d/rc.mc_defaults

# Default rates
param set-default IMU_GYRO_CUTOFF 60
param set-default IMU_DGYRO_CUTOFF 30
param set-default IMU_GYRO_CUTOFF 30
param set-default IMU_DGYRO_CUTOFF 15
param set-default IMU_GYRO_RATEMAX 800
param set-default MC_ROLLRATE_P 0.14
param set-default MC_PITCHRATE_P 0.14
param set-default MC_ROLLRATE_I 0.3
param set-default MC_PITCHRATE_I 0.3
param set-default MC_ROLLRATE_D 0.004
param set-default MC_PITCHRATE_D 0.004

# Master gain parameters
param set-default MC_ROLLRATE_K 0.4
param set-default MC_PITCHRATE_K 0.4
param set-default MC_ROLLRATE_I 0.350
param set-default MC_PITCHRATE_I 0.300
param set-default MC_ROLLRATE_D 0.0052
param set-default MC_PITCHRATE_D 0.0048
param set-default MC_PITCHRATE_K 1.10
param set-default MC_PITCH_P 6.50
param set-default MC_ROLLRATE_K 1.15
param set-default MC_ROLL_P 6.00
param set-default MC_YAWRATE_I 0.24
param set-default MC_YAWRATE_K 1.75
param set-default MC_YAWRATE_P 0.20
param set-default MC_YAW_P 3.50

# Control allocator parameters
param set-default CA_ROTOR_COUNT 4
4 changes: 4 additions & 0 deletions Tools/px_uploader.Dockerfile
Original file line number Diff line number Diff line change
@@ -5,6 +5,7 @@ ARG saluki_v3_fpga_version
FROM ghcr.io/tiiuae/saluki-pi-fpga:$saluki_pi_fpga_version AS SALUKI_PI
FROM ghcr.io/tiiuae/saluki-pi-fpga:$saluki_v2_fpga_version AS SALUKI_V2
FROM ghcr.io/tiiuae/saluki-pi-fpga:$saluki_v3_fpga_version AS SALUKI_V3
FROM ghcr.io/tiiuae/saluki_bootloader_v2:master AS SALUKI_BOOTLOADER_v2

FROM python:alpine3.14

@@ -25,9 +26,12 @@ FROM python:alpine3.14
# ("/" above is relative to GH action runner home dir)
# (see .github/workflows/tiiuae-pixhawk.yaml)

# copy fpga files from separate saluki containers
COPY --from=SALUKI_PI /firmware/saluki_pi-fpga /firmware/fpga/saluki_pi
COPY --from=SALUKI_V2 /firmware/saluki_v2-fpga /firmware/fpga/saluki_v2
COPY --from=SALUKI_V3 /firmware/saluki_v3-fpga /firmware/fpga/saluki_v3
# copy px_uploader.py from saluki_bootloader_v2 container
COPY --from=SALUKI_BOOTLOADER_v2 /firmware/bootloader_v2/px_uploader.py /firmware/px_uploader.py

WORKDIR /firmware

2 changes: 1 addition & 1 deletion Tools/saluki-sec-scripts
2 changes: 1 addition & 1 deletion boards/ssrc/common
2 changes: 1 addition & 1 deletion boards/ssrc/saluki-pi
2 changes: 1 addition & 1 deletion boards/ssrc/saluki-v2
2 changes: 1 addition & 1 deletion boards/ssrc/saluki-v3
2 changes: 2 additions & 0 deletions msg/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -103,10 +103,12 @@ set(msg_files
GpsInjectData.msg
GimbalControls.msg
Gripper.msg
Guid.msg
HealthReport.msg
HeaterStatus.msg
HomePosition.msg
HoverThrustEstimate.msg
HwInfo.msg
InputRc.msg
InternalCombustionEngineStatus.msg
IridiumsbdStatus.msg
4 changes: 4 additions & 0 deletions msg/Guid.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
# Guid (serial number)

uint64 timestamp # time since system start (microseconds)
uint8[32] mfguid # guid
4 changes: 4 additions & 0 deletions msg/HwInfo.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
# Hardware info string

uint64 timestamp # time since system start (microseconds)
char[64] hw_info # main autopilot hw info string
2 changes: 2 additions & 0 deletions msg/SystemVersion.msg
Original file line number Diff line number Diff line change
@@ -2,6 +2,8 @@

uint64 timestamp # time since system start (microseconds)
uint64 hw_version # main autopilot hw version
uint64 hw_revision # main autopilot hw revision
uint64 soc_arch_id # main autopilot soc architecture identifier
uint64 sw_version # main autopilot sw version
uint64 os_version # OS or middleware version
uint64 bl_version # bootloader version
2 changes: 2 additions & 0 deletions packaging/entrypoint.sh
Original file line number Diff line number Diff line change
@@ -4,6 +4,8 @@ source /opt/ros/humble/setup.bash

if [ "$1" == "sim_tcp_server" ] || [ "$PX4_SIM_USE_TCP_SERVER" != "" ]; then
px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS.sim_tcp_server
elif [ "$1" == "sim_udp" ] || [ "$PX4_SIM_USE_UDP" != "" ]; then
px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS.sim_udp
else
px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS
fi
2 changes: 1 addition & 1 deletion platforms/common/uORB/IndexedStack.hpp
Original file line number Diff line number Diff line change
@@ -67,7 +67,7 @@ class IndexedStack

if (!handle_valid(handle) ||
!handle_valid(p)) {
return r;
return false;
}

if (p == handle) {
79 changes: 25 additions & 54 deletions platforms/common/uORB/SubscriptionCallback.hpp
Original file line number Diff line number Diff line change
@@ -42,19 +42,14 @@
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_platform_common/posix.h>

#ifdef CONFIG_BUILD_FLAT
#define CB_LOCK()
#define CB_UNLOCK()
#else
#define CB_LOCK() lock()
#define CB_UNLOCK() unlock()
#endif

namespace uORB
{

// Subscription wrapper class with callbacks on new publications
class SubscriptionCallback : public SubscriptionInterval
#ifndef CONFIG_BUILD_FLAT
, public ListNode<SubscriptionCallback *>
#endif
{
public:
/**
@@ -67,52 +62,34 @@ class SubscriptionCallback : public SubscriptionInterval
SubscriptionCallback(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
SubscriptionInterval(meta, interval_us, instance)
{
#ifndef CONFIG_BUILD_FLAT
px4_sem_init(&_lock, 1, 1);
#endif
}

virtual ~SubscriptionCallback()
{
unregisterCallback();
#ifndef CONFIG_BUILD_FLAT
px4_sem_destroy(&_lock);
#endif
};

bool registerCallback()
{
CB_LOCK();

if (!registered()) {
if (!orb_advert_valid(_subscription.get_node())) {
// force topic creation
if (!_subscription.subscribe(true)) {
CB_UNLOCK();
return false;
}
if (!orb_advert_valid(_subscription.get_node())) {
// force topic creation
if (!_subscription.subscribe(true)) {
return false;
}
}

if (orb_advert_valid(_subscription.get_node())) {
_cb_handle = DeviceNode::register_callback(_subscription.get_node(), this, -1, _last_update, _interval_us);
}
bool ret = false;

if (orb_advert_valid(_subscription.get_node())) {
ret = Manager::registerCallback(_subscription.get_node(), this, _last_update, _interval_us, _cb_handle);
}

CB_UNLOCK();
return registered();
return ret;
}

void unregisterCallback()
{
CB_LOCK();

if (registered()) {
uorb_cb_handle_t handle = _cb_handle;
_cb_handle = UORB_INVALID_CB_HANDLE;
DeviceNode::unregister_callback(_subscription.get_node(), handle);
}

CB_UNLOCK();
Manager::unregisterCallback(_subscription.get_node(), this, _cb_handle);
}

/**
@@ -148,28 +125,21 @@ class SubscriptionCallback : public SubscriptionInterval

virtual void call() = 0;

void do_call()
#ifndef CONFIG_BUILD_FLAT
bool do_call()
{
CB_LOCK();
bool dequeued = DeviceNode::cb_dequeue(_subscription.get_node(), _cb_handle);

// Run the callback if it is still valid
if (registered()) {
if (dequeued) {
call();
}

CB_UNLOCK();
return dequeued;
}
#endif

bool registered() const { return uorb_cb_handle_valid(_cb_handle); }

private:
#ifndef CONFIG_BUILD_FLAT
/* Make sure the callback remains valid during callback execution */

void lock() { do {} while (px4_sem_wait(&_lock) != 0); }
void unlock() { px4_sem_post(&_lock); }
px4_sem_t _lock; /* Lock to protect registered callback */
#endif
protected:

uorb_cb_handle_t _cb_handle{UORB_INVALID_CB_HANDLE};
@@ -240,14 +210,15 @@ class SubscriptionPollable : public SubscriptionInterval

void registerPoll(int8_t lock_idx)
{
_cb_handle = DeviceNode::register_callback(_subscription.get_node(), nullptr, lock_idx, _last_update, _interval_us);
DeviceNode::register_callback(_subscription.get_node(), nullptr, lock_idx, _last_update, _interval_us, _cb_handle);
}

void unregisterPoll()
{
uorb_cb_handle_t handle = _cb_handle;
_cb_handle = UORB_INVALID_CB_HANDLE;
DeviceNode::unregister_callback(_subscription.get_node(), handle);
// Calling this while a poll is not registered is a no-op
if (uorb_cb_handle_valid(_cb_handle)) {
DeviceNode::unregister_callback(_subscription.get_node(), _cb_handle);
}
}
private:
uorb_cb_handle_t _cb_handle{UORB_INVALID_CB_HANDLE};
76 changes: 43 additions & 33 deletions platforms/common/uORB/uORBDeviceNode.cpp
Original file line number Diff line number Diff line change
@@ -599,32 +599,38 @@ uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert
/* Mark at least one data has been published */
_data_valid = true;

uORB::DeviceNode *n = node(handle);
IndexedStackHandle<CB_LIST_T> callbacks(n->_callbacks);
IndexedStackHandle<CB_LIST_T> callbacks(_callbacks);

uorb_cb_handle_t cb = callbacks.head();

while (callbacks.handle_valid(cb)) {
EventWaitItem *item = callbacks.peek(cb);

if (item->interval_us == 0 || hrt_elapsed_time(&item->last_update) >= item->interval_us) {
if (item->subscriber != nullptr) {

#ifdef CONFIG_BUILD_FLAT

if (item->subscriber != nullptr) {
// execute callback
item->subscriber->call();
#else
Manager::queueCallback(item->subscriber, item->lock);
#endif

} else {
// release poll
Manager::unlockThread(item->lock);
}

// Release poll waiters (and callback threads in non-flat builds)
if (item->lock != -1) {
if (Manager::isThreadAlive(item->lock)) {
Manager::unlockThread(item->lock);
#else

} else {
remove_cb = cb;
}
// Release poll waiters and callback threads
if (item->cb_triggered < CB_ALIVE_MAX_VALUE) {
__atomic_fetch_add(&item->cb_triggered, 1, __ATOMIC_SEQ_CST);
Manager::unlockThread(item->lock);

} else {
remove_cb = cb;
}

#endif
}

cb = callbacks.next(cb);
@@ -854,69 +860,73 @@ int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
return PX4_OK;
}

//TODO: make this a normal member function
uorb_cb_handle_t
bool
uORB::DeviceNode::_register_callback(uORB::SubscriptionCallback *cb_sub,
int8_t poll_lock, hrt_abstime last_update, uint32_t interval_us)
int8_t poll_lock, hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle)
{
#ifndef CONFIG_BUILD_FLAT
// Get the cb lock for this process from the Manager
int8_t cb_lock = poll_lock == -1 ? Manager::getCallbackLock() : poll_lock;
#else
int8_t cb_lock = poll_lock;
#endif

// TODO: Check for duplicate registrations?

IndexedStackHandle<CB_LIST_T> callbacks(_callbacks);

ATOMIC_ENTER;

uorb_cb_handle_t i = callbacks.pop_free();
EventWaitItem *item = callbacks.peek(i);
cb_handle = callbacks.pop_free();
EventWaitItem *item = callbacks.peek(cb_handle);

#ifdef CONFIG_BUILD_FLAT

if (!item) {
px4_leave_critical_section(flags);
item = new EventWaitItem;
flags = px4_enter_critical_section();
i = item;
cb_handle = item;
}

#endif

if (item != nullptr) {
item->lock = cb_lock;
#ifdef CONFIG_BUILD_FLAT
item->subscriber = cb_sub;
#else
item->cb_triggered = 0;
#endif
item->last_update = last_update;
item->interval_us = interval_us;

callbacks.push(i);
callbacks.push(cb_handle);

} else {
PX4_ERR("register fail\n");
}

ATOMIC_LEAVE;

return i;
return uorb_cb_handle_valid(cb_handle);
}

//TODO: make this a normal member function?
void
uORB::DeviceNode::_unregister_callback(uorb_cb_handle_t cb_handle)
int
uORB::DeviceNode::_unregister_callback(uorb_cb_handle_t &cb_handle)
{
IndexedStackHandle<CB_LIST_T> callbacks(_callbacks);
int ret = 0;

ATOMIC_ENTER;

if (!callbacks.rm(cb_handle)) {
bool ret_rm = callbacks.rm(cb_handle);

if (!ret_rm) {
PX4_ERR("unregister fail\n");

} else {
#ifndef CONFIG_BUILD_FLAT
EventWaitItem *item = callbacks.peek(cb_handle);
ret = item->cb_triggered;
#endif
callbacks.push_free(cb_handle);
cb_handle = UORB_INVALID_CB_HANDLE;
}

ATOMIC_LEAVE;

return ret;
}
44 changes: 30 additions & 14 deletions platforms/common/uORB/uORBDeviceNode.hpp
Original file line number Diff line number Diff line change
@@ -65,6 +65,7 @@ typedef void *uorb_cb_handle_t;
#define UORB_INVALID_CB_HANDLE -1
typedef int8_t uorb_cb_handle_t;
#define uorb_cb_handle_valid(x) ((x) >= 0)
#define CB_ALIVE_MAX_VALUE 50
#endif

#define CB_LIST_T struct EventWaitItem, uorb_cb_handle_t, MAX_EVENT_WAITERS
@@ -218,18 +219,15 @@ class DeviceNode
*/
bool copy(void *dst, orb_advert_t &handle, unsigned &generation);

static void orb_callback(int signo, siginfo_t *si_info, void *data);

static uorb_cb_handle_t register_callback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub,
int8_t poll_lock,
hrt_abstime last_update, uint32_t interval_us)
static bool register_callback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub, int8_t poll_lock,
hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle)
{
return node(node_handle)->_register_callback(callback_sub, poll_lock, last_update, interval_us);
return node(node_handle)->_register_callback(callback_sub, poll_lock, last_update, interval_us, cb_handle);
}

static void unregister_callback(orb_advert_t &node_handle, uorb_cb_handle_t cb_handle)
static int unregister_callback(orb_advert_t &node_handle, uorb_cb_handle_t &cb_handle)
{
node(node_handle)->_unregister_callback(cb_handle);
return node(node_handle)->_unregister_callback(cb_handle);
}

void *operator new (size_t, void *p)
@@ -243,6 +241,21 @@ class DeviceNode

const char *get_devname() const {return _devname;}

#ifndef CONFIG_BUILD_FLAT
static bool cb_dequeue(orb_advert_t &node_handle, uorb_cb_handle_t cb)
{
IndexedStackHandle<CB_LIST_T> callbacks(node(node_handle)->_callbacks);
EventWaitItem *item = callbacks.peek(cb);

if (__atomic_load_n(&item->cb_triggered, __ATOMIC_SEQ_CST) > 0) {
__atomic_fetch_sub(&item->cb_triggered, 1, __ATOMIC_SEQ_CST);
return true;
}

return false;
}
#endif

private:
friend uORBTest::UnitTest;

@@ -253,11 +266,15 @@ class DeviceNode
px4::atomic<unsigned> _generation{0}; /**< object generation count */

struct EventWaitItem {
class SubscriptionCallback *subscriber;
hrt_abstime last_update;
#ifdef CONFIG_BUILD_FLAT
class SubscriptionCallback *subscriber;
#else
uint32_t cb_triggered;
#endif
uint32_t interval_us;
int8_t lock;
uorb_cb_handle_t next; // List ptr
int8_t lock;
};

IndexedStack<CB_LIST_T> _callbacks;
@@ -328,10 +345,9 @@ class DeviceNode
inline static void *node_data(const orb_advert_t &handle) { return handle.data; }
#endif

uorb_cb_handle_t _register_callback(SubscriptionCallback *callback_sub,
int8_t poll_lock,
hrt_abstime last_update, uint32_t interval_us);
void _unregister_callback(uorb_cb_handle_t cb_handle);
bool _register_callback(SubscriptionCallback *callback_sub, int8_t poll_lock, hrt_abstime last_update,
uint32_t interval_us, uorb_cb_handle_t &cb_handle);
int _unregister_callback(uorb_cb_handle_t &cb_handle);

#ifdef CONFIG_BUILD_FLAT
char *_devname;
112 changes: 105 additions & 7 deletions platforms/common/uORB/uORBManager.cpp
Original file line number Diff line number Diff line change
@@ -43,6 +43,7 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>

#include "uORBDeviceNode.hpp"
#include "uORBUtils.hpp"
@@ -65,6 +66,10 @@ uORB::Manager *uORB::Manager::_Instance = nullptr;
#ifndef CONFIG_BUILD_FLAT
int8_t uORB::Manager::per_process_lock = -1;
pid_t uORB::Manager::per_process_cb_thread = -1;
List<class uORB::SubscriptionCallback *> uORB::Manager::per_process_cb_list;
px4_sem_t uORB::Manager::per_process_cb_list_mutex;

static int callback_count = 1;
#endif

void uORB::Manager::cleanup()
@@ -460,17 +465,26 @@ int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeo
int8_t
uORB::Manager::launchCallbackThread()
{
if (px4_sem_init(&per_process_cb_list_mutex, 1, 1) != 0) {
PX4_ERR("Can't initialize cb mutex");
return -1;
}

per_process_lock = Manager::getThreadLock();

if (per_process_lock < 0) {
PX4_ERR("Out of thread locks\n");
return -1;
}

/* Set the priority to 1 higher than the highest controller, which is always nav_and_controllers */

int priority = sched_get_priority_max(SCHED_FIFO) + px4::wq_configurations::nav_and_controllers.relative_priority + 1;

if (per_process_cb_thread == -1) {
per_process_cb_thread = px4_task_spawn_cmd("orb_callback",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 1,
priority,
PX4_STACK_ADJUSTED(1024),
callback_thread,
nullptr);
@@ -488,17 +502,29 @@ uORB::Manager::launchCallbackThread()
int
uORB::Manager::callback_thread(int argc, char *argv[])
{
int tmp = 1;

while (true) {
lockThread(per_process_lock);
/* Sleep here waiting for callbacks, lock as many times as it has been unlocked */

class SubscriptionCallback *sub = dequeueCallback(per_process_lock);
lockThread(per_process_lock, tmp);

// Pass nullptr to this thread to exit
if (sub == nullptr) {
break;
lock_cb_list();

callback_count -= tmp;

for (auto sub : per_process_cb_list) {
/* Just in cast the callback thread has been starved,
* run all the queued callbacks now
*/
while (sub->do_call()) {
callback_count++;
}
}

sub->do_call();
tmp = callback_count;

unlock_cb_list();
}

Manager::freeThreadLock(per_process_lock);
@@ -509,6 +535,78 @@ uORB::Manager::callback_thread(int argc, char *argv[])

#endif

bool
uORB::Manager::registerCallback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub,
hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle)
{
if (uorb_cb_handle_valid(cb_handle)) {
// double registration, ok
return true;
}

bool ret = false;
int8_t cb_lock = -1;
#ifndef CONFIG_BUILD_FLAT
cb_lock = getCallbackLock();

if (cb_lock >= 0) {
// The callback needs to be first added to the cb_list, before registering to the publisher.
// Otherwise there is a race condition where the cb may be triggered withouth it being handled
// by the callback thread. If the callback thread is on higher priority than this registration
// function, it may spin endlessly blocking this

lock_cb_list();
per_process_cb_list.add(callback_sub);
unlock_cb_list();
#endif
ret = uORB::DeviceNode::register_callback(node_handle, callback_sub, cb_lock, last_update,

interval_us, cb_handle);
#ifndef CONFIG_BUILD_FLAT
}

if (!ret) {
lock_cb_list();
per_process_cb_list.remove(callback_sub);
unlock_cb_list();
}

#endif

return ret;
}

void
uORB::Manager::unregisterCallback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub,
uorb_cb_handle_t &cb_handle)
{
if (!uorb_cb_handle_valid(cb_handle)) {
// not registered
return;
}

#ifndef CONFIG_BUILD_FLAT
lock_cb_list();

// Unregister the callback from the device node and retrieve amount of unhandled callback triggers
// The unregister from the node needs to be done callback_thread locked; otherwise we don't know
// if there are unhandled triggers left or not (due to a race between the callback thread and
// this unregistration function).

// Unregister_callback returns the number of callbacks that has been triggered but not yet handled
// by the callback thread. This may happen if the publisher is on higher or equal priority than this

callback_count += DeviceNode::unregister_callback(node_handle, cb_handle);

// Remove the callback from the list

per_process_cb_list.remove(callback_sub);

unlock_cb_list();
#else
DeviceNode::unregister_callback(node_handle, cb_handle);
#endif
}

void uORB::Manager::GlobalSemPool::init(void)
{
74 changes: 23 additions & 51 deletions platforms/common/uORB/uORBManager.hpp
Original file line number Diff line number Diff line change
@@ -43,14 +43,14 @@
#include <uORB/topics/uORBTopics.hpp> // For ORB_ID enum
#include <stdint.h>
#include <px4_platform_common/px4_config.h>
#include <containers/List.hpp>

#ifdef CONFIG_ORB_COMMUNICATOR
#include "ORBSet.hpp"
#include "uORBCommunicator.hpp"
#endif /* CONFIG_ORB_COMMUNICATOR */

#define NUM_GLOBAL_SEMS 40
#define SUB_ALIVE_SEM_MAX_VALUE 100

namespace uORB
{
@@ -370,15 +370,15 @@ class Manager
#ifndef CONFIG_BUILD_FLAT
static uint8_t getCallbackLock()
{
uint8_t cbLock;
int8_t ret = per_process_lock;

// TODO: think about if this needs protection, maybe not use the
// same lock as for node advertise/subscribe
if (ret < 0) {
_Instance->lock();
ret = per_process_lock >= 0 ? per_process_lock : launchCallbackThread();
_Instance->unlock();
}

_Instance->lock();
cbLock = per_process_lock >= 0 ? per_process_lock : launchCallbackThread();
_Instance->unlock();
return cbLock;
return ret;
}
#endif

@@ -430,9 +430,17 @@ class Manager
px4_munmap(p, sizeof(uORB::Manager));
}

static void lockThread(int idx)
static bool registerCallback(orb_advert_t &node_handle, class SubscriptionCallback *callback_sub,
hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle);

static void unregisterCallback(orb_advert_t &node_handle, class SubscriptionCallback *callback_sub,
uorb_cb_handle_t &cb_handle);

static void lockThread(int idx, unsigned count = 1)
{
_Instance->g_sem_pool.take(idx);
while (count--) {
_Instance->g_sem_pool.take(idx);
}
}

static void unlockThread(int idx)
@@ -444,33 +452,6 @@ class Manager

static int8_t getThreadLock() {return _Instance->g_sem_pool.reserve();}

static void queueCallback(class SubscriptionCallback *sub, int idx)
{
if (idx >= 0) {
_Instance->g_sem_pool.cb_lock(idx);
_Instance->g_sem_pool.cb_set(idx, sub);
// The manager is unlocked in callback thread
}
}

static class SubscriptionCallback *dequeueCallback(int idx)
{
class SubscriptionCallback *sub = nullptr;

if (idx >= 0) {
sub = _Instance->g_sem_pool.cb_get(idx);
_Instance->g_sem_pool.cb_unlock(idx);
}

return sub;
}

static bool isThreadAlive(int idx)
{
int value = _Instance->g_sem_pool.value(idx);
return value <= SUB_ALIVE_SEM_MAX_VALUE;
}

private: // class methods
inline static uORB::DeviceNode *node(orb_advert_t handle) {return static_cast<uORB::DeviceNode *>(handle.node);}

@@ -647,24 +628,16 @@ class Manager
void release(int8_t i) {_global_sem[i].release(); }
int value(int8_t i) { return _global_sem[i].value(); }

void cb_lock(int8_t i) { do {} while (_global_sem[i].cb_lock() != 0); }
void cb_unlock(int8_t i) { _global_sem[i].cb_unlock(); }
void cb_set(int8_t i, class SubscriptionCallback *callback_ptr) { _global_sem[i].cb_set(callback_ptr); }
class SubscriptionCallback *cb_get(int8_t i) { return _global_sem[i].cb_get(); }

class GlobalLock
{
public:
void init()
{
px4_sem_init(&_sem, 1, 0);
px4_sem_init(&_lock, 1, 1);
#if defined(__PX4_NUTTX)
sem_setprotocol(&_sem, SEM_PRIO_NONE);
sem_setprotocol(&_lock, SEM_PRIO_NONE);
#endif
in_use = false;
_callback_ptr = nullptr;
}
void free() { px4_sem_destroy(&_sem); }
int take() { return px4_sem_wait(&_sem); }
@@ -673,14 +646,8 @@ class Manager
int value() { int value; px4_sem_getvalue(&_sem, &value); return value; }
bool in_use{false};

int cb_lock() { return px4_sem_wait(&_lock); }
void cb_unlock() { px4_sem_post(&_lock); }
void cb_set(class SubscriptionCallback *callback_ptr) { _callback_ptr = callback_ptr; }
class SubscriptionCallback *cb_get() { return _callback_ptr; }
private:
class SubscriptionCallback *_callback_ptr {nullptr};
px4_sem_t _sem; /* For signaling to the callback thread */
px4_sem_t _lock; /* For signaling back from the callback thread */
};
private:

@@ -692,8 +659,13 @@ class Manager
} g_sem_pool;

#ifndef CONFIG_BUILD_FLAT
static void lock_cb_list() { do {} while (px4_sem_wait(&per_process_cb_list_mutex) != 0); }
static void unlock_cb_list() { px4_sem_post(&per_process_cb_list_mutex); }

static int8_t per_process_lock;
static pid_t per_process_cb_thread;
static List<class SubscriptionCallback *> per_process_cb_list;
static px4_sem_t per_process_cb_list_mutex;
#endif
};
} // namespace uORB
2 changes: 1 addition & 1 deletion platforms/nuttx/NuttX/extern/pf_crypto
2 changes: 1 addition & 1 deletion platforms/nuttx/NuttX/nuttx
Submodule nuttx updated 57 files
+7 −99 .github/workflows/build.yml
+8 −2 arch/arm/src/armv7-a/CMakeLists.txt
+1 −0 arch/arm/src/armv7-a/Make.defs
+75 −0 arch/arm/src/armv7-a/arm_addrenv_pgmap.c
+20 −0 arch/risc-v/src/common/riscv_addrenv_pgmap.c
+1 −26 arch/risc-v/src/common/riscv_pmp.c
+19 −6 arch/risc-v/src/mpfs/Kconfig
+8 −0 arch/risc-v/src/mpfs/Make.defs
+152 −0 arch/risc-v/src/mpfs/hardware/mpfs_mpucfg.h
+0 −32 arch/risc-v/src/mpfs/hardware/mpfs_sgmii.h
+9 −55 arch/risc-v/src/mpfs/mpfs_coremmc.c
+6 −6 arch/risc-v/src/mpfs/mpfs_coremmc.h
+16 −0 arch/risc-v/src/mpfs/mpfs_corespi.c
+17 −1 arch/risc-v/src/mpfs/mpfs_ddr.c
+48 −86 arch/risc-v/src/mpfs/mpfs_emmcsd.c
+6 −6 arch/risc-v/src/mpfs/mpfs_emmcsd.h
+4 −21 arch/risc-v/src/mpfs/mpfs_ethernet.c
+0 −2 arch/risc-v/src/mpfs/mpfs_ethernet.h
+169 −43 arch/risc-v/src/mpfs/mpfs_i2c.c
+13 −9 arch/risc-v/src/mpfs/mpfs_irq.c
+283 −0 arch/risc-v/src/mpfs/mpfs_mpu.c
+95 −0 arch/risc-v/src/mpfs/mpfs_mpu.h
+177 −0 arch/risc-v/src/mpfs/mpfs_sdio.c
+112 −0 arch/risc-v/src/mpfs/mpfs_sdio.h
+92 −0 arch/risc-v/src/mpfs/mpfs_sdio_dev.h
+4 −14 arch/risc-v/src/mpfs/mpfs_usb.c
+1 −1 boards/risc-v/mpfs/common/src/mpfs_emmcsd.c
+54 −10 drivers/net/Kconfig
+124 −0 drivers/net/ksz9477.c
+8 −0 drivers/net/ksz9477_reg.h
+3 −3 fs/fat/fs_fat32.c
+12 −12 fs/fat/fs_fat32util.c
+1 −13 fs/hostfs/CMakeLists.txt
+15 −5 fs/littlefs/CMakeLists.txt
+13 −2 fs/partition/CMakeLists.txt
+1 −0 fs/procfs/CMakeLists.txt
+2 −6 fs/rpmsgfs/CMakeLists.txt
+2 −2 fs/shm/CMakeLists.txt
+1 −1 fs/shm/Kconfig
+5 −0 fs/shm/shmfs.c
+10 −0 fs/shm/shmfs_alloc.c
+6 −0 fs/vfs/CMakeLists.txt
+2 −1 fs/vfs/fs_stat.c
+22 −0 include/cxx/cstdlib
+20 −1 include/nuttx/arch.h
+1 −1 include/nuttx/fs/fs.h
+32 −0 include/nuttx/lib/math32.h
+56 −0 include/nuttx/net/ksz9477.h
+3 −2 libs/libc/machine/arm/armv7-m/arch_elf.c
+4 −0 libs/libc/misc/CMakeLists.txt
+4 −0 libs/libc/misc/Make.defs
+55 −0 libs/libc/misc/lib_log2ceil.c
+55 −0 libs/libc/misc/lib_log2floor.c
+3 −3 libs/libc/modlib/modlib_bind.c
+6 −6 tools/ci/cibuild.sh
+2 −0 tools/ci/testlist/ssrc-arm.dat
+2 −0 tools/ci/testlist/ssrc-riscv.dat
1 change: 1 addition & 0 deletions platforms/nuttx/src/px4/common/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -59,6 +59,7 @@ if(NOT PX4_BOARD MATCHES "io-v2")
arch_board_critmon
arch_version
nuttx_sched
crc
)

if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
50 changes: 47 additions & 3 deletions platforms/nuttx/src/px4/common/console_buffer.cpp
Original file line number Diff line number Diff line change
@@ -206,13 +206,57 @@ ssize_t console_buffer_write(struct file *filep, const char *buffer, size_t len)
return len;
}

ssize_t console_buffer_read(struct file *filep, char *buffer, size_t buflen)
{
return g_console_buffer.read(buffer, buflen, (int *)&filep->f_pos);
}

off_t console_buffer_seek(struct file *filep, off_t offset, int whence)
{
switch (whence) {
case SEEK_SET:
filep->f_pos = offset;
break;

case SEEK_CUR:
filep->f_pos += offset;
break;

case SEEK_END:
filep->f_pos = (g_console_buffer.size() + offset) % BOARD_CONSOLE_BUFFER_SIZE;
break;

default:
return -1;
}

return filep->f_pos;
}

int console_buffer_ioctl(struct file *filep, int cmd, unsigned long arg)
{
int ret = OK;

switch (cmd) {
case FIONSPACE:
*(int *)arg = g_console_buffer.size();
break;

default:
ret = -ENOTTY;
break;
}

return ret;
}

static const struct file_operations g_console_buffer_fops = {
NULL, /* open */
NULL, /* close */
NULL, /* read */
console_buffer_read, /* read */
console_buffer_write, /* write */
NULL, /* seek */
NULL /* ioctl */
console_buffer_seek, /* seek */
console_buffer_ioctl /* ioctl */
#ifndef CONFIG_DISABLE_POLL
, NULL /* poll */
#endif
44 changes: 36 additions & 8 deletions platforms/nuttx/src/px4/common/console_buffer_usr.cpp
Original file line number Diff line number Diff line change
@@ -38,28 +38,56 @@
#include <pthread.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <stdio.h>

#ifdef BOARD_ENABLE_CONSOLE_BUFFER
#ifndef BOARD_CONSOLE_BUFFER_SIZE
# define BOARD_CONSOLE_BUFFER_SIZE (1024*4) // default buffer size
#endif


// TODO: User side implementation of px4_console_buffer
static int console_fd = -1;

int px4_console_buffer_init()
{
return 0;
console_fd = open(CONSOLE_BUFFER_DEVICE, O_RDWR);

if (console_fd < 0) {
return ERROR;
}

return OK;
}

int px4_console_buffer_size()
{
return 0;
int size;

if (ioctl(console_fd, FIONSPACE, &size) < 0) {
return 0;
}

return size;
}

int px4_console_buffer_read(char *buffer, int buffer_length, int *offset)
{
return 0;
FILE *fp;
ssize_t nread;

/* Open a file stream to keep track of offset */
fp = fdopen(dup(console_fd), "r");

if (fp == NULL) {
return -1;
}

/* The driver does not utilize file position, we have to do it for it */
fseek(fp, *offset, SEEK_SET);
nread = read(console_fd, buffer, buffer_length);
*offset = fseek(fp, 0, SEEK_CUR);

/* Now we can close the file */
fclose(fp);

return (int)nread;
}

#endif /* BOARD_ENABLE_CONSOLE_BUFFER */
54 changes: 42 additions & 12 deletions platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c
Original file line number Diff line number Diff line change
@@ -71,6 +71,7 @@

#include <perf/perf_counter.h>
#include <board_config.h>
#include <lib/crc/crc.h>

/************************************************************************************
* Pre-processor Definitions
@@ -155,6 +156,9 @@
# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20
#endif

/* Length of the CRC code of each block, in bytes */
#define CRC_LEN 2

/************************************************************************************
* Private Types
************************************************************************************/
@@ -220,10 +224,14 @@ static int at24c_eraseall(FAR struct at24c_dev_s *priv)

memset(&buf[2], 0xff, priv->pagesize);

uint16_t crc = crc16_signature(0xffff, priv->pagesize, &buf[2]);
buf[2 + priv->pagesize] = crc & 0xff;
buf[2 + priv->pagesize + 1] = crc >> 8;

BOARD_EEPROM_WP_CTRL(false);

for (startblock = 0; startblock < priv->npages; startblock++) {
uint16_t offset = startblock * priv->pagesize;
uint16_t offset = startblock * (priv->pagesize + CRC_LEN);
buf[1] = offset & 0xff;
buf[0] = (offset >> 8) & 0xff;

@@ -288,7 +296,10 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
size_t blocksleft;
uint8_t addr[2];
uint8_t buf[AT24XX_PAGESIZE];
int ret;
FAR uint8_t *buffer_start = buffer;
bool crc_error = false;

struct i2c_msg_s msgv[2] = {
{
@@ -302,8 +313,8 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
.frequency = 400000,
.addr = priv->addr,
.flags = I2C_M_READ,
.buffer = 0,
.length = priv->pagesize,
.buffer = buf,
.length = priv->pagesize + CRC_LEN,
}
};

@@ -324,21 +335,27 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
}

while (blocksleft-- > 0) {
uint16_t offset = startblock * priv->pagesize;
uint16_t offset = startblock * (priv->pagesize + CRC_LEN);
unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;

addr[1] = offset & 0xff;
addr[0] = (offset >> 8) & 0xff;
msgv[1].buffer = buffer;

for (;;) {

perf_begin(priv->perf_transfers);
ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
perf_end(priv->perf_transfers);

if (ret >= 0) {
break;
uint16_t crc_expected = ((uint16_t)buf[priv->pagesize + 1]) << 8 | buf[priv->pagesize];

if (crc_expected == crc16_signature(0xffff, priv->pagesize, buf)) {
memcpy(buffer, buf, priv->pagesize);
break;

} else {
crc_error = true;
ferr("read error, retrying");
}
}

finfo("read stall");
@@ -354,6 +371,15 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,

if (--tries == 0) {
perf_count(priv->perf_errors);
memset(buffer_start, 0xff, priv->pagesize * nblocks);

if (crc_error) {
// The data in this block is permanently corrupt. Try to reset the block so that
// it becomes usable again
ferr("CRC error, resetting block %jd\n", (intmax_t)startblock);
at24c_bwrite(dev, startblock, 1, buffer);
}

return ERROR;
}
}
@@ -413,13 +439,17 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
BOARD_EEPROM_WP_CTRL(false);

while (blocksleft-- > 0) {
uint16_t offset = startblock * priv->pagesize;
uint16_t offset = startblock * (priv->pagesize + CRC_LEN);
unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;

buf[1] = offset & 0xff;
buf[0] = (offset >> 8) & 0xff;
memcpy(&buf[2], buffer, priv->pagesize);

uint16_t crc = crc16_signature(0xffff, priv->pagesize, &buf[2]);
buf[2 + priv->pagesize] = crc & 0xff;
buf[2 + priv->pagesize + 1] = crc >> 8;

for (;;) {

perf_begin(priv->perf_transfers);
@@ -494,8 +524,8 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
*/

#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE;
geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE;
geo->blocksize = priv->pagesize * (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
geo->erasesize = priv->pagesize * (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE;
#else
geo->blocksize = priv->pagesize;
@@ -559,7 +589,7 @@ int px4_at24c_initialize(FAR struct i2c_master_s *dev,
if (priv) {
/* Initialize the allocated structure */
priv->addr = address;
priv->pagesize = AT24XX_PAGESIZE;
priv->pagesize = AT24XX_PAGESIZE - CRC_LEN;
priv->npages = AT24XX_NPAGES;

priv->mtd.erase = at24c_erase;
6 changes: 6 additions & 0 deletions platforms/nuttx/src/px4/common/px4_init.cpp
Original file line number Diff line number Diff line change
@@ -98,6 +98,12 @@ static void cxx_initialize(void)
}
}
}

int __cxa_atexit(CODE void (*func)(FAR void *), FAR void *arg,
FAR void *dso_handle)
{
return -ENOTSUP;
}
#endif

int px4_platform_init()
6 changes: 6 additions & 0 deletions platforms/nuttx/src/px4/common/px4_userspace_init.cpp
Original file line number Diff line number Diff line change
@@ -38,9 +38,11 @@
*/

#include <drivers/drv_hrt.h>
#include <px4_platform_common/console_buffer.h>
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
#include <px4_platform_common/spi.h>
#include <px4_platform_common/log.h>
#include <px4_platform/board_determine_hw_info.h>

extern void cdcacm_init(void);

@@ -50,6 +52,8 @@ extern "C" void px4_userspace_init(void)

px4_set_spi_buses_from_hw_version();

board_determine_hw_info();

// Do lazy init of wq:manager for kernel. This saves a considerable amount
// of scheduling time due to unnecessary threads.
#ifndef CONFIG_BUILD_KERNEL
@@ -58,6 +62,8 @@ extern "C" void px4_userspace_init(void)

px4_log_initialize();

px4_console_buffer_init();

#if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_PROTECTED)
cdcacm_init();
#endif
122 changes: 107 additions & 15 deletions platforms/nuttx/src/px4/common/usr_mcu_version.cpp
Original file line number Diff line number Diff line change
@@ -37,17 +37,40 @@
* Implementation of generic user-space version API
*/

#ifndef MODULE_NAME
#define MODULE_NAME "usr_mcu_version"
#endif

#include <systemlib/px4_macros.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform/board_determine_hw_info.h>

#include <lib/systemlib/px4_macros.h>

#include <uORB/uORB.h>
#include <uORB/topics/guid.h>
#include <uORB/topics/hw_info.h>
#include <uORB/topics/system_version.h>
#include <uORB/topics/system_version_string.h>

#include "board_config.h"

static int hw_version = 0;
static int hw_revision = 0;
static char hw_info[] = HW_INFO_INIT_PREFIX HW_INFO_SUFFIX;
#define HW_INFO_SIZE 64

#ifndef max
# define max(a,b) (((a) > (b)) ? (a) : (b))
#endif
#ifndef min
# define min(a,b) (((a) < (b)) ? (a) : (b))
#endif

static unsigned hw_version = 0;
static unsigned hw_revision = 0;
static char hw_info[HW_INFO_SIZE] = { 0 };
static uint16_t soc_arch_id = 0;
static mfguid_t device_serial_number = { 0 };

__EXPORT const char *board_get_hw_type_name(void)
{
@@ -66,9 +89,9 @@ __EXPORT int board_get_hw_revision()

__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words)
{
/* TODO: This is a stub for userspace build. Use some proper interface
* to fetch the uuid32 from the kernel
*/

/* DEPRECATED. Use board_get_px4_guid() */

uint32_t chip_uuid[PX4_CPU_UUID_WORD32_LENGTH];
memset((uint8_t *)chip_uuid, 0, PX4_CPU_UUID_WORD32_LENGTH * 4);

@@ -79,19 +102,36 @@ __EXPORT void board_get_uuid32(uuid_uint32_t uuid_words)

int board_mcu_version(char *rev, const char **revstr, const char **errata)
{
/* TODO: This is a stub for userspace build. Use some proper interface
* to fetch the version from the kernel
*/
return -1;
const struct {
const char *revstr;
char rev;
const char *errata;
} hw_version_table[] = BOARD_REVISIONS;

unsigned len = sizeof(hw_version_table) / sizeof(hw_version_table[0]);

if (hw_version < len) {
*rev = hw_version_table[hw_version].rev;
*revstr = hw_version_table[hw_version].revstr;
*errata = hw_version_table[hw_version].errata;
}

return hw_version;
}

int board_get_px4_guid(px4_guid_t px4_guid)
{
/* TODO: This is a stub for userspace build. Use some proper interface
* to fetch the guid from the kernel
*/
uint8_t *pb = (uint8_t *) &px4_guid[0];
memset(pb, 0, PX4_GUID_BYTE_LENGTH);
uint8_t *pb = (uint8_t *) &px4_guid[0];

memset(pb, 0, sizeof(px4_guid_t));

static_assert(sizeof(device_serial_number) == 16);
static_assert(sizeof(px4_guid_t) >= sizeof(device_serial_number) + 2);

*pb++ = (soc_arch_id >> 8) & 0xff;
*pb++ = (soc_arch_id & 0xff);

memcpy(pb, device_serial_number, sizeof(device_serial_number));

return PX4_GUID_BYTE_LENGTH;
}
@@ -115,3 +155,55 @@ int board_get_px4_guid_formated(char *format_buffer, int size)
return offset;
}

int board_determine_hw_info(void)
{
orb_sub_t hwinfo_sub;
orb_sub_t guid_sub;
orb_sub_t system_version_sub;
struct system_version_s ver;
struct guid_s guid;
struct hw_info_s hwinfo;

/* System hw_version, hw_revision, soc_arch_id */
system_version_sub = orb_subscribe(ORB_ID(system_version));

if (!orb_sub_valid(system_version_sub)) {
PX4_ERR("Failed to subscribe to system version (%i)", errno);
return -1;
}

orb_copy(ORB_ID(system_version), system_version_sub, &ver);
orb_unsubscribe(system_version_sub);

hw_version = ver.hw_version;
hw_revision = ver.hw_revision;
soc_arch_id = ver.soc_arch_id;

/* Hardware information string */
hwinfo_sub = orb_subscribe(ORB_ID(hw_info));

if (!orb_sub_valid(hwinfo_sub)) {
PX4_ERR("Failed to subscribe to hwinfo_sub (%i)", errno);
return -1;
}

orb_copy(ORB_ID(hw_info), hwinfo_sub, &hwinfo);
orb_unsubscribe(hwinfo_sub);

memcpy(hw_info, &hwinfo, min(sizeof(hwinfo), sizeof(hw_info)));

/* Drone guid */
guid_sub = orb_subscribe(ORB_ID(guid));

if (!orb_sub_valid(guid_sub)) {
PX4_ERR("Failed to subscribe to guid (%i)", errno);
return -1;
}

orb_copy(ORB_ID(guid), guid_sub, &guid);
orb_unsubscribe(guid_sub);

memcpy(device_serial_number, &guid, min(sizeof(device_serial_number), sizeof(guid)));

return 0;
}
Original file line number Diff line number Diff line change
@@ -53,7 +53,7 @@ typedef struct __attribute__((__packed__))

/* Device MAC addresses */

uint8_t mac[6][4];
uint8_t mac[4][6];

/* Bootloader version info */

Original file line number Diff line number Diff line change
@@ -47,6 +47,8 @@
#include <mpfs_dsn.h>

#include <uORB/uORB.h>
#include <uORB/topics/guid.h>
#include <uORB/topics/hw_info.h>
#include <uORB/topics/system_version.h>
#include <uORB/topics/system_version_string.h>

@@ -73,6 +75,13 @@
#define getreg32(a) (*(volatile uint32_t *)(a))
#define putreg32(v,a) (*(volatile uint32_t *)(a) = (v))

#ifndef max
# define max(a,b) (((a) > (b)) ? (a) : (b))
#endif
#ifndef min
# define min(a,b) (((a) < (b)) ? (a) : (b))
#endif

static unsigned hw_version = 0;
static unsigned hw_revision = 0;
static unsigned fpga_version_major;
@@ -262,12 +271,17 @@ static uint64_t parse_tag_to_version(const char *ver_str)
* 3) hw_info is populated
*
************************************************************************************/

int board_determine_hw_info(void)
{
struct system_version_string_s ver_str;
struct system_version_s ver;
struct guid_s guid;
struct hw_info_s hwinfo;
orb_advert_t ver_str_pub = orb_advertise(ORB_ID(system_version_string), NULL);
orb_advert_t ver_pub = orb_advertise(ORB_ID(system_version), NULL);
orb_advert_t mfguid_pub = orb_advertise(ORB_ID(guid), NULL);
orb_advert_t hw_info_pub = orb_advertise(ORB_ID(hw_info), NULL);

uint32_t fpga_version = getreg32(FPGA_VER_REGISTER); // todo: replace eventually with device_boot_info

@@ -282,7 +296,15 @@ int board_determine_hw_info(void)
/* HW version */

snprintf(ver_str.hw_version, sizeof(ver_str.hw_version), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision);
ver.hw_version = fpga_version;
ver.hw_version = hw_version;

/* HW revision */

ver.hw_revision = hw_revision;

/* SoC architecture ID */

ver.soc_arch_id = soc_arch_id;

/* PX4 version */

@@ -305,8 +327,17 @@ int board_determine_hw_info(void)
HW_INFO_FPGA_PREFIX HW_INFO_FPGA_SUFFIX " (0x%x)", fpga_version_major, fpga_version_minor, getreg32(FPGA_VER_REGISTER));
ver.component_version1 = fpga_version;

/* Make local copies of guid and hwinfo */

memcpy(&guid, device_serial_number, min(sizeof(device_serial_number), sizeof(guid)));
memcpy(&hwinfo, hw_info, min(sizeof(hwinfo), sizeof(hw_info)));

/* Then publish the topics */

orb_publish(ORB_ID(system_version_string), &ver_str_pub, &ver_str);
orb_publish(ORB_ID(system_version), &ver_pub, &ver);
orb_publish(ORB_ID(guid), &mfguid_pub, &guid);
orb_publish(ORB_ID(hw_info), &hw_info_pub, &hwinfo);

return OK;
}
10 changes: 8 additions & 2 deletions platforms/nuttx/toc/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -60,8 +60,14 @@ if(NOT USE_LD_GOLD)
target_link_libraries(toc PRIVATE -Wl,--print-memory-usage)
endif()

target_link_libraries(toc PRIVATE board_toc)
target_link_libraries(drivers_board PRIVATE board_toc)
set(toc_libs board_toc)

if (TARGET image_cfg)
list(APPEND toc_libs image_cfg)
endif()

target_link_libraries(toc PRIVATE ${toc_libs})
target_link_libraries(drivers_board PRIVATE ${toc_libs})

set(TOC_BINARY_OUTPUT ${PX4_BINARY_DIR}/toc.bin)

1 change: 1 addition & 0 deletions src/include/image_toc.h
Original file line number Diff line number Diff line change
@@ -48,6 +48,7 @@
#define TOC_FLAG1_DECRYPT 0x8
#define TOC_FLAG1_COPY 0x10

#define TOC_FLAG1_ICFG 0x40
#define TOC_FLAG1_RDCT 0x80

#define TOC_START_MAGIC 0x00434f54 /* "TOC" */
265 changes: 165 additions & 100 deletions src/lib/perf/perf_counter.cpp
Original file line number Diff line number Diff line change
@@ -37,24 +37,44 @@
* @brief Performance measuring tools.
*/

#ifndef MODULE_NAME
#define MODULE_NAME "perf"
#endif

#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <pthread.h>
#include <systemlib/err.h>
#include <dirent.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>

#include <px4_platform_common/mmap.h>

#include "perf_counter.h"

#define PERF_SHMNAME_PREFIX "_perf_"
#define PERF_SHMNAME_STR PERF_SHMNAME_PREFIX "%s"
#define PERF_SHMNAME_MAX NAME_MAX + 1

#ifndef CONFIG_FS_SHMFS_VFS_PATH
#define CONFIG_FS_SHMFS_VFS_PATH "/dev/shm"
#endif

/**
* Header common to all counters.
*/
struct perf_ctr_header {
sq_entry_t link; /**< list linkage */
enum perf_counter_type type; /**< counter type */
#ifdef CONFIG_BUILD_FLAT
const char *name; /**< counter name */
#else
char name[PERF_SHMNAME_MAX]; /**< counter name */
#endif
};

/**
@@ -91,51 +111,124 @@ struct perf_ctr_interval : public perf_ctr_header {
float M2{0.0f};
};

/**
* List of all known counters.
*/
static sq_queue_t perf_counters = { nullptr, nullptr };

/**
* mutex protecting access to the perf_counters linked list (which is read from & written to by different threads)
*/
pthread_mutex_t perf_counters_mutex = PTHREAD_MUTEX_INITIALIZER;
// FIXME: the mutex does **not** protect against access to/from the perf
// counter's data. It can still happen that a counter is updated while it is
// printed. This can lead to inconsistent output, or completely bogus values
// (especially the 64bit values which are in general not atomically updated).
// The same holds for shared perf counters (perf_alloc_once), that can be updated
// concurrently (this affects the 'ctrl_latency' counter).


perf_counter_t
perf_alloc(enum perf_counter_type type, const char *name)
static size_t perf_size(enum perf_counter_type type)
{
perf_counter_t ctr = nullptr;

switch (type) {
case PC_COUNT:
ctr = new perf_ctr_count();
break;
return sizeof(perf_ctr_count);

case PC_ELAPSED:
ctr = new perf_ctr_elapsed();
break;
return sizeof(perf_ctr_elapsed);

case PC_INTERVAL:
ctr = new perf_ctr_interval();
break;
return sizeof(perf_ctr_interval);

default:
break;
}

if (ctr != nullptr) {
return 0;
}

static size_t perf_shmsize(int fd)
{
struct stat sb;

if (fstat(fd, &sb) < 0) {
PX4_ERR("Cannot stat %d", fd);
}

return sb.st_size;
}

static int perf_shmname(char *shmname, const char *name, size_t size)
{
int ret = snprintf(shmname, size, PERF_SHMNAME_STR, name);

if ((size_t)ret >= size) {
return -ENAMETOOLONG;
}

return OK;
}

static void perf_foreach(perf_callback cb, void *arg = NULL)
{
DIR *dir = opendir(CONFIG_FS_SHMFS_VFS_PATH);
struct dirent *perf;

while ((perf = readdir(dir)) != nullptr) {
if (!strncmp(perf->d_name, PERF_SHMNAME_PREFIX, sizeof(PERF_SHMNAME_PREFIX) - 1)) {
/* found the counter, map it for us */
int fd = shm_open(perf->d_name, O_RDWR, 0666);

if (fd >= 0) {
size_t size = perf_shmsize(fd);
void *p = px4_mmap(0, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);

/* the file can be closed now */
close(fd);

if (p != MAP_FAILED) {
cb((perf_counter_t)p, arg);
px4_munmap(p, size);
}
}
}
}
}

static void perf_print_cb(perf_counter_t handle, void *arg)
{
perf_print_counter(handle);
}

static void perf_reset_cb(perf_counter_t handle, void *arg)
{
perf_reset(handle);
}

perf_counter_t
perf_alloc(enum perf_counter_type type, const char *name)
{
perf_counter_t ctr;
char shmname[PERF_SHMNAME_MAX];
int ret;
int fd;

/* generate name for shmfs file */
ret = perf_shmname(shmname, name, sizeof(shmname));

if (ret < 0) {
PX4_ERR("failed to allocate perf counter %s", name);
return nullptr;
}

/* try to allocate new shm object and map it */
ctr = nullptr;
fd = shm_open(shmname, O_CREAT | O_RDWR | O_EXCL, 0666);

if (fd >= 0) {
ret = ftruncate(fd, perf_size(type));

if (ret == 0) {
void *p = px4_mmap(0, perf_size(type), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);

if (p != MAP_FAILED) {
ctr = (perf_counter_t)p;
}
}
}

close(fd);

if (ctr) {
ctr->type = type;
#ifdef CONFIG_BUILD_FLAT
ctr->name = name;
pthread_mutex_lock(&perf_counters_mutex);
sq_addfirst(&ctr->link, &perf_counters);
pthread_mutex_unlock(&perf_counters_mutex);
#else
strncpy(ctr->name, name, PERF_SHMNAME_MAX);
#endif
}

return ctr;
@@ -144,27 +237,38 @@ perf_alloc(enum perf_counter_type type, const char *name)
perf_counter_t
perf_alloc_once(enum perf_counter_type type, const char *name)
{
pthread_mutex_lock(&perf_counters_mutex);
perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);

while (handle != nullptr) {
if (!strcmp(handle->name, name)) {
if (type == handle->type) {
/* they are the same counter */
pthread_mutex_unlock(&perf_counters_mutex);
return handle;

} else {
/* same name but different type, assuming this is an error and not intended */
pthread_mutex_unlock(&perf_counters_mutex);
return nullptr;
}
}
DIR *dir = opendir(CONFIG_FS_SHMFS_VFS_PATH);
struct dirent *perf;
char shmname[PERF_SHMNAME_MAX];
int ret;

/* generate name for shmfs file */
ret = perf_shmname(shmname, name, sizeof(shmname));

handle = (perf_counter_t)sq_next(&handle->link);
if (ret < 0) {
PX4_ERR("failed to allocate perf counter %s", name);
return nullptr;
}

pthread_mutex_unlock(&perf_counters_mutex);
while ((perf = readdir(dir)) != nullptr) {
if (!strncmp(perf->d_name, shmname, sizeof(shmname))) {
/* found the counter, map it for us */
int fd = shm_open(perf->d_name, O_RDWR, 0666);

if (fd >= 0) {
void *p = px4_mmap(0, perf_size(type), PROT_READ, MAP_SHARED, fd, 0);

/* the file can be closed now */
close(fd);

if (p == MAP_FAILED) {
return nullptr;
}

return (perf_counter_t)p;
}
}
}

/* if the execution reaches here, no existing counter of that name was found */
return perf_alloc(type, name);
@@ -173,29 +277,15 @@ perf_alloc_once(enum perf_counter_type type, const char *name)
void
perf_free(perf_counter_t handle)
{
if (handle == nullptr) {
return;
}

pthread_mutex_lock(&perf_counters_mutex);
sq_rem(&handle->link, &perf_counters);
pthread_mutex_unlock(&perf_counters_mutex);

switch (handle->type) {
case PC_COUNT:
delete (struct perf_ctr_count *)handle;
break;
if (handle) {
char shmname[PERF_SHMNAME_MAX];

case PC_ELAPSED:
delete (struct perf_ctr_elapsed *)handle;
break;

case PC_INTERVAL:
delete (struct perf_ctr_interval *)handle;
break;
/* should not fail, if object creation succeeded ? */
if (perf_shmname(shmname, handle->name, sizeof(shmname) >= 0)) {
shm_unlink(shmname);
}

default:
break;
px4_munmap(handle, perf_size(handle->type));
}
}

@@ -586,29 +676,13 @@ perf_mean(perf_counter_t handle)
void
perf_iterate_all(perf_callback cb, void *user)
{
pthread_mutex_lock(&perf_counters_mutex);
perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);

while (handle != nullptr) {
cb(handle, user);
handle = (perf_counter_t)sq_next(&handle->link);
}

pthread_mutex_unlock(&perf_counters_mutex);
perf_foreach(cb, user);
}

void
perf_print_all(void)
{
pthread_mutex_lock(&perf_counters_mutex);
perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);

while (handle != nullptr) {
perf_print_counter(handle);
handle = (perf_counter_t)sq_next(&handle->link);
}

pthread_mutex_unlock(&perf_counters_mutex);
perf_foreach(perf_print_cb);
}

void
@@ -630,15 +704,6 @@ perf_print_latency(void)
void
perf_reset_all(void)
{
pthread_mutex_lock(&perf_counters_mutex);
perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);

while (handle != nullptr) {
perf_reset(handle);
handle = (perf_counter_t)sq_next(&handle->link);
}

pthread_mutex_unlock(&perf_counters_mutex);

perf_foreach(perf_reset_cb);
reset_latency_counters();
}
3 changes: 2 additions & 1 deletion src/modules/mavlink/mavlink_ulog.cpp
Original file line number Diff line number Diff line change
@@ -136,7 +136,8 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
}
}

while ((_current_num_msgs < _max_num_messages) && (_ulog_stream_sub.updated() || _ulog_stream_acked_sub.updated())) {
while ((_current_num_msgs < _max_num_messages) &&
(_ulog_stream_sub.updated() || (check_for_updates && _ulog_stream_acked_sub.updated()))) {
if (_ulog_stream_sub.updated()) {
const unsigned last_generation = _ulog_stream_sub.get_last_generation();
_ulog_stream_sub.update();
37 changes: 24 additions & 13 deletions src/systemcmds/uorb/uORBDeviceMaster.cpp
Original file line number Diff line number Diff line change
@@ -109,13 +109,37 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node,
DIR *shm_dir = opendir(CONFIG_FS_SHMFS_VFS_PATH);
struct dirent *shm;
const char orb_name_prefix[] = "_orb_";
char orb_name[orb_maxpath];

while ((shm = readdir(shm_dir)) != nullptr) {

if (strncmp(orb_name_prefix, shm->d_name, sizeof(orb_name_prefix) - 1)) {
continue;
}

// check if already added
cur_node = *first_node;

while (cur_node) {
int instance = cur_node->node->get_instance();

if (uORB::Utils::node_mkpath(orb_name, cur_node->node->get_meta(), &instance)) {
PX4_ERR("Can't construct orb name?");
break;
}

if (!strcmp(orb_name, shm->d_name)) {
break;
}

cur_node = cur_node->next;
}

if (cur_node) {
// already added or there was an error
continue;
}

void *ptr = nullptr;
uORB::DeviceNode *node = nullptr;

@@ -139,19 +163,6 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node,

++num_topics;

//check if already added
cur_node = *first_node;

while (cur_node && cur_node->node != node) {
cur_node = cur_node->next;
}

if (cur_node) {
// currently nuttx creates a new mapping on every mmap. TODO: check linux
px4_munmap(node, sizeof(uORB::DeviceNode));
continue;
}

if (num_filters > 0 && topic_filter) {
bool matched = false;