From ff4254a5038239776d36ee3b26235032902d101a Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Wed, 19 Feb 2025 13:05:39 +0100 Subject: [PATCH 01/11] Add tflm to px4 with module - Add TensorFlow Lite Micro(TFLM) as a library in px4 - Make a module that uses neural network inference for control, which uses TFLM for inference - Make board config files for PX4 with neural module --- .gitmodules | 4 + CMakeLists.txt | 2 +- boards/mro/pixracerpro/neural.px4board | 99 +++++ boards/px4/fmu-v6c/neural.px4board | 93 ++++ boards/px4/sitl/neural.px4board | 80 ++++ .../nuttx/cmake/Toolchain-arm-none-eabi.cmake | 89 +++- src/lib/CMakeLists.txt | 1 + src/lib/tflm/CMakeLists.txt | 93 ++++ src/lib/tflm/tflite_micro | 1 + src/modules/mc_nn_control/.gitignore | 2 + src/modules/mc_nn_control/CMakeLists.txt | 56 +++ src/modules/mc_nn_control/Kconfig | 12 + src/modules/mc_nn_control/allocation_net.hpp | 4 + src/modules/mc_nn_control/control_net.hpp | 4 + src/modules/mc_nn_control/mc_nn_control.cpp | 412 ++++++++++++++++++ src/modules/mc_nn_control/mc_nn_control.hpp | 135 ++++++ .../mc_nn_control/mc_nn_control_params.c | 63 +++ 17 files changed, 1127 insertions(+), 23 deletions(-) create mode 100644 boards/mro/pixracerpro/neural.px4board create mode 100644 boards/px4/fmu-v6c/neural.px4board create mode 100644 boards/px4/sitl/neural.px4board create mode 100644 src/lib/tflm/CMakeLists.txt create mode 160000 src/lib/tflm/tflite_micro create mode 100644 src/modules/mc_nn_control/.gitignore create mode 100644 src/modules/mc_nn_control/CMakeLists.txt create mode 100644 src/modules/mc_nn_control/Kconfig create mode 100644 src/modules/mc_nn_control/allocation_net.hpp create mode 100644 src/modules/mc_nn_control/control_net.hpp create mode 100644 src/modules/mc_nn_control/mc_nn_control.cpp create mode 100644 src/modules/mc_nn_control/mc_nn_control.hpp create mode 100644 src/modules/mc_nn_control/mc_nn_control_params.c diff --git a/.gitmodules b/.gitmodules index 1ebafcf5a4e5..15a78d33382d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -89,3 +89,7 @@ [submodule "src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan"] path = src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan url = https://github.com/dronecan/pydronecan +[submodule "src/lib/tflm/tflite_micro"] + path = src/lib/tflm/tflite_micro + url = git@github.com:tensorflow/tflite-micro.git + branch = main diff --git a/CMakeLists.txt b/CMakeLists.txt index 55362b1425c7..f89d76652c11 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -267,7 +267,7 @@ endif() set(package-contact "px4users@googlegroups.com") -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) diff --git a/boards/mro/pixracerpro/neural.px4board b/boards/mro/pixracerpro/neural.px4board new file mode 100644 index 000000000000..2d8f20446902 --- /dev/null +++ b/boards/mro/pixracerpro/neural.px4board @@ -0,0 +1,99 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI085=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=n +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MC_NN_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v6c/neural.px4board b/boards/px4/fmu-v6c/neural.px4board new file mode 100644 index 000000000000..c888615aceab --- /dev/null +++ b/boards/px4/fmu-v6c/neural.px4board @@ -0,0 +1,93 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_INS=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=n +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_NN_CONTROL=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/sitl/neural.px4board b/boards/px4/sitl/neural.px4board new file mode 100644 index 000000000000..9afbf845f180 --- /dev/null +++ b/boards/px4/sitl/neural.px4board @@ -0,0 +1,80 @@ +CONFIG_PLATFORM_POSIX=y +CONFIG_BOARD_TESTING=y +CONFIG_BOARD_ETHERNET=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_EKF2=y +CONFIG_EKF2_VERBOSE_STATUS=y +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_FIGURE_OF_EIGHT=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_NN_CONTROL=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y +CONFIG_MODULES_PAYLOAD_DELIVERER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_REPLAY=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_COMMON_SIMULATION=y +CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DYN=y +CONFIG_SYSTEMCMDS_FAILURE=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SHUTDOWN=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_DYN_HELLO=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_EXAMPLES_FAKE_IMU=y +CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y +CONFIG_EXAMPLES_PX4_SIMPLE_APP=y +CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake index 85e5caa00747..a47ab8442f27 100644 --- a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake +++ b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake @@ -1,3 +1,48 @@ +# # arm-none-eabi-gcc toolchain + +# set(CMAKE_SYSTEM_NAME Generic) +# set(CMAKE_SYSTEM_VERSION 1) + +# set(triple arm-none-eabi) +# set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) +# set(TOOLCHAIN_PREFIX ${triple}) + +# set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc) +# set(CMAKE_C_COMPILER_TARGET ${triple}) + +# set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++) +# set(CMAKE_CXX_COMPILER_TARGET ${triple}) + +# set(CMAKE_ASM_COMPILER ${TOOLCHAIN_PREFIX}-gcc) + +# # needed for test compilation +# set(CMAKE_EXE_LINKER_FLAGS_INIT "--specs=nosys.specs") + +# # compiler tools +# find_program(CMAKE_AR ${TOOLCHAIN_PREFIX}-gcc-ar) +# find_program(CMAKE_GDB ${TOOLCHAIN_PREFIX}-gdb) +# find_program(CMAKE_LD ${TOOLCHAIN_PREFIX}-ld) +# find_program(CMAKE_LINKER ${TOOLCHAIN_PREFIX}-ld) +# find_program(CMAKE_NM ${TOOLCHAIN_PREFIX}-gcc-nm) +# find_program(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}-objcopy) +# find_program(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}-objdump) +# find_program(CMAKE_RANLIB ${TOOLCHAIN_PREFIX}-gcc-ranlib) +# find_program(CMAKE_STRIP ${TOOLCHAIN_PREFIX}-strip) + +# set(CMAKE_FIND_ROOT_PATH get_file_component(${CMAKE_C_COMPILER} PATH)) +# set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +# set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +# set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# # os tools +# foreach(tool grep make) +# string(TOUPPER ${tool} TOOL) +# find_program(${TOOL} ${tool}) +# if(NOT ${TOOL}) +# message(FATAL_ERROR "could not find ${tool}") +# endif() +# endforeach() + # arm-none-eabi-gcc toolchain set(CMAKE_SYSTEM_NAME Generic) @@ -7,38 +52,38 @@ set(triple arm-none-eabi) set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) set(TOOLCHAIN_PREFIX ${triple}) -set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc) -set(CMAKE_C_COMPILER_TARGET ${triple}) - -set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++) -set(CMAKE_CXX_COMPILER_TARGET ${triple}) +# Define the path to the new toolchain +set(NEW_TOOLCHAIN_PATH "/home/sindre/Dropbox/Studier/2025_Var/dev/PX4-Autopilot-public/src/lib/tflm/tflite_micro/tensorflow/lite/micro/tools/make/downloads/gcc_embedded/bin") -set(CMAKE_ASM_COMPILER ${TOOLCHAIN_PREFIX}-gcc) +# Set the compiler paths +set(CMAKE_C_COMPILER ${NEW_TOOLCHAIN_PATH}/${TOOLCHAIN_PREFIX}-gcc) +set(CMAKE_CXX_COMPILER ${NEW_TOOLCHAIN_PATH}/${TOOLCHAIN_PREFIX}-g++) +set(CMAKE_ASM_COMPILER ${NEW_TOOLCHAIN_PATH}/${TOOLCHAIN_PREFIX}-gcc) # needed for test compilation set(CMAKE_EXE_LINKER_FLAGS_INIT "--specs=nosys.specs") # compiler tools -find_program(CMAKE_AR ${TOOLCHAIN_PREFIX}-gcc-ar) -find_program(CMAKE_GDB ${TOOLCHAIN_PREFIX}-gdb) -find_program(CMAKE_LD ${TOOLCHAIN_PREFIX}-ld) -find_program(CMAKE_LINKER ${TOOLCHAIN_PREFIX}-ld) -find_program(CMAKE_NM ${TOOLCHAIN_PREFIX}-gcc-nm) -find_program(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}-objcopy) -find_program(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}-objdump) -find_program(CMAKE_RANLIB ${TOOLCHAIN_PREFIX}-gcc-ranlib) -find_program(CMAKE_STRIP ${TOOLCHAIN_PREFIX}-strip) - -set(CMAKE_FIND_ROOT_PATH get_file_component(${CMAKE_C_COMPILER} PATH)) +find_program(CMAKE_AR ${TOOLCHAIN_PREFIX}-gcc-ar PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_GDB ${TOOLCHAIN_PREFIX}-gdb PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_LD ${TOOLCHAIN_PREFIX}-ld PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_LINKER ${TOOLCHAIN_PREFIX}-ld PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_NM ${TOOLCHAIN_PREFIX}-gcc-nm PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}-objcopy PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}-objdump PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_RANLIB ${TOOLCHAIN_PREFIX}-gcc-ranlib PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_STRIP ${TOOLCHAIN_PREFIX}-strip PATHS ${NEW_TOOLCHAIN_PATH}) + +set(CMAKE_FIND_ROOT_PATH ${NEW_TOOLCHAIN_PATH}) set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) # os tools foreach(tool grep make) - string(TOUPPER ${tool} TOOL) - find_program(${TOOL} ${tool}) - if(NOT ${TOOL}) - message(FATAL_ERROR "could not find ${tool}") - endif() + string(TOUPPER ${tool} TOOL) + find_program(${TOOL} ${tool}) + if(NOT ${TOOL}) + message(FATAL_ERROR "could not find ${tool}") + endif() endforeach() diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index a9396caa1fa9..9b2c7928f286 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -76,6 +76,7 @@ add_subdirectory(systemlib EXCLUDE_FROM_ALL) add_subdirectory(system_identification EXCLUDE_FROM_ALL) add_subdirectory(tecs EXCLUDE_FROM_ALL) add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL) +add_subdirectory(tflm EXCLUDE_FROM_ALL) add_subdirectory(timesync EXCLUDE_FROM_ALL) add_subdirectory(tinybson EXCLUDE_FROM_ALL) add_subdirectory(tunes EXCLUDE_FROM_ALL) diff --git a/src/lib/tflm/CMakeLists.txt b/src/lib/tflm/CMakeLists.txt new file mode 100644 index 000000000000..23752edbebee --- /dev/null +++ b/src/lib/tflm/CMakeLists.txt @@ -0,0 +1,93 @@ +############################################################################ +# +# Copyright (c) 2017-2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_git_submodule(TARGET git_tflite-micro PATH tflite_micro) + +set(TFLITE_DOWNLOADS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/tools/make/downloads) +set(TFLITE_GEN_DIR ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/gen/cortex_m_generic_cortex-m7_default_cmsis_nn_gcc) + +get_directory_property(FLAGS COMPILE_OPTIONS) +list(REMOVE_ITEM FLAGS "-Wcast-align") +set_directory_properties(PROPERTIES COMPILE_OPTIONS "${FLAGS}") + + +file(GLOB TFLITE_MICRO_SRCS + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/arena_allocator/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/memory_planner/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/tflite_bridge/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/kernels/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/kernels/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/kernels/internal/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/core/api/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/core/c/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/schema/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/compiler/mlir/lite/core/api/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/compiler/mlir/lite/schema/*.cc +) + +# Filter out tests as they cause errors +list(FILTER TFLITE_MICRO_SRCS EXCLUDE REGEX ".*_test.*\\.cc$") + +set(TFLM_INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro + ${TFLITE_DOWNLOADS_DIR} + ${TFLITE_DOWNLOADS_DIR}/flatbuffers/include + ${TFLITE_DOWNLOADS_DIR}/ruy + ${TFLITE_DOWNLOADS_DIR}/gemmlowp + ${TFLITE_DOWNLOADS_DIR}/kissfft + ${TFLITE_DOWNLOADS_DIR}/cmsis/Cortex_DFP/Device/ARMCM7/Include + ${TFLITE_DOWNLOADS_DIR}/cmsis/CMSIS/Core/Include + ${TFLITE_DOWNLOADS_DIR}/cmsis + ${TFLITE_DOWNLOADS_DIR}/cmsis_nn + ${TFLITE_DOWNLOADS_DIR}/cmsis_nn/Include + ${TFLITE_GEN_DIR}/genfiles + ) + + # Add C++ 17 std lib if building for NuttX + if(CONFIG_BOARD_TOOLCHAIN STREQUAL "arm-none-eabi") + list(APPEND TFLM_INCLUDE_DIRS + ${TFLITE_DOWNLOADS_DIR}/include/13.2.1 + ${TFLITE_DOWNLOADS_DIR}/include/13.2.1/arm-none-eabi + ) +endif() + +px4_add_library(tflm ${TFLITE_MICRO_SRCS}) + +target_include_directories(tflm PUBLIC ${TFLM_INCLUDE_DIRS}) + +target_compile_options(tflm PUBLIC + -Wno-float-equal + -Wno-shadow +) diff --git a/src/lib/tflm/tflite_micro b/src/lib/tflm/tflite_micro new file mode 160000 index 000000000000..ef6459127069 --- /dev/null +++ b/src/lib/tflm/tflite_micro @@ -0,0 +1 @@ +Subproject commit ef64591270691022a329cf04ba9e73ecfb15ddb8 diff --git a/src/modules/mc_nn_control/.gitignore b/src/modules/mc_nn_control/.gitignore new file mode 100644 index 000000000000..7e06bb76f920 --- /dev/null +++ b/src/modules/mc_nn_control/.gitignore @@ -0,0 +1,2 @@ +control_net.cpp +allocation_net.cpp diff --git a/src/modules/mc_nn_control/CMakeLists.txt b/src/modules/mc_nn_control/CMakeLists.txt new file mode 100644 index 000000000000..84f0a15be5e1 --- /dev/null +++ b/src/modules/mc_nn_control/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# Copyright (c) 2024-2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +add_compile_options(-Wno-float-equal) +get_directory_property(FLAGS COMPILE_OPTIONS) +list(REMOVE_ITEM FLAGS "-Wcast-align") +set_directory_properties(PROPERTIES COMPILE_OPTIONS "${FLAGS}") + +px4_add_module( + MODULE mc_nn_control + MAIN mc_nn_control + COMPILE_FLAGS + SRCS + mc_nn_control.cpp + mc_nn_control.hpp + control_net.cpp + control_net.hpp + allocation_net.cpp + allocation_net.hpp + DEPENDS + tflm + px4_work_queue + mathlib +) +target_link_libraries(mc_nn_control PRIVATE tflm) +target_include_directories(mc_nn_control PRIVATE + ${CMAKE_SOURCE_DIR}/src/lib/tflm) diff --git a/src/modules/mc_nn_control/Kconfig b/src/modules/mc_nn_control/Kconfig new file mode 100644 index 000000000000..30829664452c --- /dev/null +++ b/src/modules/mc_nn_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_MC_NN_CONTROL + bool "mc_nn_control" + default n + ---help--- + Enable support for mc_nn_control" + +menuconfig USER_MC_NN_CONTROL + bool "mc_nn_control running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_MC_NN_CONTROL + ---help--- + Put mc_nn_control in userspace memory diff --git a/src/modules/mc_nn_control/allocation_net.hpp b/src/modules/mc_nn_control/allocation_net.hpp new file mode 100644 index 000000000000..4dfaa9600a4a --- /dev/null +++ b/src/modules/mc_nn_control/allocation_net.hpp @@ -0,0 +1,4 @@ +#include + +constexpr unsigned int allocation_net_tflite_size = 8012; +extern const unsigned char allocation_net_tflite[]; diff --git a/src/modules/mc_nn_control/control_net.hpp b/src/modules/mc_nn_control/control_net.hpp new file mode 100644 index 000000000000..09f7e8fcb044 --- /dev/null +++ b/src/modules/mc_nn_control/control_net.hpp @@ -0,0 +1,4 @@ +#include + +constexpr unsigned int control_net_tflite_size = 8012; +extern const unsigned char control_net_tflite[]; diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp new file mode 100644 index 000000000000..5b691d177905 --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -0,0 +1,412 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mc_nn_control.cpp + * Multicopter Neural Network Control module, from position setpoints to control allocator. + * + * @author Sindre Meyer Hegre + */ + +#include "mc_nn_control.hpp" +//#include +#include + +namespace { +using NNControlOpResolver = tflite::MicroMutableOpResolver<4>; // This number should be the number of operations in the model, like tanh and fully connected + +TfLiteStatus RegisterOps(NNControlOpResolver& op_resolver) { + // Add the operations to you need to the op_resolver + TF_LITE_ENSURE_STATUS(op_resolver.AddFullyConnected()); + TF_LITE_ENSURE_STATUS(op_resolver.AddTanh()); + TF_LITE_ENSURE_STATUS(op_resolver.AddRelu()); + TF_LITE_ENSURE_STATUS(op_resolver.AddAdd()); + return kTfLiteOk; +} +} // namespace + +MulticopterNeuralNetworkControl::MulticopterNeuralNetworkControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + +} + +MulticopterNeuralNetworkControl::~MulticopterNeuralNetworkControl() +{ + perf_free(_loop_perf); +} + + +bool MulticopterNeuralNetworkControl::init() +{ + if (!_angular_velocity_sub.registerCallback()) + { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int MulticopterNeuralNetworkControl::InitializeNetwork() { + // Initialize the neural network + // Load the model + const tflite::Model* control_model = ::tflite::GetModel(control_net_tflite); // TODO: Replace with your model data variable + // if (control_model->version() != TFLITE_SCHEMA_VERSION) { + // PX4_ERR("Model provided is schema version %d not equal to supported version %d.", + // control_model->version(), TFLITE_SCHEMA_VERSION); + // return -1; + // } + + const tflite::Model* allocation_model = ::tflite::GetModel(allocation_net_tflite); // TODO: Replace with your model data variable + // if (allocation_model->version() != TFLITE_SCHEMA_VERSION) { + // PX4_ERR("Model provided is schema version %d not equal to supported version %d.", + // allocation_model->version(), TFLITE_SCHEMA_VERSION); + // return -1; + // } + + // Set up the interpreter + static NNControlOpResolver resolver; + if (RegisterOps(resolver) != kTfLiteOk) { + PX4_ERR("Failed to register ops"); + return -1; + } + constexpr int kTensorArenaSize = 10 * 1024; //TODO: Check this size + static uint8_t tensor_arena[kTensorArenaSize]; + _control_interpreter = new tflite::MicroInterpreter(control_model, resolver, tensor_arena, kTensorArenaSize); + _allocation_interpreter = new tflite::MicroInterpreter(allocation_model, resolver, tensor_arena, kTensorArenaSize); + + // Allocate memory for the model's tensors + TfLiteStatus allocate_status_control = _control_interpreter->AllocateTensors(); + if (allocate_status_control != kTfLiteOk) { + PX4_ERR("AllocateTensors() failed"); + return -1; + } + TfLiteStatus allocate_status = _allocation_interpreter->AllocateTensors(); + if (allocate_status != kTfLiteOk) { + PX4_ERR("AllocateTensors() failed"); + return -1; + } + + _input_tensor = _control_interpreter->input(0); + if (_input_tensor == nullptr) { + PX4_ERR("Input tensor is null"); + return -1; + } + if (_allocation_interpreter->input(0) == nullptr) { + PX4_ERR("Input tensor is null"); + return -1; + } + + return PX4_OK; +} + + +void MulticopterNeuralNetworkControl::PopulateInputTensor() { + // Creates a 15 element input tensor for the neural network [pos_err(3), lin_vel(3), att(6), ang_vel(3)] + + // transform observations in correct frame + matrix::Dcmf frame_transf; + frame_transf(0, 0) = 1.0f; + frame_transf(0, 1) = 0.0f; + frame_transf(0, 2) = 0.0f; + frame_transf(1, 0) = 0.0f; + frame_transf(1, 1) = -1.0f; + frame_transf(1, 2) = 0.0f; + frame_transf(2, 0) = 0.0f; + frame_transf(2, 1) = 0.0f; + frame_transf(2, 2) = -1.0f; + + matrix::Dcmf frame_transf_2; + frame_transf_2(0, 0) = 0.0f; + frame_transf_2(0, 1) = 1.0f; + frame_transf_2(0, 2) = 0.0f; + frame_transf_2(1, 0) = -1.0f; + frame_transf_2(1, 1) = 0.0f; + frame_transf_2(1, 2) = 0.0f; + frame_transf_2(2, 0) = 0.0f; + frame_transf_2(2, 1) = 0.0f; + frame_transf_2(2, 2) = 1.0f; + + matrix::Vector3f position_local = matrix::Vector3f(_position.x, _position.y, _position.z); + position_local = frame_transf * frame_transf_2 * position_local; + + matrix::Vector3f position_setpoint_local = matrix::Vector3f(_position_setpoint.x, _position_setpoint.y, _position_setpoint.z); + position_setpoint_local = frame_transf * frame_transf_2 * position_setpoint_local; + + matrix::Vector3f linear_velocity_local = matrix::Vector3f(_position.vx, _position.vy, _position.vz); + linear_velocity_local = frame_transf * frame_transf_2 * linear_velocity_local; + + matrix::Quatf attitude = matrix::Quatf(_attitude.q); + matrix::Dcmf _attitude_local_mat = frame_transf * (frame_transf_2 * matrix::Dcmf(attitude)) * frame_transf.transpose(); + + matrix::Vector3f angular_vel_local = matrix::Vector3f( _angular_velocity.xyz[0], _angular_velocity.xyz[1], _angular_velocity.xyz[2]); + angular_vel_local = frame_transf * angular_vel_local; + + _input_tensor->data.f[0] = position_setpoint_local(0) - position_local(0); + _input_tensor->data.f[1] = position_setpoint_local(1) - position_local(1); + _input_tensor->data.f[2] = position_setpoint_local(2) - position_local(2); + _input_tensor->data.f[3] = _attitude_local_mat(0, 0); + _input_tensor->data.f[4] = _attitude_local_mat(0, 1); + _input_tensor->data.f[5] = _attitude_local_mat(0, 2); + _input_tensor->data.f[6] = _attitude_local_mat(1, 0); + _input_tensor->data.f[7] = _attitude_local_mat(1, 1); + _input_tensor->data.f[8] = _attitude_local_mat(1, 2); + _input_tensor->data.f[9] = linear_velocity_local(0); + _input_tensor->data.f[10] = linear_velocity_local(1); + _input_tensor->data.f[11] = linear_velocity_local(2); + _input_tensor->data.f[12] = angular_vel_local(0); + _input_tensor->data.f[13] = angular_vel_local(1); + _input_tensor->data.f[14] = angular_vel_local(2); + + if (_param_debug_input_tensor.get()) { + PX4_INFO("Input tensor:"); + PX4_INFO("pos_err: [%f, %f, %f]", static_cast(_input_tensor->data.f[0]), static_cast(_input_tensor->data.f[1]), static_cast(_input_tensor->data.f[2])); + PX4_INFO("att: [%f, %f, %f, %f, %f, %f]", static_cast(_input_tensor->data.f[3]), static_cast(_input_tensor->data.f[4]), static_cast(_input_tensor->data.f[5]), static_cast(_input_tensor->data.f[6]), static_cast(_input_tensor->data.f[7]), static_cast(_input_tensor->data.f[8])); + PX4_INFO("vel: [%f, %f, %f]", static_cast(_input_tensor->data.f[9]), static_cast(_input_tensor->data.f[10]), static_cast(_input_tensor->data.f[11])); + PX4_INFO("ang_vel: [%f, %f, %f]", static_cast(_input_tensor->data.f[12]), static_cast(_input_tensor->data.f[13]), static_cast(_input_tensor->data.f[14])); + } + return; +} + +void MulticopterNeuralNetworkControl::PublishOutput(float* command_actions) { + + actuator_motors_s actuator_motors; + actuator_motors.timestamp = hrt_absolute_time(); + + actuator_motors.control[0] = PX4_ISFINITE(command_actions[0]) ? command_actions[0] : NAN; + actuator_motors.control[1] = PX4_ISFINITE(command_actions[2]) ? command_actions[2] : NAN; + actuator_motors.control[2] = PX4_ISFINITE(command_actions[3]) ? command_actions[3] : NAN; + actuator_motors.control[3] = PX4_ISFINITE(command_actions[1]) ? command_actions[1] : NAN; + actuator_motors.control[4] = -NAN; + actuator_motors.control[5] = -NAN; + actuator_motors.control[6] = -NAN; + actuator_motors.control[7] = -NAN; + actuator_motors.control[8] = -NAN; + actuator_motors.control[9] = -NAN; + actuator_motors.control[10] = -NAN; + actuator_motors.control[11] = -NAN; + actuator_motors.reversible_flags = 0; + + _actuator_motors_pub.publish(actuator_motors); +} + + + +inline void MulticopterNeuralNetworkControl::RescaleActions() { + //static const float _thrust_coefficient = 0.00001286412; + static const float _thrust_coefficient = 0.00001006412; + const float a = 0.8f; + const float b = (1.f - 0.8f); + const float tmp1 = b / (2.f * a); + const float tmp2 = b * b / (4.f * a * a); + const int max_rpm = 22000.0f; + const int min_rpm = 1000.0f; + for (int i = 0; i < 4; i++) { + if (_output_tensor->data.f[i] < 0.2f){ + _output_tensor->data.f[i] = 0.2f; + } + else if (_output_tensor->data.f[i] > 1.2f){ + _output_tensor->data.f[i] = 1.2f; + } + float rps = _output_tensor->data.f[i]/_thrust_coefficient; + rps = sqrt(rps); + float rpm = rps * 60.0f; + _output_tensor->data.f[i] = (rpm*2.0f - max_rpm - min_rpm) / (max_rpm - min_rpm); + _output_tensor->data.f[i] = a * (((_output_tensor->data.f[i] + 1.0f) / 2.0f + tmp1) * ((_output_tensor->data.f[i] + 1.0f) / 2.0f + tmp1) - tmp2); + } +} + + +int MulticopterNeuralNetworkControl::task_spawn(int argc, char *argv[]) +{ + // This function loads the model, sets up the interpreter, allocates memory for the model's tensors, and prepares the input data. + MulticopterNeuralNetworkControl *instance = new MulticopterNeuralNetworkControl(); + + if (instance){ + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() and instance->InitializeNetwork() == PX4_OK) { + return PX4_OK; + } + else { + PX4_ERR("init failed"); + } + } else { + PX4_ERR("alloc failed"); + } + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +void MulticopterNeuralNetworkControl::Run() +{ + if (should_exit()) + { + _angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + //std::chrono::time_point start1, end1; + //std::chrono::time_point start2, end2; + //start1 = std::chrono::system_clock::now(); + + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); + } + + vehicle_control_mode_s vehicle_control_mode; + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + //_use_neural = vehicle_control_mode.flag_control_neural_enabled; + _use_neural = true; + } + + if(!_use_neural) { + // If the neural network flight mode is not enabled, do nothing + return; + } + + // run controller on angular velocity updates + if (_angular_velocity_sub.update(&_angular_velocity)) { + _last_run = _angular_velocity.timestamp_sample; + + if(_attitude_sub.updated()) { + _attitude_sub.copy(&_attitude); + } + if(_position_sub.updated()) { + _position_sub.copy(&_position); + } + if(_position_setpoint_sub.updated()) { + _position_setpoint_sub.copy(&_position_setpoint); + } + + PopulateInputTensor(); + + // Run inference + //start2 = std::chrono::system_clock::now(); + TfLiteStatus invoke_status_control = _control_interpreter->Invoke(); + //end2 = std::chrono::system_clock::now(); + if (invoke_status_control != kTfLiteOk) { + PX4_ERR("Invoke() failed"); + return; + } + // Print the output + TfLiteTensor* control_output_tensor = _control_interpreter->output(0); + if (control_output_tensor == nullptr) { + PX4_ERR("Output tensor is null"); + return; + } + + TfLiteTensor* allocation_input_tensor = _allocation_interpreter->input(0); + // rescale actions + allocation_input_tensor->data.f[0] = control_output_tensor->data.f[0] * 0; + allocation_input_tensor->data.f[1] = control_output_tensor->data.f[1] * 0; + allocation_input_tensor->data.f[2] = control_output_tensor->data.f[2] * 2.0f + 2.8f; + allocation_input_tensor->data.f[3] = control_output_tensor->data.f[3] * 0.32f; + allocation_input_tensor->data.f[4] = control_output_tensor->data.f[4] * 0.32f; + allocation_input_tensor->data.f[5] = control_output_tensor->data.f[5] * 0.02f; + + TfLiteStatus invoke_status = _allocation_interpreter->Invoke(); + if (invoke_status != kTfLiteOk) { + PX4_ERR("Invoke() failed"); + return; + } + _output_tensor = _allocation_interpreter->output(0); + if (_output_tensor == nullptr) { + PX4_ERR("Output tensor is null"); + return; + } + + // Convert the output tensor to actuator values + RescaleActions(); + + // Publish the actuator values + PublishOutput(_output_tensor->data.f); + + //end1 = std::chrono::system_clock::now(); + + if(_param_debug_output_tensor.get()) { + PX4_INFO("Actuator motors values:"); + PX4_INFO("[%f, %f, %f, %f]", static_cast(_output_tensor->data.f[0]), static_cast(_output_tensor->data.f[1]), static_cast(_output_tensor->data.f[2]), static_cast(_output_tensor->data.f[3])); + } + + // if(_param_debug_inference_time.get()) { + // PX4_INFO("Total controller time: %f microseconds", double(std::chrono::duration_cast(end1 - start1).count())); + // PX4_INFO("Inference time: %f microseconds", double(std::chrono::duration_cast(end2 - start2).count())); + // } + } + perf_end(_loop_perf); +} + +int MulticopterNeuralNetworkControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterNeuralNetworkControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Multicopter Neural Network Control module. +This module is an end-to-end neural network control system for multicopters. +It takes in 15 input values and outputs 4 control actions. +Inputs: [pos_err(3), att(6), vel(3), ang_vel(3)] +Outputs: [Actuator motors(4)] +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_nn_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_nn_control_main(int argc, char *argv[]) +{ + return MulticopterNeuralNetworkControl::main(argc, argv); +} diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp new file mode 100644 index 000000000000..ee8df4716cce --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_nn_control.h + * Multicopter Neural Network Control module, from position setpoints to control allocator. + * + * @author Sindre Meyer Hegre + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Include model +#include "allocation_net.hpp" +#include "control_net.hpp" + +#include +#include +#include + +// Subscriptions +#include +#include +#include +#include +#include +#include + +// Publications +#include + +using namespace time_literals; // For the 1_s in the subscription callback + +class MulticopterNeuralNetworkControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + + MulticopterNeuralNetworkControl(); + ~MulticopterNeuralNetworkControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + // Functions + void PopulateInputTensor(); + void PublishOutput(float* command_actions); + void RescaleActions(); + int InitializeNetwork(); + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionCallbackWorkItem _angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + // Publications + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + + // Variables + bool _use_neural{false}; + perf_counter_t _loop_perf; /**< loop duration performance counter */ + hrt_abstime _last_run{0}; + tflite::MicroInterpreter* _control_interpreter; + tflite::MicroInterpreter* _allocation_interpreter; + TfLiteTensor* _input_tensor; + TfLiteTensor* _output_tensor; + vehicle_angular_velocity_s _angular_velocity; + vehicle_local_position_s _position; + vehicle_local_position_setpoint_s _position_setpoint; + vehicle_attitude_s _attitude; + + DEFINE_PARAMETERS( + (ParamBool) _param_debug_input_tensor, + (ParamBool) _param_debug_output_tensor, + (ParamBool) _param_debug_inference_time + ) +}; diff --git a/src/modules/mc_nn_control/mc_nn_control_params.c b/src/modules/mc_nn_control/mc_nn_control_params.c new file mode 100644 index 000000000000..5d47968621cb --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control_params.c @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_nn_control_params.c + * Parameters for the Multicopter Neural Network Control module + * + * @author Sindre Meyer Hegre + */ + +/** + * Toggle if the input tensor should be printed to the console + * + * @boolean + * @group Multicopter Neural Network Control + */ +PARAM_DEFINE_INT32(NN_IN_DEBUG, 0); + +/** + * Toggle if the output tensor should be printed to the console + * + * @boolean + * @group Multicopter Neural Network Control + */ +PARAM_DEFINE_INT32(NN_OUT_DEBUG, 0); + +/** + * Toggle if the inference time should be printed to the console + * + * @boolean + * @group Multicopter Neural Network Control + */ +PARAM_DEFINE_INT32(NN_TIME_DEBUG, 0); From 8fe4e32652334003d5cc47d0befa1d0a96f0bbde Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Thu, 20 Feb 2025 16:05:12 +0100 Subject: [PATCH 02/11] Added neural flight mode --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 +++++ msg/versioned/VehicleControlMode.msg | 1 + msg/versioned/VehicleStatus.msg | 2 +- src/drivers/rc/crsf_rc/CrsfRc.cpp | 1 + src/drivers/rc_input/crsf_telemetry.cpp | 1 + src/lib/events/enums.json | 8 ++++++++ src/lib/modes/standard_modes.hpp | 1 + src/lib/modes/ui.hpp | 3 ++- src/modules/commander/Commander.cpp | 10 +++++++++- src/modules/commander/ModeUtil/control_mode.cpp | 11 +++++++++++ src/modules/commander/module.yaml | 1 + src/modules/commander/px4_custom_mode.h | 6 ++++++ src/modules/mc_nn_control/mc_nn_control.cpp | 3 +-- 13 files changed, 48 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index d5d9989c8666..5fef04f2d0e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -36,3 +36,8 @@ mc_pos_control start # Start Multicopter Land Detector. # land_detector start multicopter + +# +# Start Multicopter Neural Network Controller. +# +mc_nn_control start diff --git a/msg/versioned/VehicleControlMode.msg b/msg/versioned/VehicleControlMode.msg index 0a1fdedfbf14..676313d4c598 100644 --- a/msg/versioned/VehicleControlMode.msg +++ b/msg/versioned/VehicleControlMode.msg @@ -17,6 +17,7 @@ bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_allocation_enabled # true if control allocation is enabled bool flag_control_termination_enabled # true if flighttermination is enabled +bool flag_control_neural_enabled # true if neural networks are used for control # TODO: use dedicated topic for external requests uint8 source_id # Mode ID (nav_state) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index b9a3edd91fbe..5f28806773af 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -40,7 +40,7 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode uint8 NAVIGATION_STATE_POSITION_SLOW = 6 -uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_AUTO_NEURAL = 7 # Neural network control mode uint8 NAVIGATION_STATE_FREE4 = 8 uint8 NAVIGATION_STATE_FREE3 = 9 uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 3ea6c0ac273d..00639923752c 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -294,6 +294,7 @@ void CrsfRc::Run() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: flight_mode = "Auto"; break; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 60286e0f40aa..0ecc94ee23dc 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -159,6 +159,7 @@ bool CRSFTelemetry::send_flight_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: flight_mode = "Auto"; break; diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 3b73cff62ca7..4c6fef39adc1 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -111,6 +111,10 @@ "name": "vtol_takeoff", "description": "VTOL Takeoff" }, + "168034304": { + "name": "neural", + "description": "Neural" + }, "8388608": { "name": "external1", "description": "External 1" @@ -604,6 +608,10 @@ "name": "external8", "description": "External 8" }, + "24": { + "name": "auto_neural", + "description": "Neural" + }, "255": { "name": "unknown", "description": "[Unknown]" diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp index b009d815d425..2a0a9600d2d9 100644 --- a/src/lib/modes/standard_modes.hpp +++ b/src/lib/modes/standard_modes.hpp @@ -51,6 +51,7 @@ enum class StandardMode : uint8_t { MISSION = 6, LAND = 7, TAKEOFF = 8, + NEUTRAL = 9, }; /** diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index 9e4156ae007a..38cf5e2c89d6 100644 --- a/src/lib/modes/ui.hpp +++ b/src/lib/modes/ui.hpp @@ -52,6 +52,7 @@ static inline uint32_t getValidNavStates() (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) | (1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL) | (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) | (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) | (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) | @@ -74,7 +75,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "Hold", "Return", "Position Slow", - "7: unallocated", + "Neural", "8: unallocated", "9: unallocated", "Acro", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f0537c7c6e86..e5c1a4f6e7c5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -392,6 +392,10 @@ int Commander::custom_command(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL); + } else if (!strcmp(argv[1], "auto:neural")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_NEURAL); + } else if (!strcmp(argv[1], "acro")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO); @@ -844,6 +848,10 @@ Commander::handle_command(const vehicle_command_s &cmd) desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; + case PX4_CUSTOM_SUB_MODE_AUTO_NEURAL: + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL; + break; + case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8: desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1); break; @@ -3020,7 +3028,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("land"); PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); - PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1", + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:neural|auto:takeoff|auto:land|auto:precland|ext1", "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("pair"); PRINT_MODULE_USAGE_COMMAND("lockdown"); diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index fbebc7b93d8b..e039952c1686 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -100,6 +100,17 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_allocation_enabled = true; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: + vehicle_control_mode.flag_control_auto_enabled = true; + vehicle_control_mode.flag_control_position_enabled = true; + vehicle_control_mode.flag_control_velocity_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = true; + vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_neural_enabled = true; + break; + case vehicle_status_s::NAVIGATION_STATE_ACRO: vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index d2113d935955..bc57be1428ee 100644 --- a/src/modules/commander/module.yaml +++ b/src/modules/commander/module.yaml @@ -38,6 +38,7 @@ parameters: 8: Stabilized 12: Follow Me 13: Precision Land + 14: Neural 100: External Mode 1 101: External Mode 2 102: External Mode 3 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e4936982ff1b..0d15e8557fea 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -64,6 +64,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05 PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, + PX4_CUSTOM_SUB_MODE_AUTO_NEURAL, PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF, PX4_CUSTOM_SUB_MODE_EXTERNAL1, PX4_CUSTOM_SUB_MODE_EXTERNAL2, @@ -136,6 +137,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_NEURAL; + break; + case vehicle_status_s::NAVIGATION_STATE_ACRO: custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 5b691d177905..952bc79444c7 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -297,8 +297,7 @@ void MulticopterNeuralNetworkControl::Run() vehicle_control_mode_s vehicle_control_mode; if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { - //_use_neural = vehicle_control_mode.flag_control_neural_enabled; - _use_neural = true; + _use_neural = vehicle_control_mode.flag_control_neural_enabled; } if(!_use_neural) { From 51ae6aeb1be1c4e05c3fb73768dac3d198d53bf6 Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Fri, 21 Feb 2025 10:09:35 +0100 Subject: [PATCH 03/11] Add posibility to read of inference times --- src/modules/mc_nn_control/mc_nn_control.cpp | 30 +++++++++++-------- src/modules/mc_nn_control/mc_nn_control.hpp | 5 +++- .../mc_nn_control/mc_nn_control_params.c | 28 +++++++++++++++++ 3 files changed, 50 insertions(+), 13 deletions(-) diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 952bc79444c7..70df7ea48787 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -38,8 +38,7 @@ */ #include "mc_nn_control.hpp" -//#include -#include +#include namespace { using NNControlOpResolver = tflite::MicroMutableOpResolver<4>; // This number should be the number of operations in the model, like tanh and fully connected @@ -285,9 +284,7 @@ void MulticopterNeuralNetworkControl::Run() perf_begin(_loop_perf); - //std::chrono::time_point start1, end1; - //std::chrono::time_point start2, end2; - //start1 = std::chrono::system_clock::now(); + hrt_abstime start_time1 = hrt_absolute_time(); if (_parameter_update_sub.updated()) { parameter_update_s param_update; @@ -322,9 +319,9 @@ void MulticopterNeuralNetworkControl::Run() PopulateInputTensor(); // Run inference - //start2 = std::chrono::system_clock::now(); + hrt_abstime start_time2 = hrt_absolute_time(); TfLiteStatus invoke_status_control = _control_interpreter->Invoke(); - //end2 = std::chrono::system_clock::now(); + hrt_abstime inference_time_control = hrt_absolute_time() - start_time2; if (invoke_status_control != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -345,7 +342,9 @@ void MulticopterNeuralNetworkControl::Run() allocation_input_tensor->data.f[4] = control_output_tensor->data.f[4] * 0.32f; allocation_input_tensor->data.f[5] = control_output_tensor->data.f[5] * 0.02f; + hrt_abstime start_time3 = hrt_absolute_time(); TfLiteStatus invoke_status = _allocation_interpreter->Invoke(); + hrt_abstime inference_time_allocation = hrt_absolute_time() - start_time3; if (invoke_status != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -362,17 +361,24 @@ void MulticopterNeuralNetworkControl::Run() // Publish the actuator values PublishOutput(_output_tensor->data.f); - //end1 = std::chrono::system_clock::now(); + hrt_abstime full_controller_time = hrt_absolute_time() - start_time1; if(_param_debug_output_tensor.get()) { PX4_INFO("Actuator motors values:"); PX4_INFO("[%f, %f, %f, %f]", static_cast(_output_tensor->data.f[0]), static_cast(_output_tensor->data.f[1]), static_cast(_output_tensor->data.f[2]), static_cast(_output_tensor->data.f[3])); } - // if(_param_debug_inference_time.get()) { - // PX4_INFO("Total controller time: %f microseconds", double(std::chrono::duration_cast(end1 - start1).count())); - // PX4_INFO("Inference time: %f microseconds", double(std::chrono::duration_cast(end2 - start2).count())); - // } + if(_param_debug_inference_time.get()) { + _param_control_inference_time.set(inference_time_control); + _param_control_inference_time.commit(); + _param_allocation_inference_time.set(inference_time_allocation); + _param_allocation_inference_time.commit(); + _param_full_inference_time.set(full_controller_time); + _param_full_inference_time.commit(); + PX4_INFO("Inference time control net: %llu us", (unsigned long long)inference_time_control); + PX4_INFO("Inference time allocation net: %llu us", (unsigned long long)inference_time_allocation); + PX4_INFO("Full controller time : %llu us", (unsigned long long)full_controller_time); + } } perf_end(_loop_perf); } diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp index ee8df4716cce..fa3a24b695df 100644 --- a/src/modules/mc_nn_control/mc_nn_control.hpp +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -130,6 +130,9 @@ class MulticopterNeuralNetworkControl : public ModuleBase) _param_debug_input_tensor, (ParamBool) _param_debug_output_tensor, - (ParamBool) _param_debug_inference_time + (ParamBool) _param_debug_inference_time, + (ParamFloat) _param_control_inference_time, + (ParamFloat) _param_allocation_inference_time, + (ParamFloat) _param_full_inference_time ) }; diff --git a/src/modules/mc_nn_control/mc_nn_control_params.c b/src/modules/mc_nn_control/mc_nn_control_params.c index 5d47968621cb..7fa22ab2dc56 100644 --- a/src/modules/mc_nn_control/mc_nn_control_params.c +++ b/src/modules/mc_nn_control/mc_nn_control_params.c @@ -61,3 +61,31 @@ PARAM_DEFINE_INT32(NN_OUT_DEBUG, 0); * @group Multicopter Neural Network Control */ PARAM_DEFINE_INT32(NN_TIME_DEBUG, 0); + + +/** + * Save the inference time as logging is not implemented yet + * + * @group Multicopter Neural Network Control + * @unit us + * + */ +PARAM_DEFINE_FLOAT(NN_CONTROL_INF, 0.0f); + +/** + * Save the inference time as logging is not implemented yet + * + * @group Multicopter Neural Network Control + * @unit us + * + */ +PARAM_DEFINE_FLOAT(NN_ALLOC_INF, 0.0f); + +/** + * Save the inference time as logging is not implemented yet + * + * @group Multicopter Neural Network Control + * @unit us + * + */ +PARAM_DEFINE_FLOAT(NN_FULL_INF, 0.0f); From a09e1a54209fa4f427d8d796da1250f19f18d1e0 Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Wed, 19 Feb 2025 13:05:39 +0100 Subject: [PATCH 04/11] Add tflm to px4 with module - Add TensorFlow Lite Micro(TFLM) as a library in px4 - Make a module that uses neural network inference for control, which uses TFLM for inference - Make board config files for PX4 with neural module --- .gitmodules | 4 + CMakeLists.txt | 2 +- boards/mro/pixracerpro/neural.px4board | 99 +++++ boards/px4/fmu-v6c/neural.px4board | 93 ++++ boards/px4/sitl/neural.px4board | 80 ++++ .../nuttx/cmake/Toolchain-arm-none-eabi.cmake | 89 +++- src/lib/CMakeLists.txt | 1 + src/lib/tflm/CMakeLists.txt | 93 ++++ src/lib/tflm/tflite_micro | 1 + src/modules/mc_nn_control/.gitignore | 2 + src/modules/mc_nn_control/CMakeLists.txt | 56 +++ src/modules/mc_nn_control/Kconfig | 12 + src/modules/mc_nn_control/allocation_net.hpp | 4 + src/modules/mc_nn_control/control_net.hpp | 4 + src/modules/mc_nn_control/mc_nn_control.cpp | 412 ++++++++++++++++++ src/modules/mc_nn_control/mc_nn_control.hpp | 135 ++++++ .../mc_nn_control/mc_nn_control_params.c | 63 +++ 17 files changed, 1127 insertions(+), 23 deletions(-) create mode 100644 boards/mro/pixracerpro/neural.px4board create mode 100644 boards/px4/fmu-v6c/neural.px4board create mode 100644 boards/px4/sitl/neural.px4board create mode 100644 src/lib/tflm/CMakeLists.txt create mode 160000 src/lib/tflm/tflite_micro create mode 100644 src/modules/mc_nn_control/.gitignore create mode 100644 src/modules/mc_nn_control/CMakeLists.txt create mode 100644 src/modules/mc_nn_control/Kconfig create mode 100644 src/modules/mc_nn_control/allocation_net.hpp create mode 100644 src/modules/mc_nn_control/control_net.hpp create mode 100644 src/modules/mc_nn_control/mc_nn_control.cpp create mode 100644 src/modules/mc_nn_control/mc_nn_control.hpp create mode 100644 src/modules/mc_nn_control/mc_nn_control_params.c diff --git a/.gitmodules b/.gitmodules index 1ebafcf5a4e5..15a78d33382d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -89,3 +89,7 @@ [submodule "src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan"] path = src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan url = https://github.com/dronecan/pydronecan +[submodule "src/lib/tflm/tflite_micro"] + path = src/lib/tflm/tflite_micro + url = git@github.com:tensorflow/tflite-micro.git + branch = main diff --git a/CMakeLists.txt b/CMakeLists.txt index 55362b1425c7..f89d76652c11 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -267,7 +267,7 @@ endif() set(package-contact "px4users@googlegroups.com") -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) diff --git a/boards/mro/pixracerpro/neural.px4board b/boards/mro/pixracerpro/neural.px4board new file mode 100644 index 000000000000..2d8f20446902 --- /dev/null +++ b/boards/mro/pixracerpro/neural.px4board @@ -0,0 +1,99 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI085=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=n +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MC_NN_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v6c/neural.px4board b/boards/px4/fmu-v6c/neural.px4board new file mode 100644 index 000000000000..c888615aceab --- /dev/null +++ b/boards/px4/fmu-v6c/neural.px4board @@ -0,0 +1,93 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_INS=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=n +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_NN_CONTROL=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/sitl/neural.px4board b/boards/px4/sitl/neural.px4board new file mode 100644 index 000000000000..9afbf845f180 --- /dev/null +++ b/boards/px4/sitl/neural.px4board @@ -0,0 +1,80 @@ +CONFIG_PLATFORM_POSIX=y +CONFIG_BOARD_TESTING=y +CONFIG_BOARD_ETHERNET=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_EKF2=y +CONFIG_EKF2_VERBOSE_STATUS=y +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_FIGURE_OF_EIGHT=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_NN_CONTROL=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y +CONFIG_MODULES_PAYLOAD_DELIVERER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_REPLAY=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_COMMON_SIMULATION=y +CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DYN=y +CONFIG_SYSTEMCMDS_FAILURE=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SHUTDOWN=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_DYN_HELLO=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_EXAMPLES_FAKE_IMU=y +CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y +CONFIG_EXAMPLES_PX4_SIMPLE_APP=y +CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake index 85e5caa00747..a47ab8442f27 100644 --- a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake +++ b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake @@ -1,3 +1,48 @@ +# # arm-none-eabi-gcc toolchain + +# set(CMAKE_SYSTEM_NAME Generic) +# set(CMAKE_SYSTEM_VERSION 1) + +# set(triple arm-none-eabi) +# set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) +# set(TOOLCHAIN_PREFIX ${triple}) + +# set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc) +# set(CMAKE_C_COMPILER_TARGET ${triple}) + +# set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++) +# set(CMAKE_CXX_COMPILER_TARGET ${triple}) + +# set(CMAKE_ASM_COMPILER ${TOOLCHAIN_PREFIX}-gcc) + +# # needed for test compilation +# set(CMAKE_EXE_LINKER_FLAGS_INIT "--specs=nosys.specs") + +# # compiler tools +# find_program(CMAKE_AR ${TOOLCHAIN_PREFIX}-gcc-ar) +# find_program(CMAKE_GDB ${TOOLCHAIN_PREFIX}-gdb) +# find_program(CMAKE_LD ${TOOLCHAIN_PREFIX}-ld) +# find_program(CMAKE_LINKER ${TOOLCHAIN_PREFIX}-ld) +# find_program(CMAKE_NM ${TOOLCHAIN_PREFIX}-gcc-nm) +# find_program(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}-objcopy) +# find_program(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}-objdump) +# find_program(CMAKE_RANLIB ${TOOLCHAIN_PREFIX}-gcc-ranlib) +# find_program(CMAKE_STRIP ${TOOLCHAIN_PREFIX}-strip) + +# set(CMAKE_FIND_ROOT_PATH get_file_component(${CMAKE_C_COMPILER} PATH)) +# set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +# set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +# set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# # os tools +# foreach(tool grep make) +# string(TOUPPER ${tool} TOOL) +# find_program(${TOOL} ${tool}) +# if(NOT ${TOOL}) +# message(FATAL_ERROR "could not find ${tool}") +# endif() +# endforeach() + # arm-none-eabi-gcc toolchain set(CMAKE_SYSTEM_NAME Generic) @@ -7,38 +52,38 @@ set(triple arm-none-eabi) set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) set(TOOLCHAIN_PREFIX ${triple}) -set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc) -set(CMAKE_C_COMPILER_TARGET ${triple}) - -set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++) -set(CMAKE_CXX_COMPILER_TARGET ${triple}) +# Define the path to the new toolchain +set(NEW_TOOLCHAIN_PATH "/home/sindre/Dropbox/Studier/2025_Var/dev/PX4-Autopilot-public/src/lib/tflm/tflite_micro/tensorflow/lite/micro/tools/make/downloads/gcc_embedded/bin") -set(CMAKE_ASM_COMPILER ${TOOLCHAIN_PREFIX}-gcc) +# Set the compiler paths +set(CMAKE_C_COMPILER ${NEW_TOOLCHAIN_PATH}/${TOOLCHAIN_PREFIX}-gcc) +set(CMAKE_CXX_COMPILER ${NEW_TOOLCHAIN_PATH}/${TOOLCHAIN_PREFIX}-g++) +set(CMAKE_ASM_COMPILER ${NEW_TOOLCHAIN_PATH}/${TOOLCHAIN_PREFIX}-gcc) # needed for test compilation set(CMAKE_EXE_LINKER_FLAGS_INIT "--specs=nosys.specs") # compiler tools -find_program(CMAKE_AR ${TOOLCHAIN_PREFIX}-gcc-ar) -find_program(CMAKE_GDB ${TOOLCHAIN_PREFIX}-gdb) -find_program(CMAKE_LD ${TOOLCHAIN_PREFIX}-ld) -find_program(CMAKE_LINKER ${TOOLCHAIN_PREFIX}-ld) -find_program(CMAKE_NM ${TOOLCHAIN_PREFIX}-gcc-nm) -find_program(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}-objcopy) -find_program(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}-objdump) -find_program(CMAKE_RANLIB ${TOOLCHAIN_PREFIX}-gcc-ranlib) -find_program(CMAKE_STRIP ${TOOLCHAIN_PREFIX}-strip) - -set(CMAKE_FIND_ROOT_PATH get_file_component(${CMAKE_C_COMPILER} PATH)) +find_program(CMAKE_AR ${TOOLCHAIN_PREFIX}-gcc-ar PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_GDB ${TOOLCHAIN_PREFIX}-gdb PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_LD ${TOOLCHAIN_PREFIX}-ld PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_LINKER ${TOOLCHAIN_PREFIX}-ld PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_NM ${TOOLCHAIN_PREFIX}-gcc-nm PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}-objcopy PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}-objdump PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_RANLIB ${TOOLCHAIN_PREFIX}-gcc-ranlib PATHS ${NEW_TOOLCHAIN_PATH}) +find_program(CMAKE_STRIP ${TOOLCHAIN_PREFIX}-strip PATHS ${NEW_TOOLCHAIN_PATH}) + +set(CMAKE_FIND_ROOT_PATH ${NEW_TOOLCHAIN_PATH}) set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) # os tools foreach(tool grep make) - string(TOUPPER ${tool} TOOL) - find_program(${TOOL} ${tool}) - if(NOT ${TOOL}) - message(FATAL_ERROR "could not find ${tool}") - endif() + string(TOUPPER ${tool} TOOL) + find_program(${TOOL} ${tool}) + if(NOT ${TOOL}) + message(FATAL_ERROR "could not find ${tool}") + endif() endforeach() diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index a9396caa1fa9..9b2c7928f286 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -76,6 +76,7 @@ add_subdirectory(systemlib EXCLUDE_FROM_ALL) add_subdirectory(system_identification EXCLUDE_FROM_ALL) add_subdirectory(tecs EXCLUDE_FROM_ALL) add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL) +add_subdirectory(tflm EXCLUDE_FROM_ALL) add_subdirectory(timesync EXCLUDE_FROM_ALL) add_subdirectory(tinybson EXCLUDE_FROM_ALL) add_subdirectory(tunes EXCLUDE_FROM_ALL) diff --git a/src/lib/tflm/CMakeLists.txt b/src/lib/tflm/CMakeLists.txt new file mode 100644 index 000000000000..23752edbebee --- /dev/null +++ b/src/lib/tflm/CMakeLists.txt @@ -0,0 +1,93 @@ +############################################################################ +# +# Copyright (c) 2017-2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_git_submodule(TARGET git_tflite-micro PATH tflite_micro) + +set(TFLITE_DOWNLOADS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/tools/make/downloads) +set(TFLITE_GEN_DIR ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/gen/cortex_m_generic_cortex-m7_default_cmsis_nn_gcc) + +get_directory_property(FLAGS COMPILE_OPTIONS) +list(REMOVE_ITEM FLAGS "-Wcast-align") +set_directory_properties(PROPERTIES COMPILE_OPTIONS "${FLAGS}") + + +file(GLOB TFLITE_MICRO_SRCS + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/arena_allocator/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/memory_planner/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/tflite_bridge/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/micro/kernels/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/kernels/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/kernels/internal/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/core/api/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/core/c/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/lite/schema/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/compiler/mlir/lite/core/api/*.cc + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro/tensorflow/compiler/mlir/lite/schema/*.cc +) + +# Filter out tests as they cause errors +list(FILTER TFLITE_MICRO_SRCS EXCLUDE REGEX ".*_test.*\\.cc$") + +set(TFLM_INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/tflite_micro + ${TFLITE_DOWNLOADS_DIR} + ${TFLITE_DOWNLOADS_DIR}/flatbuffers/include + ${TFLITE_DOWNLOADS_DIR}/ruy + ${TFLITE_DOWNLOADS_DIR}/gemmlowp + ${TFLITE_DOWNLOADS_DIR}/kissfft + ${TFLITE_DOWNLOADS_DIR}/cmsis/Cortex_DFP/Device/ARMCM7/Include + ${TFLITE_DOWNLOADS_DIR}/cmsis/CMSIS/Core/Include + ${TFLITE_DOWNLOADS_DIR}/cmsis + ${TFLITE_DOWNLOADS_DIR}/cmsis_nn + ${TFLITE_DOWNLOADS_DIR}/cmsis_nn/Include + ${TFLITE_GEN_DIR}/genfiles + ) + + # Add C++ 17 std lib if building for NuttX + if(CONFIG_BOARD_TOOLCHAIN STREQUAL "arm-none-eabi") + list(APPEND TFLM_INCLUDE_DIRS + ${TFLITE_DOWNLOADS_DIR}/include/13.2.1 + ${TFLITE_DOWNLOADS_DIR}/include/13.2.1/arm-none-eabi + ) +endif() + +px4_add_library(tflm ${TFLITE_MICRO_SRCS}) + +target_include_directories(tflm PUBLIC ${TFLM_INCLUDE_DIRS}) + +target_compile_options(tflm PUBLIC + -Wno-float-equal + -Wno-shadow +) diff --git a/src/lib/tflm/tflite_micro b/src/lib/tflm/tflite_micro new file mode 160000 index 000000000000..ef6459127069 --- /dev/null +++ b/src/lib/tflm/tflite_micro @@ -0,0 +1 @@ +Subproject commit ef64591270691022a329cf04ba9e73ecfb15ddb8 diff --git a/src/modules/mc_nn_control/.gitignore b/src/modules/mc_nn_control/.gitignore new file mode 100644 index 000000000000..7e06bb76f920 --- /dev/null +++ b/src/modules/mc_nn_control/.gitignore @@ -0,0 +1,2 @@ +control_net.cpp +allocation_net.cpp diff --git a/src/modules/mc_nn_control/CMakeLists.txt b/src/modules/mc_nn_control/CMakeLists.txt new file mode 100644 index 000000000000..84f0a15be5e1 --- /dev/null +++ b/src/modules/mc_nn_control/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# Copyright (c) 2024-2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +add_compile_options(-Wno-float-equal) +get_directory_property(FLAGS COMPILE_OPTIONS) +list(REMOVE_ITEM FLAGS "-Wcast-align") +set_directory_properties(PROPERTIES COMPILE_OPTIONS "${FLAGS}") + +px4_add_module( + MODULE mc_nn_control + MAIN mc_nn_control + COMPILE_FLAGS + SRCS + mc_nn_control.cpp + mc_nn_control.hpp + control_net.cpp + control_net.hpp + allocation_net.cpp + allocation_net.hpp + DEPENDS + tflm + px4_work_queue + mathlib +) +target_link_libraries(mc_nn_control PRIVATE tflm) +target_include_directories(mc_nn_control PRIVATE + ${CMAKE_SOURCE_DIR}/src/lib/tflm) diff --git a/src/modules/mc_nn_control/Kconfig b/src/modules/mc_nn_control/Kconfig new file mode 100644 index 000000000000..30829664452c --- /dev/null +++ b/src/modules/mc_nn_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_MC_NN_CONTROL + bool "mc_nn_control" + default n + ---help--- + Enable support for mc_nn_control" + +menuconfig USER_MC_NN_CONTROL + bool "mc_nn_control running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_MC_NN_CONTROL + ---help--- + Put mc_nn_control in userspace memory diff --git a/src/modules/mc_nn_control/allocation_net.hpp b/src/modules/mc_nn_control/allocation_net.hpp new file mode 100644 index 000000000000..4dfaa9600a4a --- /dev/null +++ b/src/modules/mc_nn_control/allocation_net.hpp @@ -0,0 +1,4 @@ +#include + +constexpr unsigned int allocation_net_tflite_size = 8012; +extern const unsigned char allocation_net_tflite[]; diff --git a/src/modules/mc_nn_control/control_net.hpp b/src/modules/mc_nn_control/control_net.hpp new file mode 100644 index 000000000000..09f7e8fcb044 --- /dev/null +++ b/src/modules/mc_nn_control/control_net.hpp @@ -0,0 +1,4 @@ +#include + +constexpr unsigned int control_net_tflite_size = 8012; +extern const unsigned char control_net_tflite[]; diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp new file mode 100644 index 000000000000..5b691d177905 --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -0,0 +1,412 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mc_nn_control.cpp + * Multicopter Neural Network Control module, from position setpoints to control allocator. + * + * @author Sindre Meyer Hegre + */ + +#include "mc_nn_control.hpp" +//#include +#include + +namespace { +using NNControlOpResolver = tflite::MicroMutableOpResolver<4>; // This number should be the number of operations in the model, like tanh and fully connected + +TfLiteStatus RegisterOps(NNControlOpResolver& op_resolver) { + // Add the operations to you need to the op_resolver + TF_LITE_ENSURE_STATUS(op_resolver.AddFullyConnected()); + TF_LITE_ENSURE_STATUS(op_resolver.AddTanh()); + TF_LITE_ENSURE_STATUS(op_resolver.AddRelu()); + TF_LITE_ENSURE_STATUS(op_resolver.AddAdd()); + return kTfLiteOk; +} +} // namespace + +MulticopterNeuralNetworkControl::MulticopterNeuralNetworkControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + +} + +MulticopterNeuralNetworkControl::~MulticopterNeuralNetworkControl() +{ + perf_free(_loop_perf); +} + + +bool MulticopterNeuralNetworkControl::init() +{ + if (!_angular_velocity_sub.registerCallback()) + { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int MulticopterNeuralNetworkControl::InitializeNetwork() { + // Initialize the neural network + // Load the model + const tflite::Model* control_model = ::tflite::GetModel(control_net_tflite); // TODO: Replace with your model data variable + // if (control_model->version() != TFLITE_SCHEMA_VERSION) { + // PX4_ERR("Model provided is schema version %d not equal to supported version %d.", + // control_model->version(), TFLITE_SCHEMA_VERSION); + // return -1; + // } + + const tflite::Model* allocation_model = ::tflite::GetModel(allocation_net_tflite); // TODO: Replace with your model data variable + // if (allocation_model->version() != TFLITE_SCHEMA_VERSION) { + // PX4_ERR("Model provided is schema version %d not equal to supported version %d.", + // allocation_model->version(), TFLITE_SCHEMA_VERSION); + // return -1; + // } + + // Set up the interpreter + static NNControlOpResolver resolver; + if (RegisterOps(resolver) != kTfLiteOk) { + PX4_ERR("Failed to register ops"); + return -1; + } + constexpr int kTensorArenaSize = 10 * 1024; //TODO: Check this size + static uint8_t tensor_arena[kTensorArenaSize]; + _control_interpreter = new tflite::MicroInterpreter(control_model, resolver, tensor_arena, kTensorArenaSize); + _allocation_interpreter = new tflite::MicroInterpreter(allocation_model, resolver, tensor_arena, kTensorArenaSize); + + // Allocate memory for the model's tensors + TfLiteStatus allocate_status_control = _control_interpreter->AllocateTensors(); + if (allocate_status_control != kTfLiteOk) { + PX4_ERR("AllocateTensors() failed"); + return -1; + } + TfLiteStatus allocate_status = _allocation_interpreter->AllocateTensors(); + if (allocate_status != kTfLiteOk) { + PX4_ERR("AllocateTensors() failed"); + return -1; + } + + _input_tensor = _control_interpreter->input(0); + if (_input_tensor == nullptr) { + PX4_ERR("Input tensor is null"); + return -1; + } + if (_allocation_interpreter->input(0) == nullptr) { + PX4_ERR("Input tensor is null"); + return -1; + } + + return PX4_OK; +} + + +void MulticopterNeuralNetworkControl::PopulateInputTensor() { + // Creates a 15 element input tensor for the neural network [pos_err(3), lin_vel(3), att(6), ang_vel(3)] + + // transform observations in correct frame + matrix::Dcmf frame_transf; + frame_transf(0, 0) = 1.0f; + frame_transf(0, 1) = 0.0f; + frame_transf(0, 2) = 0.0f; + frame_transf(1, 0) = 0.0f; + frame_transf(1, 1) = -1.0f; + frame_transf(1, 2) = 0.0f; + frame_transf(2, 0) = 0.0f; + frame_transf(2, 1) = 0.0f; + frame_transf(2, 2) = -1.0f; + + matrix::Dcmf frame_transf_2; + frame_transf_2(0, 0) = 0.0f; + frame_transf_2(0, 1) = 1.0f; + frame_transf_2(0, 2) = 0.0f; + frame_transf_2(1, 0) = -1.0f; + frame_transf_2(1, 1) = 0.0f; + frame_transf_2(1, 2) = 0.0f; + frame_transf_2(2, 0) = 0.0f; + frame_transf_2(2, 1) = 0.0f; + frame_transf_2(2, 2) = 1.0f; + + matrix::Vector3f position_local = matrix::Vector3f(_position.x, _position.y, _position.z); + position_local = frame_transf * frame_transf_2 * position_local; + + matrix::Vector3f position_setpoint_local = matrix::Vector3f(_position_setpoint.x, _position_setpoint.y, _position_setpoint.z); + position_setpoint_local = frame_transf * frame_transf_2 * position_setpoint_local; + + matrix::Vector3f linear_velocity_local = matrix::Vector3f(_position.vx, _position.vy, _position.vz); + linear_velocity_local = frame_transf * frame_transf_2 * linear_velocity_local; + + matrix::Quatf attitude = matrix::Quatf(_attitude.q); + matrix::Dcmf _attitude_local_mat = frame_transf * (frame_transf_2 * matrix::Dcmf(attitude)) * frame_transf.transpose(); + + matrix::Vector3f angular_vel_local = matrix::Vector3f( _angular_velocity.xyz[0], _angular_velocity.xyz[1], _angular_velocity.xyz[2]); + angular_vel_local = frame_transf * angular_vel_local; + + _input_tensor->data.f[0] = position_setpoint_local(0) - position_local(0); + _input_tensor->data.f[1] = position_setpoint_local(1) - position_local(1); + _input_tensor->data.f[2] = position_setpoint_local(2) - position_local(2); + _input_tensor->data.f[3] = _attitude_local_mat(0, 0); + _input_tensor->data.f[4] = _attitude_local_mat(0, 1); + _input_tensor->data.f[5] = _attitude_local_mat(0, 2); + _input_tensor->data.f[6] = _attitude_local_mat(1, 0); + _input_tensor->data.f[7] = _attitude_local_mat(1, 1); + _input_tensor->data.f[8] = _attitude_local_mat(1, 2); + _input_tensor->data.f[9] = linear_velocity_local(0); + _input_tensor->data.f[10] = linear_velocity_local(1); + _input_tensor->data.f[11] = linear_velocity_local(2); + _input_tensor->data.f[12] = angular_vel_local(0); + _input_tensor->data.f[13] = angular_vel_local(1); + _input_tensor->data.f[14] = angular_vel_local(2); + + if (_param_debug_input_tensor.get()) { + PX4_INFO("Input tensor:"); + PX4_INFO("pos_err: [%f, %f, %f]", static_cast(_input_tensor->data.f[0]), static_cast(_input_tensor->data.f[1]), static_cast(_input_tensor->data.f[2])); + PX4_INFO("att: [%f, %f, %f, %f, %f, %f]", static_cast(_input_tensor->data.f[3]), static_cast(_input_tensor->data.f[4]), static_cast(_input_tensor->data.f[5]), static_cast(_input_tensor->data.f[6]), static_cast(_input_tensor->data.f[7]), static_cast(_input_tensor->data.f[8])); + PX4_INFO("vel: [%f, %f, %f]", static_cast(_input_tensor->data.f[9]), static_cast(_input_tensor->data.f[10]), static_cast(_input_tensor->data.f[11])); + PX4_INFO("ang_vel: [%f, %f, %f]", static_cast(_input_tensor->data.f[12]), static_cast(_input_tensor->data.f[13]), static_cast(_input_tensor->data.f[14])); + } + return; +} + +void MulticopterNeuralNetworkControl::PublishOutput(float* command_actions) { + + actuator_motors_s actuator_motors; + actuator_motors.timestamp = hrt_absolute_time(); + + actuator_motors.control[0] = PX4_ISFINITE(command_actions[0]) ? command_actions[0] : NAN; + actuator_motors.control[1] = PX4_ISFINITE(command_actions[2]) ? command_actions[2] : NAN; + actuator_motors.control[2] = PX4_ISFINITE(command_actions[3]) ? command_actions[3] : NAN; + actuator_motors.control[3] = PX4_ISFINITE(command_actions[1]) ? command_actions[1] : NAN; + actuator_motors.control[4] = -NAN; + actuator_motors.control[5] = -NAN; + actuator_motors.control[6] = -NAN; + actuator_motors.control[7] = -NAN; + actuator_motors.control[8] = -NAN; + actuator_motors.control[9] = -NAN; + actuator_motors.control[10] = -NAN; + actuator_motors.control[11] = -NAN; + actuator_motors.reversible_flags = 0; + + _actuator_motors_pub.publish(actuator_motors); +} + + + +inline void MulticopterNeuralNetworkControl::RescaleActions() { + //static const float _thrust_coefficient = 0.00001286412; + static const float _thrust_coefficient = 0.00001006412; + const float a = 0.8f; + const float b = (1.f - 0.8f); + const float tmp1 = b / (2.f * a); + const float tmp2 = b * b / (4.f * a * a); + const int max_rpm = 22000.0f; + const int min_rpm = 1000.0f; + for (int i = 0; i < 4; i++) { + if (_output_tensor->data.f[i] < 0.2f){ + _output_tensor->data.f[i] = 0.2f; + } + else if (_output_tensor->data.f[i] > 1.2f){ + _output_tensor->data.f[i] = 1.2f; + } + float rps = _output_tensor->data.f[i]/_thrust_coefficient; + rps = sqrt(rps); + float rpm = rps * 60.0f; + _output_tensor->data.f[i] = (rpm*2.0f - max_rpm - min_rpm) / (max_rpm - min_rpm); + _output_tensor->data.f[i] = a * (((_output_tensor->data.f[i] + 1.0f) / 2.0f + tmp1) * ((_output_tensor->data.f[i] + 1.0f) / 2.0f + tmp1) - tmp2); + } +} + + +int MulticopterNeuralNetworkControl::task_spawn(int argc, char *argv[]) +{ + // This function loads the model, sets up the interpreter, allocates memory for the model's tensors, and prepares the input data. + MulticopterNeuralNetworkControl *instance = new MulticopterNeuralNetworkControl(); + + if (instance){ + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() and instance->InitializeNetwork() == PX4_OK) { + return PX4_OK; + } + else { + PX4_ERR("init failed"); + } + } else { + PX4_ERR("alloc failed"); + } + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +void MulticopterNeuralNetworkControl::Run() +{ + if (should_exit()) + { + _angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + //std::chrono::time_point start1, end1; + //std::chrono::time_point start2, end2; + //start1 = std::chrono::system_clock::now(); + + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); + } + + vehicle_control_mode_s vehicle_control_mode; + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + //_use_neural = vehicle_control_mode.flag_control_neural_enabled; + _use_neural = true; + } + + if(!_use_neural) { + // If the neural network flight mode is not enabled, do nothing + return; + } + + // run controller on angular velocity updates + if (_angular_velocity_sub.update(&_angular_velocity)) { + _last_run = _angular_velocity.timestamp_sample; + + if(_attitude_sub.updated()) { + _attitude_sub.copy(&_attitude); + } + if(_position_sub.updated()) { + _position_sub.copy(&_position); + } + if(_position_setpoint_sub.updated()) { + _position_setpoint_sub.copy(&_position_setpoint); + } + + PopulateInputTensor(); + + // Run inference + //start2 = std::chrono::system_clock::now(); + TfLiteStatus invoke_status_control = _control_interpreter->Invoke(); + //end2 = std::chrono::system_clock::now(); + if (invoke_status_control != kTfLiteOk) { + PX4_ERR("Invoke() failed"); + return; + } + // Print the output + TfLiteTensor* control_output_tensor = _control_interpreter->output(0); + if (control_output_tensor == nullptr) { + PX4_ERR("Output tensor is null"); + return; + } + + TfLiteTensor* allocation_input_tensor = _allocation_interpreter->input(0); + // rescale actions + allocation_input_tensor->data.f[0] = control_output_tensor->data.f[0] * 0; + allocation_input_tensor->data.f[1] = control_output_tensor->data.f[1] * 0; + allocation_input_tensor->data.f[2] = control_output_tensor->data.f[2] * 2.0f + 2.8f; + allocation_input_tensor->data.f[3] = control_output_tensor->data.f[3] * 0.32f; + allocation_input_tensor->data.f[4] = control_output_tensor->data.f[4] * 0.32f; + allocation_input_tensor->data.f[5] = control_output_tensor->data.f[5] * 0.02f; + + TfLiteStatus invoke_status = _allocation_interpreter->Invoke(); + if (invoke_status != kTfLiteOk) { + PX4_ERR("Invoke() failed"); + return; + } + _output_tensor = _allocation_interpreter->output(0); + if (_output_tensor == nullptr) { + PX4_ERR("Output tensor is null"); + return; + } + + // Convert the output tensor to actuator values + RescaleActions(); + + // Publish the actuator values + PublishOutput(_output_tensor->data.f); + + //end1 = std::chrono::system_clock::now(); + + if(_param_debug_output_tensor.get()) { + PX4_INFO("Actuator motors values:"); + PX4_INFO("[%f, %f, %f, %f]", static_cast(_output_tensor->data.f[0]), static_cast(_output_tensor->data.f[1]), static_cast(_output_tensor->data.f[2]), static_cast(_output_tensor->data.f[3])); + } + + // if(_param_debug_inference_time.get()) { + // PX4_INFO("Total controller time: %f microseconds", double(std::chrono::duration_cast(end1 - start1).count())); + // PX4_INFO("Inference time: %f microseconds", double(std::chrono::duration_cast(end2 - start2).count())); + // } + } + perf_end(_loop_perf); +} + +int MulticopterNeuralNetworkControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterNeuralNetworkControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Multicopter Neural Network Control module. +This module is an end-to-end neural network control system for multicopters. +It takes in 15 input values and outputs 4 control actions. +Inputs: [pos_err(3), att(6), vel(3), ang_vel(3)] +Outputs: [Actuator motors(4)] +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_nn_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_nn_control_main(int argc, char *argv[]) +{ + return MulticopterNeuralNetworkControl::main(argc, argv); +} diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp new file mode 100644 index 000000000000..ee8df4716cce --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_nn_control.h + * Multicopter Neural Network Control module, from position setpoints to control allocator. + * + * @author Sindre Meyer Hegre + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Include model +#include "allocation_net.hpp" +#include "control_net.hpp" + +#include +#include +#include + +// Subscriptions +#include +#include +#include +#include +#include +#include + +// Publications +#include + +using namespace time_literals; // For the 1_s in the subscription callback + +class MulticopterNeuralNetworkControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + + MulticopterNeuralNetworkControl(); + ~MulticopterNeuralNetworkControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + // Functions + void PopulateInputTensor(); + void PublishOutput(float* command_actions); + void RescaleActions(); + int InitializeNetwork(); + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionCallbackWorkItem _angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + // Publications + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + + // Variables + bool _use_neural{false}; + perf_counter_t _loop_perf; /**< loop duration performance counter */ + hrt_abstime _last_run{0}; + tflite::MicroInterpreter* _control_interpreter; + tflite::MicroInterpreter* _allocation_interpreter; + TfLiteTensor* _input_tensor; + TfLiteTensor* _output_tensor; + vehicle_angular_velocity_s _angular_velocity; + vehicle_local_position_s _position; + vehicle_local_position_setpoint_s _position_setpoint; + vehicle_attitude_s _attitude; + + DEFINE_PARAMETERS( + (ParamBool) _param_debug_input_tensor, + (ParamBool) _param_debug_output_tensor, + (ParamBool) _param_debug_inference_time + ) +}; diff --git a/src/modules/mc_nn_control/mc_nn_control_params.c b/src/modules/mc_nn_control/mc_nn_control_params.c new file mode 100644 index 000000000000..5d47968621cb --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control_params.c @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_nn_control_params.c + * Parameters for the Multicopter Neural Network Control module + * + * @author Sindre Meyer Hegre + */ + +/** + * Toggle if the input tensor should be printed to the console + * + * @boolean + * @group Multicopter Neural Network Control + */ +PARAM_DEFINE_INT32(NN_IN_DEBUG, 0); + +/** + * Toggle if the output tensor should be printed to the console + * + * @boolean + * @group Multicopter Neural Network Control + */ +PARAM_DEFINE_INT32(NN_OUT_DEBUG, 0); + +/** + * Toggle if the inference time should be printed to the console + * + * @boolean + * @group Multicopter Neural Network Control + */ +PARAM_DEFINE_INT32(NN_TIME_DEBUG, 0); From a81747356065d4211edd94a953c30f80763520c8 Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Thu, 20 Feb 2025 16:05:12 +0100 Subject: [PATCH 05/11] Added neural flight mode --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 +++++ msg/versioned/VehicleControlMode.msg | 1 + msg/versioned/VehicleStatus.msg | 2 +- src/drivers/rc/crsf_rc/CrsfRc.cpp | 1 + src/drivers/rc_input/crsf_telemetry.cpp | 1 + src/lib/events/enums.json | 8 ++++++++ src/lib/modes/standard_modes.hpp | 1 + src/lib/modes/ui.hpp | 3 ++- src/modules/commander/Commander.cpp | 10 +++++++++- src/modules/commander/ModeUtil/control_mode.cpp | 11 +++++++++++ src/modules/commander/module.yaml | 1 + src/modules/commander/px4_custom_mode.h | 6 ++++++ src/modules/mc_nn_control/mc_nn_control.cpp | 3 +-- 13 files changed, 48 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index d5d9989c8666..5fef04f2d0e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -36,3 +36,8 @@ mc_pos_control start # Start Multicopter Land Detector. # land_detector start multicopter + +# +# Start Multicopter Neural Network Controller. +# +mc_nn_control start diff --git a/msg/versioned/VehicleControlMode.msg b/msg/versioned/VehicleControlMode.msg index 0a1fdedfbf14..676313d4c598 100644 --- a/msg/versioned/VehicleControlMode.msg +++ b/msg/versioned/VehicleControlMode.msg @@ -17,6 +17,7 @@ bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_allocation_enabled # true if control allocation is enabled bool flag_control_termination_enabled # true if flighttermination is enabled +bool flag_control_neural_enabled # true if neural networks are used for control # TODO: use dedicated topic for external requests uint8 source_id # Mode ID (nav_state) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index b9a3edd91fbe..5f28806773af 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -40,7 +40,7 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode uint8 NAVIGATION_STATE_POSITION_SLOW = 6 -uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_AUTO_NEURAL = 7 # Neural network control mode uint8 NAVIGATION_STATE_FREE4 = 8 uint8 NAVIGATION_STATE_FREE3 = 9 uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 3ea6c0ac273d..00639923752c 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -294,6 +294,7 @@ void CrsfRc::Run() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: flight_mode = "Auto"; break; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 60286e0f40aa..0ecc94ee23dc 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -159,6 +159,7 @@ bool CRSFTelemetry::send_flight_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: flight_mode = "Auto"; break; diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 3b73cff62ca7..4c6fef39adc1 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -111,6 +111,10 @@ "name": "vtol_takeoff", "description": "VTOL Takeoff" }, + "168034304": { + "name": "neural", + "description": "Neural" + }, "8388608": { "name": "external1", "description": "External 1" @@ -604,6 +608,10 @@ "name": "external8", "description": "External 8" }, + "24": { + "name": "auto_neural", + "description": "Neural" + }, "255": { "name": "unknown", "description": "[Unknown]" diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp index b009d815d425..2a0a9600d2d9 100644 --- a/src/lib/modes/standard_modes.hpp +++ b/src/lib/modes/standard_modes.hpp @@ -51,6 +51,7 @@ enum class StandardMode : uint8_t { MISSION = 6, LAND = 7, TAKEOFF = 8, + NEUTRAL = 9, }; /** diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index 9e4156ae007a..38cf5e2c89d6 100644 --- a/src/lib/modes/ui.hpp +++ b/src/lib/modes/ui.hpp @@ -52,6 +52,7 @@ static inline uint32_t getValidNavStates() (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) | (1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL) | (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) | (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) | (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) | @@ -74,7 +75,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "Hold", "Return", "Position Slow", - "7: unallocated", + "Neural", "8: unallocated", "9: unallocated", "Acro", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f0537c7c6e86..e5c1a4f6e7c5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -392,6 +392,10 @@ int Commander::custom_command(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL); + } else if (!strcmp(argv[1], "auto:neural")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_NEURAL); + } else if (!strcmp(argv[1], "acro")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO); @@ -844,6 +848,10 @@ Commander::handle_command(const vehicle_command_s &cmd) desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; + case PX4_CUSTOM_SUB_MODE_AUTO_NEURAL: + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL; + break; + case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8: desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1); break; @@ -3020,7 +3028,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("land"); PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); - PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1", + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:neural|auto:takeoff|auto:land|auto:precland|ext1", "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("pair"); PRINT_MODULE_USAGE_COMMAND("lockdown"); diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index fbebc7b93d8b..e039952c1686 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -100,6 +100,17 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_allocation_enabled = true; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: + vehicle_control_mode.flag_control_auto_enabled = true; + vehicle_control_mode.flag_control_position_enabled = true; + vehicle_control_mode.flag_control_velocity_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = true; + vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_neural_enabled = true; + break; + case vehicle_status_s::NAVIGATION_STATE_ACRO: vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index d2113d935955..bc57be1428ee 100644 --- a/src/modules/commander/module.yaml +++ b/src/modules/commander/module.yaml @@ -38,6 +38,7 @@ parameters: 8: Stabilized 12: Follow Me 13: Precision Land + 14: Neural 100: External Mode 1 101: External Mode 2 102: External Mode 3 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e4936982ff1b..0d15e8557fea 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -64,6 +64,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05 PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, + PX4_CUSTOM_SUB_MODE_AUTO_NEURAL, PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF, PX4_CUSTOM_SUB_MODE_EXTERNAL1, PX4_CUSTOM_SUB_MODE_EXTERNAL2, @@ -136,6 +137,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_NEURAL; + break; + case vehicle_status_s::NAVIGATION_STATE_ACRO: custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 5b691d177905..952bc79444c7 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -297,8 +297,7 @@ void MulticopterNeuralNetworkControl::Run() vehicle_control_mode_s vehicle_control_mode; if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { - //_use_neural = vehicle_control_mode.flag_control_neural_enabled; - _use_neural = true; + _use_neural = vehicle_control_mode.flag_control_neural_enabled; } if(!_use_neural) { From 4ceaaa9046fff1466a1ad093314b66f5ea59e26a Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Fri, 21 Feb 2025 10:09:35 +0100 Subject: [PATCH 06/11] Add posibility to read of inference times --- src/modules/mc_nn_control/mc_nn_control.cpp | 30 +++++++++++-------- src/modules/mc_nn_control/mc_nn_control.hpp | 5 +++- .../mc_nn_control/mc_nn_control_params.c | 28 +++++++++++++++++ 3 files changed, 50 insertions(+), 13 deletions(-) diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 952bc79444c7..70df7ea48787 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -38,8 +38,7 @@ */ #include "mc_nn_control.hpp" -//#include -#include +#include namespace { using NNControlOpResolver = tflite::MicroMutableOpResolver<4>; // This number should be the number of operations in the model, like tanh and fully connected @@ -285,9 +284,7 @@ void MulticopterNeuralNetworkControl::Run() perf_begin(_loop_perf); - //std::chrono::time_point start1, end1; - //std::chrono::time_point start2, end2; - //start1 = std::chrono::system_clock::now(); + hrt_abstime start_time1 = hrt_absolute_time(); if (_parameter_update_sub.updated()) { parameter_update_s param_update; @@ -322,9 +319,9 @@ void MulticopterNeuralNetworkControl::Run() PopulateInputTensor(); // Run inference - //start2 = std::chrono::system_clock::now(); + hrt_abstime start_time2 = hrt_absolute_time(); TfLiteStatus invoke_status_control = _control_interpreter->Invoke(); - //end2 = std::chrono::system_clock::now(); + hrt_abstime inference_time_control = hrt_absolute_time() - start_time2; if (invoke_status_control != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -345,7 +342,9 @@ void MulticopterNeuralNetworkControl::Run() allocation_input_tensor->data.f[4] = control_output_tensor->data.f[4] * 0.32f; allocation_input_tensor->data.f[5] = control_output_tensor->data.f[5] * 0.02f; + hrt_abstime start_time3 = hrt_absolute_time(); TfLiteStatus invoke_status = _allocation_interpreter->Invoke(); + hrt_abstime inference_time_allocation = hrt_absolute_time() - start_time3; if (invoke_status != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -362,17 +361,24 @@ void MulticopterNeuralNetworkControl::Run() // Publish the actuator values PublishOutput(_output_tensor->data.f); - //end1 = std::chrono::system_clock::now(); + hrt_abstime full_controller_time = hrt_absolute_time() - start_time1; if(_param_debug_output_tensor.get()) { PX4_INFO("Actuator motors values:"); PX4_INFO("[%f, %f, %f, %f]", static_cast(_output_tensor->data.f[0]), static_cast(_output_tensor->data.f[1]), static_cast(_output_tensor->data.f[2]), static_cast(_output_tensor->data.f[3])); } - // if(_param_debug_inference_time.get()) { - // PX4_INFO("Total controller time: %f microseconds", double(std::chrono::duration_cast(end1 - start1).count())); - // PX4_INFO("Inference time: %f microseconds", double(std::chrono::duration_cast(end2 - start2).count())); - // } + if(_param_debug_inference_time.get()) { + _param_control_inference_time.set(inference_time_control); + _param_control_inference_time.commit(); + _param_allocation_inference_time.set(inference_time_allocation); + _param_allocation_inference_time.commit(); + _param_full_inference_time.set(full_controller_time); + _param_full_inference_time.commit(); + PX4_INFO("Inference time control net: %llu us", (unsigned long long)inference_time_control); + PX4_INFO("Inference time allocation net: %llu us", (unsigned long long)inference_time_allocation); + PX4_INFO("Full controller time : %llu us", (unsigned long long)full_controller_time); + } } perf_end(_loop_perf); } diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp index ee8df4716cce..fa3a24b695df 100644 --- a/src/modules/mc_nn_control/mc_nn_control.hpp +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -130,6 +130,9 @@ class MulticopterNeuralNetworkControl : public ModuleBase) _param_debug_input_tensor, (ParamBool) _param_debug_output_tensor, - (ParamBool) _param_debug_inference_time + (ParamBool) _param_debug_inference_time, + (ParamFloat) _param_control_inference_time, + (ParamFloat) _param_allocation_inference_time, + (ParamFloat) _param_full_inference_time ) }; diff --git a/src/modules/mc_nn_control/mc_nn_control_params.c b/src/modules/mc_nn_control/mc_nn_control_params.c index 5d47968621cb..7fa22ab2dc56 100644 --- a/src/modules/mc_nn_control/mc_nn_control_params.c +++ b/src/modules/mc_nn_control/mc_nn_control_params.c @@ -61,3 +61,31 @@ PARAM_DEFINE_INT32(NN_OUT_DEBUG, 0); * @group Multicopter Neural Network Control */ PARAM_DEFINE_INT32(NN_TIME_DEBUG, 0); + + +/** + * Save the inference time as logging is not implemented yet + * + * @group Multicopter Neural Network Control + * @unit us + * + */ +PARAM_DEFINE_FLOAT(NN_CONTROL_INF, 0.0f); + +/** + * Save the inference time as logging is not implemented yet + * + * @group Multicopter Neural Network Control + * @unit us + * + */ +PARAM_DEFINE_FLOAT(NN_ALLOC_INF, 0.0f); + +/** + * Save the inference time as logging is not implemented yet + * + * @group Multicopter Neural Network Control + * @unit us + * + */ +PARAM_DEFINE_FLOAT(NN_FULL_INF, 0.0f); From 564b79a697747e93d7f1e3a898a89029fcea06fe Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Mon, 24 Feb 2025 14:16:54 +0100 Subject: [PATCH 07/11] Fix comments from review: - Switch ssh link to https link in submodule - Remove mc_nn_control from startup --- .gitmodules | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 ----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/.gitmodules b/.gitmodules index 15a78d33382d..3a4b364a4f76 100644 --- a/.gitmodules +++ b/.gitmodules @@ -91,5 +91,5 @@ url = https://github.com/dronecan/pydronecan [submodule "src/lib/tflm/tflite_micro"] path = src/lib/tflm/tflite_micro - url = git@github.com:tensorflow/tflite-micro.git + url = https://github.com/tensorflow/tflite-micro.git branch = main diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 5fef04f2d0e5..d5d9989c8666 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -36,8 +36,3 @@ mc_pos_control start # Start Multicopter Land Detector. # land_detector start multicopter - -# -# Start Multicopter Neural Network Controller. -# -mc_nn_control start From 202b292ba2a4d22dfef4535ed798d83d18d2dbd8 Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Mon, 24 Feb 2025 14:23:45 +0100 Subject: [PATCH 08/11] Remove auto start --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 5fef04f2d0e5..d5d9989c8666 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -36,8 +36,3 @@ mc_pos_control start # Start Multicopter Land Detector. # land_detector start multicopter - -# -# Start Multicopter Neural Network Controller. -# -mc_nn_control start From 222f2c775d34992611b929eb77f0ec3248702c4b Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Tue, 25 Feb 2025 11:38:29 +0100 Subject: [PATCH 09/11] Add logging from neural control module --- msg/CMakeLists.txt | 1 + msg/NeuralControl.msg | 10 ++ src/modules/logger/logged_topics.cpp | 1 + src/modules/mc_nn_control/CMakeLists.txt | 2 +- src/modules/mc_nn_control/allocation_net.hpp | 2 +- src/modules/mc_nn_control/control_net.hpp | 2 +- src/modules/mc_nn_control/mc_nn_control.cpp | 92 +++++++++++-------- src/modules/mc_nn_control/mc_nn_control.hpp | 13 +-- .../mc_nn_control/mc_nn_control_params.c | 91 ------------------ 9 files changed, 70 insertions(+), 144 deletions(-) create mode 100644 msg/NeuralControl.msg delete mode 100644 src/modules/mc_nn_control/mc_nn_control_params.c diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a88567b2866f..ed12893743bc 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -137,6 +137,7 @@ set(msg_files MountOrientation.msg NavigatorMissionItem.msg NavigatorStatus.msg + NeuralControl.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg ObstacleDistance.msg diff --git a/msg/NeuralControl.msg b/msg/NeuralControl.msg new file mode 100644 index 000000000000..a9fbd5b1ee0b --- /dev/null +++ b/msg/NeuralControl.msg @@ -0,0 +1,10 @@ +# Neural Control Message +uint64 timestamp # time since system start (microseconds) + +float32[15] observation # Observation vector (pos error (3), att (6d), lin vel (3), ang vel (3)) +float32[6] wrench # Forces and Torques before allocation +float32[4] motor_thrust # Thrust per motor + +int32 controller_time # Time spent from input to output in microseconds +int32 control_inference_time # Time spent in the control inference in microseconds +int32 allocation_inference_time # Time spent in the allocation inference in microseconds diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index b98d185aff4f..1abb75be3988 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -287,6 +287,7 @@ void LoggedTopics::add_debug_topics() add_topic("mag_worker_data"); add_topic("sensor_preflight_mag", 500); add_topic("actuator_test", 500); + add_topic("neural_control", 50); } void LoggedTopics::add_estimator_replay_topics() diff --git a/src/modules/mc_nn_control/CMakeLists.txt b/src/modules/mc_nn_control/CMakeLists.txt index 84f0a15be5e1..0b4100137452 100644 --- a/src/modules/mc_nn_control/CMakeLists.txt +++ b/src/modules/mc_nn_control/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024-2025 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/mc_nn_control/allocation_net.hpp b/src/modules/mc_nn_control/allocation_net.hpp index 4dfaa9600a4a..aae7b9c8d1ab 100644 --- a/src/modules/mc_nn_control/allocation_net.hpp +++ b/src/modules/mc_nn_control/allocation_net.hpp @@ -1,4 +1,4 @@ #include -constexpr unsigned int allocation_net_tflite_size = 8012; +constexpr unsigned int allocation_net_tflite_size = 2612; extern const unsigned char allocation_net_tflite[]; diff --git a/src/modules/mc_nn_control/control_net.hpp b/src/modules/mc_nn_control/control_net.hpp index 09f7e8fcb044..367f3d425edb 100644 --- a/src/modules/mc_nn_control/control_net.hpp +++ b/src/modules/mc_nn_control/control_net.hpp @@ -1,4 +1,4 @@ #include -constexpr unsigned int control_net_tflite_size = 8012; +constexpr unsigned int control_net_tflite_size = 8000; extern const unsigned char control_net_tflite[]; diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 70df7ea48787..27577e640344 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,7 +38,11 @@ */ #include "mc_nn_control.hpp" +#ifdef __PX4_NUTTX #include +#else +#include +#endif namespace { using NNControlOpResolver = tflite::MicroMutableOpResolver<4>; // This number should be the number of operations in the model, like tanh and fully connected @@ -82,18 +86,7 @@ int MulticopterNeuralNetworkControl::InitializeNetwork() { // Initialize the neural network // Load the model const tflite::Model* control_model = ::tflite::GetModel(control_net_tflite); // TODO: Replace with your model data variable - // if (control_model->version() != TFLITE_SCHEMA_VERSION) { - // PX4_ERR("Model provided is schema version %d not equal to supported version %d.", - // control_model->version(), TFLITE_SCHEMA_VERSION); - // return -1; - // } - const tflite::Model* allocation_model = ::tflite::GetModel(allocation_net_tflite); // TODO: Replace with your model data variable - // if (allocation_model->version() != TFLITE_SCHEMA_VERSION) { - // PX4_ERR("Model provided is schema version %d not equal to supported version %d.", - // allocation_model->version(), TFLITE_SCHEMA_VERSION); - // return -1; - // } // Set up the interpreter static NNControlOpResolver resolver; @@ -189,14 +182,6 @@ void MulticopterNeuralNetworkControl::PopulateInputTensor() { _input_tensor->data.f[13] = angular_vel_local(1); _input_tensor->data.f[14] = angular_vel_local(2); - if (_param_debug_input_tensor.get()) { - PX4_INFO("Input tensor:"); - PX4_INFO("pos_err: [%f, %f, %f]", static_cast(_input_tensor->data.f[0]), static_cast(_input_tensor->data.f[1]), static_cast(_input_tensor->data.f[2])); - PX4_INFO("att: [%f, %f, %f, %f, %f, %f]", static_cast(_input_tensor->data.f[3]), static_cast(_input_tensor->data.f[4]), static_cast(_input_tensor->data.f[5]), static_cast(_input_tensor->data.f[6]), static_cast(_input_tensor->data.f[7]), static_cast(_input_tensor->data.f[8])); - PX4_INFO("vel: [%f, %f, %f]", static_cast(_input_tensor->data.f[9]), static_cast(_input_tensor->data.f[10]), static_cast(_input_tensor->data.f[11])); - PX4_INFO("ang_vel: [%f, %f, %f]", static_cast(_input_tensor->data.f[12]), static_cast(_input_tensor->data.f[13]), static_cast(_input_tensor->data.f[14])); - } - return; } void MulticopterNeuralNetworkControl::PublishOutput(float* command_actions) { @@ -284,7 +269,11 @@ void MulticopterNeuralNetworkControl::Run() perf_begin(_loop_perf); - hrt_abstime start_time1 = hrt_absolute_time(); + #ifdef __PX4_NUTTX + hrt_abstime start_time1 = hrt_absolute_time(); + #else + auto start_time1 = std::chrono::high_resolution_clock::now(); + #endif if (_parameter_update_sub.updated()) { parameter_update_s param_update; @@ -319,9 +308,18 @@ void MulticopterNeuralNetworkControl::Run() PopulateInputTensor(); // Run inference - hrt_abstime start_time2 = hrt_absolute_time(); + #ifdef __PX4_NUTTX + hrt_abstime start_time2 = hrt_absolute_time(); + #else + auto start_time2 = std::chrono::high_resolution_clock::now(); + #endif + // Inference TfLiteStatus invoke_status_control = _control_interpreter->Invoke(); - hrt_abstime inference_time_control = hrt_absolute_time() - start_time2; + #ifdef __PX4_NUTTX + hrt_abstime inference_time_control = hrt_absolute_time() - start_time2; + #else + auto inference_time_control = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time2).count(); + #endif if (invoke_status_control != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -342,9 +340,17 @@ void MulticopterNeuralNetworkControl::Run() allocation_input_tensor->data.f[4] = control_output_tensor->data.f[4] * 0.32f; allocation_input_tensor->data.f[5] = control_output_tensor->data.f[5] * 0.02f; - hrt_abstime start_time3 = hrt_absolute_time(); + #ifdef __PX4_NUTTX + hrt_abstime start_time3 = hrt_absolute_time(); + #else + auto start_time3 = std::chrono::high_resolution_clock::now(); + #endif TfLiteStatus invoke_status = _allocation_interpreter->Invoke(); - hrt_abstime inference_time_allocation = hrt_absolute_time() - start_time3; + #ifdef __PX4_NUTTX + hrt_abstime inference_time_allocation = hrt_absolute_time() - start_time3; + #else + auto inference_time_allocation = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time3).count(); + #endif if (invoke_status != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -361,24 +367,30 @@ void MulticopterNeuralNetworkControl::Run() // Publish the actuator values PublishOutput(_output_tensor->data.f); - hrt_abstime full_controller_time = hrt_absolute_time() - start_time1; - if(_param_debug_output_tensor.get()) { - PX4_INFO("Actuator motors values:"); - PX4_INFO("[%f, %f, %f, %f]", static_cast(_output_tensor->data.f[0]), static_cast(_output_tensor->data.f[1]), static_cast(_output_tensor->data.f[2]), static_cast(_output_tensor->data.f[3])); + #ifdef __PX4_NUTTX + hrt_abstime full_controller_time = hrt_absolute_time() - start_time1; + #else + auto full_controller_time = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time1).count(); + #endif + + // Publish the neural control debug message + neural_control_s neural_control; + neural_control.timestamp = hrt_absolute_time(); + neural_control.control_inference_time = static_cast(inference_time_control); + neural_control.controller_time = static_cast(full_controller_time); + neural_control.allocation_inference_time = static_cast(inference_time_allocation); + for (int i = 0; i < 15; i++) { + neural_control.observation[i] = _input_tensor->data.f[i]; } - - if(_param_debug_inference_time.get()) { - _param_control_inference_time.set(inference_time_control); - _param_control_inference_time.commit(); - _param_allocation_inference_time.set(inference_time_allocation); - _param_allocation_inference_time.commit(); - _param_full_inference_time.set(full_controller_time); - _param_full_inference_time.commit(); - PX4_INFO("Inference time control net: %llu us", (unsigned long long)inference_time_control); - PX4_INFO("Inference time allocation net: %llu us", (unsigned long long)inference_time_allocation); - PX4_INFO("Full controller time : %llu us", (unsigned long long)full_controller_time); + for (int i = 0; i < 6; i++) { + neural_control.wrench[i] = allocation_input_tensor->data.f[i]; } + neural_control.motor_thrust[0] = _output_tensor->data.f[0]; + neural_control.motor_thrust[1] = _output_tensor->data.f[2]; + neural_control.motor_thrust[2] = _output_tensor->data.f[3]; + neural_control.motor_thrust[3] = _output_tensor->data.f[1]; + _neural_control_pub.publish(neural_control); } perf_end(_loop_perf); } diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp index fa3a24b695df..6e8b1fa9e74a 100644 --- a/src/modules/mc_nn_control/mc_nn_control.hpp +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -71,6 +71,7 @@ // Publications #include +#include using namespace time_literals; // For the 1_s in the subscription callback @@ -113,6 +114,7 @@ class MulticopterNeuralNetworkControl : public ModuleBase _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _neural_control_pub{ORB_ID(neural_control)}; // Variables bool _use_neural{false}; @@ -126,13 +128,4 @@ class MulticopterNeuralNetworkControl : public ModuleBase) _param_debug_input_tensor, - (ParamBool) _param_debug_output_tensor, - (ParamBool) _param_debug_inference_time, - (ParamFloat) _param_control_inference_time, - (ParamFloat) _param_allocation_inference_time, - (ParamFloat) _param_full_inference_time - ) }; diff --git a/src/modules/mc_nn_control/mc_nn_control_params.c b/src/modules/mc_nn_control/mc_nn_control_params.c deleted file mode 100644 index 7fa22ab2dc56..000000000000 --- a/src/modules/mc_nn_control/mc_nn_control_params.c +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mc_nn_control_params.c - * Parameters for the Multicopter Neural Network Control module - * - * @author Sindre Meyer Hegre - */ - -/** - * Toggle if the input tensor should be printed to the console - * - * @boolean - * @group Multicopter Neural Network Control - */ -PARAM_DEFINE_INT32(NN_IN_DEBUG, 0); - -/** - * Toggle if the output tensor should be printed to the console - * - * @boolean - * @group Multicopter Neural Network Control - */ -PARAM_DEFINE_INT32(NN_OUT_DEBUG, 0); - -/** - * Toggle if the inference time should be printed to the console - * - * @boolean - * @group Multicopter Neural Network Control - */ -PARAM_DEFINE_INT32(NN_TIME_DEBUG, 0); - - -/** - * Save the inference time as logging is not implemented yet - * - * @group Multicopter Neural Network Control - * @unit us - * - */ -PARAM_DEFINE_FLOAT(NN_CONTROL_INF, 0.0f); - -/** - * Save the inference time as logging is not implemented yet - * - * @group Multicopter Neural Network Control - * @unit us - * - */ -PARAM_DEFINE_FLOAT(NN_ALLOC_INF, 0.0f); - -/** - * Save the inference time as logging is not implemented yet - * - * @group Multicopter Neural Network Control - * @unit us - * - */ -PARAM_DEFINE_FLOAT(NN_FULL_INF, 0.0f); From d0369ac76da6e57faebe528e31b46be78faca234 Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Wed, 26 Feb 2025 09:21:09 +0100 Subject: [PATCH 10/11] Fix automatic startup to only be when module is included --- ROMFS/px4fmu_common/init.d-posix/rcS | 5 ++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 ++ .../mc_nn_control/mc_nn_control_params.c | 47 +++++++++++++++++++ 3 files changed, 57 insertions(+) create mode 100644 src/modules/mc_nn_control/mc_nn_control_params.c diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index c53dbbd46a35..a946531f6c9c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -342,6 +342,11 @@ then internal_combustion_engine_control start fi +if param compare -s MC_NN_EN 1 +then + mc_nn_control start +fi + #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index d5d9989c8666..adff4e963f22 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -36,3 +36,8 @@ mc_pos_control start # Start Multicopter Land Detector. # land_detector start multicopter + +if param compare -s MC_NN_EN 1 +then + mc_nn_control start +fi diff --git a/src/modules/mc_nn_control/mc_nn_control_params.c b/src/modules/mc_nn_control/mc_nn_control_params.c new file mode 100644 index 000000000000..b8c7df60bd47 --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control_params.c @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_nn_control_params.c + * Parameters for the Multicopter Neural Network Control module + * + * @author Sindre Meyer Hegre + */ + + /** + * If true the neural network control is automatically started on boot. + * + * @boolean + * @group Multicopter Neural Network Control + */ +PARAM_DEFINE_INT32(MC_NN_EN, 1); From 2243d9ba6c162b87fc00bfbed35981578ebd5a88 Mon Sep 17 00:00:00 2001 From: Sindre Meyer Hegre Date: Wed, 26 Feb 2025 15:54:31 +0100 Subject: [PATCH 11/11] Switch to flight mode registration --- ROMFS/px4fmu_common/init.d-posix/rcS | 5 - msg/versioned/VehicleControlMode.msg | 1 - msg/versioned/VehicleStatus.msg | 2 +- src/drivers/rc/crsf_rc/CrsfRc.cpp | 1 - src/drivers/rc_input/crsf_telemetry.cpp | 1 - src/lib/events/enums.json | 8 - src/lib/modes/standard_modes.hpp | 1 - src/lib/modes/ui.hpp | 3 +- src/modules/commander/Commander.cpp | 10 +- .../commander/ModeUtil/control_mode.cpp | 11 -- src/modules/commander/module.yaml | 1 - src/modules/commander/px4_custom_mode.h | 6 - src/modules/mc_nn_control/mc_nn_control.cpp | 179 +++++++++++++----- src/modules/mc_nn_control/mc_nn_control.hpp | 34 +++- .../mc_nn_control/mc_nn_control_params.c | 2 +- 15 files changed, 167 insertions(+), 98 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index a946531f6c9c..c53dbbd46a35 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -342,11 +342,6 @@ then internal_combustion_engine_control start fi -if param compare -s MC_NN_EN 1 -then - mc_nn_control start -fi - #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/msg/versioned/VehicleControlMode.msg b/msg/versioned/VehicleControlMode.msg index 676313d4c598..0a1fdedfbf14 100644 --- a/msg/versioned/VehicleControlMode.msg +++ b/msg/versioned/VehicleControlMode.msg @@ -17,7 +17,6 @@ bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_allocation_enabled # true if control allocation is enabled bool flag_control_termination_enabled # true if flighttermination is enabled -bool flag_control_neural_enabled # true if neural networks are used for control # TODO: use dedicated topic for external requests uint8 source_id # Mode ID (nav_state) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 5f28806773af..b9a3edd91fbe 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -40,7 +40,7 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode uint8 NAVIGATION_STATE_POSITION_SLOW = 6 -uint8 NAVIGATION_STATE_AUTO_NEURAL = 7 # Neural network control mode +uint8 NAVIGATION_STATE_FREE5 = 7 uint8 NAVIGATION_STATE_FREE4 = 8 uint8 NAVIGATION_STATE_FREE3 = 9 uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 00639923752c..3ea6c0ac273d 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -294,7 +294,6 @@ void CrsfRc::Run() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: - case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: flight_mode = "Auto"; break; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 0ecc94ee23dc..60286e0f40aa 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -159,7 +159,6 @@ bool CRSFTelemetry::send_flight_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: - case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: flight_mode = "Auto"; break; diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 4c6fef39adc1..3b73cff62ca7 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -111,10 +111,6 @@ "name": "vtol_takeoff", "description": "VTOL Takeoff" }, - "168034304": { - "name": "neural", - "description": "Neural" - }, "8388608": { "name": "external1", "description": "External 1" @@ -608,10 +604,6 @@ "name": "external8", "description": "External 8" }, - "24": { - "name": "auto_neural", - "description": "Neural" - }, "255": { "name": "unknown", "description": "[Unknown]" diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp index 2a0a9600d2d9..b009d815d425 100644 --- a/src/lib/modes/standard_modes.hpp +++ b/src/lib/modes/standard_modes.hpp @@ -51,7 +51,6 @@ enum class StandardMode : uint8_t { MISSION = 6, LAND = 7, TAKEOFF = 8, - NEUTRAL = 9, }; /** diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index 38cf5e2c89d6..9e4156ae007a 100644 --- a/src/lib/modes/ui.hpp +++ b/src/lib/modes/ui.hpp @@ -52,7 +52,6 @@ static inline uint32_t getValidNavStates() (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) | (1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) | - (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL) | (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) | (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) | (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) | @@ -75,7 +74,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "Hold", "Return", "Position Slow", - "Neural", + "7: unallocated", "8: unallocated", "9: unallocated", "Acro", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e5c1a4f6e7c5..f0537c7c6e86 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -392,10 +392,6 @@ int Commander::custom_command(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL); - } else if (!strcmp(argv[1], "auto:neural")) { - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, - PX4_CUSTOM_SUB_MODE_AUTO_NEURAL); - } else if (!strcmp(argv[1], "acro")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO); @@ -848,10 +844,6 @@ Commander::handle_command(const vehicle_command_s &cmd) desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; - case PX4_CUSTOM_SUB_MODE_AUTO_NEURAL: - desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL; - break; - case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8: desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1); break; @@ -3028,7 +3020,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("land"); PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); - PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:neural|auto:takeoff|auto:land|auto:precland|ext1", + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1", "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("pair"); PRINT_MODULE_USAGE_COMMAND("lockdown"); diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index e039952c1686..fbebc7b93d8b 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -100,17 +100,6 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_allocation_enabled = true; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: - vehicle_control_mode.flag_control_auto_enabled = true; - vehicle_control_mode.flag_control_position_enabled = true; - vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_neural_enabled = true; - break; - case vehicle_status_s::NAVIGATION_STATE_ACRO: vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index bc57be1428ee..d2113d935955 100644 --- a/src/modules/commander/module.yaml +++ b/src/modules/commander/module.yaml @@ -38,7 +38,6 @@ parameters: 8: Stabilized 12: Follow Me 13: Precision Land - 14: Neural 100: External Mode 1 101: External Mode 2 102: External Mode 3 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 0d15e8557fea..e4936982ff1b 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -64,7 +64,6 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05 PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, - PX4_CUSTOM_SUB_MODE_AUTO_NEURAL, PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF, PX4_CUSTOM_SUB_MODE_EXTERNAL1, PX4_CUSTOM_SUB_MODE_EXTERNAL2, @@ -137,11 +136,6 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_NEURAL: - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_NEURAL; - break; - case vehicle_status_s::NAVIGATION_STATE_ACRO: custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 27577e640344..e1465fcc5d89 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -124,6 +124,90 @@ int MulticopterNeuralNetworkControl::InitializeNetwork() { return PX4_OK; } +int32_t MulticopterNeuralNetworkControl::GetTime(){ + #ifdef __PX4_NUTTX + return static_cast(hrt_absolute_time()); + #else + return static_cast(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); + #endif +} + +void MulticopterNeuralNetworkControl::RegisterNeuralFlightMode() { + // Register the neural flight mode with the commander + register_ext_component_request_s register_ext_component_request{}; + register_ext_component_request.timestamp = static_cast(GetTime()); + strncpy(register_ext_component_request.name, "Neural Control", sizeof(register_ext_component_request.name) - 1); + register_ext_component_request.request_id = _mode_request_id; + register_ext_component_request.px4_ros2_api_version = 1; + register_ext_component_request.register_arming_check = true; + register_ext_component_request.register_mode = true; + _register_ext_component_request_pub.publish(register_ext_component_request); +} + + +void MulticopterNeuralNetworkControl::UnregisterNeuralFlightMode(int8 arming_check_id, int8 mode_id) { + // Unregister the neural flight mode with the commander + unregister_ext_component_s unregister_ext_component{}; + unregister_ext_component.timestamp = static_cast(GetTime()); + strncpy(unregister_ext_component.name, "Neural Control", sizeof(unregister_ext_component.name) - 1); + unregister_ext_component.arming_check_id = arming_check_id; + unregister_ext_component.mode_id = mode_id; + _unregister_ext_component_pub.publish(unregister_ext_component); +} + + +void MulticopterNeuralNetworkControl::ConfigureNeuralFlightMode(int8 mode_id) { + // Configure the neural flight mode with the commander + vehicle_control_mode_s config_control_setpoints{}; + config_control_setpoints.timestamp = static_cast(GetTime()); + config_control_setpoints.source_id = mode_id; + // TODO: Should these be stopped to save computing, or is it best to keep them running for safety? + // config_control_setpoints.flag_control_position_enabled = false; + // config_control_setpoints.flag_control_velocity_enabled = false; + // config_control_setpoints.flag_control_altitude_enabled = false; + // config_control_setpoints.flag_control_climb_rate_enabled = false; + // config_control_setpoints.flag_control_acceleration_enabled = false; + // config_control_setpoints.flag_control_attitude_enabled = false; + // config_control_setpoints.flag_control_rates_enabled = false; + config_control_setpoints.flag_control_allocation_enabled = false; + _config_control_setpoints_pub.publish(config_control_setpoints); +} + + +void MulticopterNeuralNetworkControl::ReplyToArmingCheck(int8 request_id) { + // Reply to the arming check request + arming_check_reply_s arming_check_reply; + arming_check_reply.timestamp = static_cast(GetTime()); + arming_check_reply.request_id = request_id; + arming_check_reply.registration_id = _arming_check_id; + arming_check_reply.health_component_index = arming_check_reply.HEALTH_COMPONENT_INDEX_NONE; // Think this says that there is no new health component that needs to be present to fly + arming_check_reply.num_events = 0; // I do not know what events are being referenced here + arming_check_reply.can_arm_and_run = true; // I think this tells that this mode does not add any new requirements to the arming check + arming_check_reply.mode_req_angular_velocity = true; + arming_check_reply.mode_req_local_position = true; + arming_check_reply.mode_req_attitude = true; + arming_check_reply.mode_req_mission = false; + arming_check_reply.mode_req_global_position = false; + arming_check_reply.mode_req_prevent_arming = false; + arming_check_reply.mode_req_manual_control = false; + _arming_check_reply_pub.publish(arming_check_reply); +} + + +void MulticopterNeuralNetworkControl::CheckModeRegistration() { + register_ext_component_reply_s register_ext_component_reply; + int tries = register_ext_component_reply.ORB_QUEUE_LENGTH; + while (_register_ext_component_reply_sub.update(®ister_ext_component_reply) && --tries >= 0) { + if (register_ext_component_reply.request_id == _mode_request_id && register_ext_component_reply.success) { + _arming_check_id = register_ext_component_reply.arming_check_id; + _mode_id = register_ext_component_reply.mode_id; + PX4_INFO("NeuralControl mode registration successful, arming_check_id: %d, mode_id: %d", _arming_check_id, _mode_id); + ConfigureNeuralFlightMode(_mode_id); + break; + } + } +} + void MulticopterNeuralNetworkControl::PopulateInputTensor() { // Creates a 15 element input tensor for the neural network [pos_err(3), lin_vel(3), att(6), ang_vel(3)] @@ -187,7 +271,7 @@ void MulticopterNeuralNetworkControl::PopulateInputTensor() { void MulticopterNeuralNetworkControl::PublishOutput(float* command_actions) { actuator_motors_s actuator_motors; - actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp = static_cast(GetTime()); actuator_motors.control[0] = PX4_ISFINITE(command_actions[0]) ? command_actions[0] : NAN; actuator_motors.control[1] = PX4_ISFINITE(command_actions[2]) ? command_actions[2] : NAN; @@ -263,34 +347,50 @@ void MulticopterNeuralNetworkControl::Run() if (should_exit()) { _angular_velocity_sub.unregisterCallback(); + if (_sent_mode_registration) { + UnregisterNeuralFlightMode(_arming_check_id, _mode_id); + } exit_and_cleanup(); return; } - perf_begin(_loop_perf); + // Register the flight mode with the commander + if (!_sent_mode_registration) { + RegisterNeuralFlightMode(); + _sent_mode_registration = true; + return; + } - #ifdef __PX4_NUTTX - hrt_abstime start_time1 = hrt_absolute_time(); - #else - auto start_time1 = std::chrono::high_resolution_clock::now(); - #endif + // Check if registration was successful + if (_mode_id == -1 || _arming_check_id == -1) { + CheckModeRegistration(); + return; + } - if (_parameter_update_sub.updated()) { - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - updateParams(); + perf_begin(_loop_perf); + + // Check if an arming check request is received + if (_arming_check_request_sub.updated()) { + arming_check_request_s arming_check_request; + _arming_check_request_sub.copy(&arming_check_request); + ReplyToArmingCheck(arming_check_request.request_id); } - vehicle_control_mode_s vehicle_control_mode; - if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { - _use_neural = vehicle_control_mode.flag_control_neural_enabled; + // Check if navigation mode is set to Neural Control + vehicle_status_s vehicle_status; + if (_vehicle_status_sub.updated()) { + _vehicle_status_sub.copy(&vehicle_status); + _use_neural = vehicle_status.nav_state == _mode_id; } if(!_use_neural) { // If the neural network flight mode is not enabled, do nothing + perf_end(_loop_perf); return; } + int32_t start_time1 = GetTime(); + // run controller on angular velocity updates if (_angular_velocity_sub.update(&_angular_velocity)) { _last_run = _angular_velocity.timestamp_sample; @@ -308,18 +408,10 @@ void MulticopterNeuralNetworkControl::Run() PopulateInputTensor(); // Run inference - #ifdef __PX4_NUTTX - hrt_abstime start_time2 = hrt_absolute_time(); - #else - auto start_time2 = std::chrono::high_resolution_clock::now(); - #endif + int32_t start_time2 = GetTime(); // Inference TfLiteStatus invoke_status_control = _control_interpreter->Invoke(); - #ifdef __PX4_NUTTX - hrt_abstime inference_time_control = hrt_absolute_time() - start_time2; - #else - auto inference_time_control = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time2).count(); - #endif + int32_t inference_time_control = GetTime() - start_time2; if (invoke_status_control != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -340,17 +432,9 @@ void MulticopterNeuralNetworkControl::Run() allocation_input_tensor->data.f[4] = control_output_tensor->data.f[4] * 0.32f; allocation_input_tensor->data.f[5] = control_output_tensor->data.f[5] * 0.02f; - #ifdef __PX4_NUTTX - hrt_abstime start_time3 = hrt_absolute_time(); - #else - auto start_time3 = std::chrono::high_resolution_clock::now(); - #endif + int32_t start_time3 = GetTime(); TfLiteStatus invoke_status = _allocation_interpreter->Invoke(); - #ifdef __PX4_NUTTX - hrt_abstime inference_time_allocation = hrt_absolute_time() - start_time3; - #else - auto inference_time_allocation = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time3).count(); - #endif + int32_t inference_time_allocation = GetTime() - start_time3; if (invoke_status != kTfLiteOk) { PX4_ERR("Invoke() failed"); return; @@ -368,18 +452,14 @@ void MulticopterNeuralNetworkControl::Run() PublishOutput(_output_tensor->data.f); - #ifdef __PX4_NUTTX - hrt_abstime full_controller_time = hrt_absolute_time() - start_time1; - #else - auto full_controller_time = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time1).count(); - #endif + int32_t full_controller_time = GetTime() - start_time1; // Publish the neural control debug message neural_control_s neural_control; - neural_control.timestamp = hrt_absolute_time(); - neural_control.control_inference_time = static_cast(inference_time_control); - neural_control.controller_time = static_cast(full_controller_time); - neural_control.allocation_inference_time = static_cast(inference_time_allocation); + neural_control.timestamp = static_cast(GetTime()); + neural_control.control_inference_time = inference_time_control; + neural_control.controller_time = full_controller_time; + neural_control.allocation_inference_time = inference_time_allocation; for (int i = 0; i < 15; i++) { neural_control.observation[i] = _input_tensor->data.f[i]; } @@ -400,6 +480,17 @@ int MulticopterNeuralNetworkControl::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } +int MulticopterNeuralNetworkControl::print_status() +{ + if(_mode_id == -1) { + PX4_INFO("Neural control flight mode: Mode registration failed"); + PX4_INFO("Neural control flight mode: Request sent: %d", _sent_mode_registration); + }else{ + PX4_INFO("Neural control flight mode: Registered, mode id: %d, arming check id: %d", _mode_id, _arming_check_id); + } + return 0; +} + int MulticopterNeuralNetworkControl::print_usage(const char *reason) { if (reason) { @@ -423,6 +514,8 @@ Outputs: [Actuator motors(4)] return 0; } + + extern "C" __EXPORT int mc_nn_control_main(int argc, char *argv[]) { return MulticopterNeuralNetworkControl::main(argc, argv); diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp index 6e8b1fa9e74a..afbcd74c7043 100644 --- a/src/modules/mc_nn_control/mc_nn_control.hpp +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -66,14 +66,17 @@ #include #include #include -#include -#include +#include +#include +#include // Publications #include #include - -using namespace time_literals; // For the 1_s in the subscription callback +#include +#include +#include +#include class MulticopterNeuralNetworkControl : public ModuleBase, public ModuleParams, public px4::WorkItem @@ -92,6 +95,9 @@ class MulticopterNeuralNetworkControl : public ModuleBase _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _neural_control_pub{ORB_ID(neural_control)}; + uORB::Publication _register_ext_component_request_pub{ORB_ID(register_ext_component_request)}; + uORB::Publication _unregister_ext_component_pub{ORB_ID(unregister_ext_component)}; + uORB::Publication _config_control_setpoints_pub{ORB_ID(config_control_setpoints)}; + uORB::Publication _arming_check_reply_pub{ORB_ID(arming_check_reply)}; // Variables bool _use_neural{false}; + bool _sent_mode_registration{false}; perf_counter_t _loop_perf; /**< loop duration performance counter */ hrt_abstime _last_run{0}; + uint8 _mode_request_id{231}; //Random value + int8 _arming_check_id{-1}; + int8 _mode_id{-1}; tflite::MicroInterpreter* _control_interpreter; tflite::MicroInterpreter* _allocation_interpreter; TfLiteTensor* _input_tensor; diff --git a/src/modules/mc_nn_control/mc_nn_control_params.c b/src/modules/mc_nn_control/mc_nn_control_params.c index b8c7df60bd47..d96f2d46ce3a 100644 --- a/src/modules/mc_nn_control/mc_nn_control_params.c +++ b/src/modules/mc_nn_control/mc_nn_control_params.c @@ -42,6 +42,6 @@ * If true the neural network control is automatically started on boot. * * @boolean - * @group Multicopter Neural Network Control + * @group Neural Control */ PARAM_DEFINE_INT32(MC_NN_EN, 1);