diff --git a/.gitmodules b/.gitmodules index 1ebafcf5a4e5..3a4b364a4f76 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 = https://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/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/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/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/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/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/.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..0b4100137452 --- /dev/null +++ b/src/modules/mc_nn_control/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# 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. +# +############################################################################ +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..aae7b9c8d1ab --- /dev/null +++ b/src/modules/mc_nn_control/allocation_net.hpp @@ -0,0 +1,4 @@ +#include + +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 new file mode 100644 index 000000000000..367f3d425edb --- /dev/null +++ b/src/modules/mc_nn_control/control_net.hpp @@ -0,0 +1,4 @@ +#include + +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 new file mode 100644 index 000000000000..e1465fcc5d89 --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -0,0 +1,522 @@ +/**************************************************************************** + * + * 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.cpp + * Multicopter Neural Network Control module, from position setpoints to control allocator. + * + * @author Sindre Meyer Hegre + */ + +#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 + +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 + const tflite::Model* allocation_model = ::tflite::GetModel(allocation_net_tflite); // TODO: Replace with your model data variable + + // 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; +} + +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)] + + // 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); + +} + +void MulticopterNeuralNetworkControl::PublishOutput(float* command_actions) { + + actuator_motors_s actuator_motors; + 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; + 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(); + if (_sent_mode_registration) { + UnregisterNeuralFlightMode(_arming_check_id, _mode_id); + } + exit_and_cleanup(); + return; + } + + // Register the flight mode with the commander + if (!_sent_mode_registration) { + RegisterNeuralFlightMode(); + _sent_mode_registration = true; + return; + } + + // Check if registration was successful + if (_mode_id == -1 || _arming_check_id == -1) { + CheckModeRegistration(); + return; + } + + 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); + } + + // 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; + + 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 + int32_t start_time2 = GetTime(); + // Inference + TfLiteStatus invoke_status_control = _control_interpreter->Invoke(); + int32_t inference_time_control = GetTime() - start_time2; + 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; + + int32_t start_time3 = GetTime(); + TfLiteStatus invoke_status = _allocation_interpreter->Invoke(); + int32_t inference_time_allocation = GetTime() - start_time3; + 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); + + + int32_t full_controller_time = GetTime() - start_time1; + + // Publish the neural control debug message + neural_control_s neural_control; + 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]; + } + 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); +} + +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) { + 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..afbcd74c7043 --- /dev/null +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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.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 +#include + +// Publications +#include +#include +#include +#include +#include +#include + +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); + + /** @see ModuleBase */ + int print_status() override; + + bool init(); + +private: + void Run() override; + + // Functions + void PopulateInputTensor(); + void PublishOutput(float* command_actions); + void RescaleActions(); + int InitializeNetwork(); + int32_t GetTime(); + void RegisterNeuralFlightMode(); + void UnregisterNeuralFlightMode(int8 arming_check_id, int8 mode_id); + void ConfigureNeuralFlightMode(int8 mode_id); + void ReplyToArmingCheck(int8 request_id); + void CheckModeRegistration(); + + // Subscriptions + uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)}; + uORB::Subscription _arming_check_request_sub{ORB_ID(arming_check_request)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + 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)}; + 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; + 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; +}; 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..d96f2d46ce3a --- /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 Neural Control + */ +PARAM_DEFINE_INT32(MC_NN_EN, 1);