diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index ffbb271a90..aeb0789fb3 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -3,92 +3,45 @@ description: Create a report to help us improve title: "[Bug] " labels: ["bug-report"] body: + - type: markdown + attributes: + value: | + **Tips for a great bug report:** + - Describe what went wrong and what you expected + - Include a flight log link from [logs.px4.io](http://logs.px4.io/) if possible + - Mention your PX4 version, flight controller, and vehicle type if relevant + - type: textarea attributes: label: Describe the bug - description: A clear and concise description of the bug. + description: A clear description of the bug and what you expected to happen. + placeholder: | + What happened and what did you expect instead? + + Steps to reproduce (if applicable): + 1. + 2. + 3. validations: required: true - type: textarea attributes: - label: To Reproduce + label: Flight Log / Additional Information description: | - Steps to reproduce the behavior. - 1. Drone switched on '...' - 2. Uploaded mission '....' (attach QGC mission file) - 3. Took off '....' - 4. See error - validations: - required: false + **Flight log** (highly recommended for flight-related issues): + - Upload to [PX4 Flight Review](http://logs.px4.io/) and paste the link - - type: textarea - attributes: - label: Expected behavior - description: A clear and concise description of what you expected to happen. - validations: - required: false - - - type: textarea - attributes: - label: Screenshot / Media - description: Add screenshot / media if you have them - - - type: textarea - attributes: - label: Flight Log - description: | - *Always* provide a link to the flight log file: - - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)). - - Upload the log to the [PX4 Flight Review](http://logs.px4.io/) - - Share the link to the log (Copy and paste the URL of the log) + **Additional details** (if relevant): + - PX4 version (output of `ver all` in MAVLink Shell) + - Flight controller model + - Vehicle type (multicopter, fixed-wing, VTOL, etc.) + - Screenshots or media placeholder: | - # PASTE HERE THE LINK TO THE LOG + Flight log link: + + Version: + + Hardware: validations: required: false - - - type: markdown - attributes: - value: | - ## Setup - - - type: textarea - attributes: - label: Software Version - description: | - Which version of PX4 are you using? - placeholder: | - # If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC - validations: - required: false - - - type: input - attributes: - label: Flight controller - description: Specify your flight controller model (what type is it, where was it bought from, ...). - validations: - required: false - - - type: dropdown - attributes: - label: Vehicle type - options: - - Multicopter - - Helicopter - - Fixed Wing - - Hybrid VTOL - - Airship/Balloon - - Rover - - Boat - - Submarine - - Other - - - type: textarea - attributes: - label: How are the different components wired up (including port information) - description: Details about how all is wired. - - - type: textarea - attributes: - label: Additional context - description: Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 6dd27384ad..9660e1b97f 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -1,4 +1,4 @@ -blank_issues_enabled: false +blank_issues_enabled: true contact_links: - name: Support Question url: https://docs.px4.io/main/en/contribute/support.html#forums-and-chat diff --git a/.gitmodules b/.gitmodules index 4061b65c11..86cb85c961 100644 --- a/.gitmodules +++ b/.gitmodules @@ -103,3 +103,9 @@ [submodule "src/drivers/ins/sbgecom/sbgECom"] path = src/drivers/ins/sbgecom/sbgECom url = https://github.com/PX4/sbgECom.git +[submodule "src/modules/mc_raptor/blob"] + path = src/modules/mc_raptor/blob + url = https://github.com/rl-tools/px4-blob +[submodule "src/lib/rl_tools/rl_tools"] + path = src/lib/rl_tools/rl_tools + url = https://github.com/rl-tools/rl-tools.git diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 4b2c95eb34..461ce2d95b 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -6,6 +6,16 @@ CONFIG: buildType: RelWithDebInfo settings: CONFIG: px4_sitl_default + px4_sitl_raptor: + short: px4_sitl_raptor + buildType: RelWithDebInfo + settings: + CONFIG: px4_sitl_raptor + px4_sitl_raptor_debug: + short: px4_sitl_raptor_debug + buildType: Debug + settings: + CONFIG: px4_sitl_raptor px4_sitl_spacecraft: short: px4_sitl_spacecraft buildType: RelWithDebInfo diff --git a/CMakeLists.txt b/CMakeLists.txt index d827f7a8c8..7f4ff17ac4 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-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index a2d331ba1e..0e443d30c4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -126,15 +126,6 @@ then set AUTOCNF yes fi -# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value} -env | while IFS='=' read -r line; do - value=${line#*=} - name=${line%%=*} - case $name in - "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" ;; - esac -done - # multi-instance setup # shellcheck disable=SC2154 param set MAV_SYS_ID $((px4_instance+1)) @@ -238,6 +229,15 @@ then exit 1 fi +# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value} +env | while IFS='=' read -r line; do + value=${line#*=} + name=${line%%=*} + case $name in + "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" ;; + esac +done + dataman start # only start the simulator if not in replay mode, as both control the lockstep time diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index 7d266b0691..1e699b5180 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -77,9 +77,6 @@ param set-default NAV_ACC_RAD 2 param set-default RTL_DESCEND_ALT 5 param set-default RTL_RETURN_ALT 5 -# Logging Parameters -param set-default SDLOG_PROFILE 131 - # Sensors Parameters param set-default SENS_CM8JL65_CFG 104 param set-default SENS_FLOW_MAXHGT 25 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index bc9ff44679..b134cabd6c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -78,9 +78,6 @@ param set-default NAV_ACC_RAD 2 param set-default RTL_DESCEND_ALT 5 param set-default RTL_RETURN_ALT 5 -# Logging Parameters -param set-default SDLOG_PROFILE 131 - # Sensors Parameters param set-default SENS_CM8JL65_CFG 202 param set-default SENS_FLOW_MAXHGT 25 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 index 22195b8457..625a879c49 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 @@ -29,9 +29,6 @@ param set-default MPC_MAN_TILT_MAX 60 param set-default THR_MDL_FAC 0.3 -# enable high-rate logging profile (helps with tuning) -param set-default SDLOG_PROFILE 19 - param set-default IMU_DGYRO_CUTOFF 50 param set-default IMU_GYRO_CUTOFF 90 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_atmos b/ROMFS/px4fmu_common/init.d/airframes/70000_atmos index 3ad05a85e0..7cd7a6e743 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_atmos +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_atmos @@ -20,6 +20,9 @@ . ${R}etc/init.d/rc.sc_defaults +# Overwrite DDS AG IP to `192.168.0.1` +param set-default UXRCE_DDS_AG_IP -1062731775 + param set-default CA_AIRFRAME 14 param set-default MAV_TYPE 45 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index adff4e963f..24a3f81ed7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -41,3 +41,9 @@ if param compare -s MC_NN_EN 1 then mc_nn_control start fi + + +if param compare -s MC_RAPTOR_ENABLE 1 +then + mc_raptor start +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 0caa831cef..87bd5981a3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -8,9 +8,6 @@ set VEHICLE_TYPE spacecraft # MAV_TYPE_SPACECRAFT_ORBITTER param set-default MAV_TYPE 45 -# Set micro-dds-client to use ethernet and IP-address 192.168.0.1 -param set-default UXRCE_DDS_AG_IP -1062731775 - # Disable preflight disarm to not interfere with external launching param set-default COM_DISARM_PRFLT -1 param set-default CBRK_SUPPLY_CHK 894281 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index b67384483b..ea6b8d4f1e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -237,6 +237,12 @@ then tla2528 start -X fi +# Start TMP102 temperature sensor +if param compare SENS_EN_TMP102 1 +then + tmp102 start -X +fi + # probe for optional external I2C devices if param compare SENS_EXT_I2C_PRB 1 then diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 1931deb9a3..bd92940ab7 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -17,37 +17,12 @@ if [[ -f $1"/.git" || -d $1"/.git" ]]; then SUBMODULE_STATUS=$(git submodule summary "$1") STATUSRETVAL=$(echo $SUBMODULE_STATUS | grep -A20 -i "$1") if ! [[ -z "$STATUSRETVAL" ]]; then - echo -e "\033[31mChecked $1 submodule, ACTION REQUIRED:\033[0m" - echo "" - echo -e "Different commits:" + echo -e "\033[33mWarning: $1 submodule has uncommitted changes:\033[0m" echo -e "$SUBMODULE_STATUS" echo "" + echo -e "To update submodules to the expected version, run:" + echo -e " \033[94mgit submodule sync --recursive && git submodule update --init --recursive\033[0m" echo "" - echo -e " *******************************************************************************" - echo -e " * \033[31mIF YOU DID NOT CHANGE THIS FILE (OR YOU DON'T KNOW WHAT A SUBMODULE IS):\033[0m *" - echo -e " * \033[31mHit 'u' and to update ALL submodules and resolve this.\033[0m *" - echo -e " * (performs \033[94mgit submodule sync --recursive\033[0m *" - echo -e " * and \033[94mgit submodule update --init --recursive\033[0m ) *" - echo -e " *******************************************************************************" - echo "" - echo "" - echo -e " Only for EXPERTS:" - echo -e " $1 submodule is not in the recommended version." - echo -e " Hit 'y' and to continue the build with this version. Hit to resolve manually." - echo -e " Use \033[94mgit add $1 && git commit -m 'Updated $1'\033[0m to choose this version (careful!)" - echo "" - read user_cmd - if [ "$user_cmd" == "y" ]; then - echo "Continuing build with manually overridden submodule.." - elif [ "$user_cmd" == "u" ]; then - git submodule sync --recursive -- $1 - git submodule update --init --recursive -- $1 || true - git submodule update --init --recursive --force -- $1 - echo "Submodule fixed, continuing build.." - else - echo "Build aborted." - exit 1 - fi fi else git submodule --quiet sync --recursive --quiet -- $1 diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index b83359785b..24b6511b6d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -98,40 +98,6 @@ class firmware(object): desc = {} image = bytes() - crctab = array.array('I', [ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) - crcpad = bytearray(b'\xff\xff\xff\xff') def __init__(self, path): @@ -149,17 +115,15 @@ class firmware(object): def property(self, propname): return self.desc[propname] - def __crc32(self, bytes, state): - for byte in bytes: - index = (state ^ byte) & 0xff - state = self.crctab[index] ^ (state >> 8) - return state - def crc(self, padlen): - state = self.__crc32(self.image, int(0)) - for _ in range(len(self.image), (padlen - 1), 4): - state = self.__crc32(self.crcpad, state) - return state + state = 0xFFFFFFFF + state = zlib.crc32(self.image, state) + padding_length = padlen - len(self.image) + if padding_length > 0: + padding = b'\xff' * padding_length + state = zlib.crc32(padding, state) + + return (state ^ 0xFFFFFFFF) & 0xFFFFFFFF class uploader: diff --git a/boards/px4/fmu-v6c/raptor.px4board b/boards/px4/fmu-v6c/raptor.px4board new file mode 100644 index 0000000000..be07cafcbe --- /dev/null +++ b/boards/px4/fmu-v6c/raptor.px4board @@ -0,0 +1,95 @@ +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_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_ACTUATORS_VERTIQ_IO=y +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_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_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_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_LIB_RL_TOOLS=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_LATERAL_LONGITUDINAL_CONTROL=n +CONFIG_MODULES_FW_MODE_MANAGER=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=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_RAPTOR=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +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_NUM_MISSION_ITMES_SUPPORTED=1000 +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_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_USE_IFCI_CONFIGURATION=y diff --git a/boards/px4/sitl/raptor.px4board b/boards/px4/sitl/raptor.px4board new file mode 100644 index 0000000000..b00735efc2 --- /dev/null +++ b/boards/px4/sitl/raptor.px4board @@ -0,0 +1,89 @@ +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_ROOT_PATH="." +CONFIG_BOARD_TESTING=y +CONFIG_COMMON_SIMULATION=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_EKF2_VERBOSE_STATUS=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 +CONFIG_FIGURE_OF_EIGHT=y +CONFIG_LIB_RL_TOOLS=y +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=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_EKF2=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_LATERAL_LONGITUDINAL_CONTROL=y +CONFIG_MODULES_FW_MODE_MANAGER=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_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_RAPTOR=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_PAYLOAD_DELIVERER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_REPLAY=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_MECANUM=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_SIMULATION_GZ_MSGS=y +CONFIG_MODULES_SIMULATION_GZ_PLUGINS=y +CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y +CONFIG_MODULES_SPACECRAFT=n +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_NUM_MISSION_ITMES_SUPPORTED=10000 +CONFIG_PLATFORM_POSIX=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 diff --git a/docs/assets/advanced/neural_networks/raptor/method.jpg b/docs/assets/advanced/neural_networks/raptor/method.jpg new file mode 100644 index 0000000000..935da7683d Binary files /dev/null and b/docs/assets/advanced/neural_networks/raptor/method.jpg differ diff --git a/docs/assets/advanced/neural_networks/raptor/results_figure_eight.svg b/docs/assets/advanced/neural_networks/raptor/results_figure_eight.svg new file mode 100644 index 0000000000..81d3a51286 --- /dev/null +++ b/docs/assets/advanced/neural_networks/raptor/results_figure_eight.svg @@ -0,0 +1 @@ + diff --git a/docs/assets/advanced/neural_networks/raptor/results_line.svg b/docs/assets/advanced/neural_networks/raptor/results_line.svg new file mode 100644 index 0000000000..0d7d142013 --- /dev/null +++ b/docs/assets/advanced/neural_networks/raptor/results_line.svg @@ -0,0 +1 @@ + diff --git a/docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg b/docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg new file mode 100644 index 0000000000..40ba52877a Binary files /dev/null and b/docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg differ diff --git a/docs/assets/config/actuators/qgc_actuators_gimbal.png b/docs/assets/config/actuators/qgc_actuators_gimbal.png index e19434e678..a062d88da7 100644 Binary files a/docs/assets/config/actuators/qgc_actuators_gimbal.png and b/docs/assets/config/actuators/qgc_actuators_gimbal.png differ diff --git a/docs/assets/hardware/gps/ark/ark_g5_rtk_gps.png b/docs/assets/hardware/gps/ark/ark_g5_rtk_gps.png new file mode 100644 index 0000000000..78ba78bac5 Binary files /dev/null and b/docs/assets/hardware/gps/ark/ark_g5_rtk_gps.png differ diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index aab7ed87c7..5cd8646f75 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -128,7 +128,7 @@ - [LED Meanings](getting_started/led_meanings.md) - [Tune/Sound Meanings](getting_started/tunes.md) - [QGroundControl Flight-Readiness Status](flying/pre_flight_checks.md) - + - [Asset Tracking](debug/asset_tracking.md) - [Hardware Selection & Setup](hardware/drone_parts.md) - [Flight Controllers (Autopilots)](flight_controller/index.md) - [Flight Controller Selection](getting_started/flight_controller_selection.md) @@ -271,6 +271,8 @@ - [Holybro M8N & M9N GPS](gps_compass/gps_holybro_m8n_m9n.md) - [Sky-Drones SmartAP GPS](gps_compass/gps_smartap.md) - [RTK GNSS](gps_compass/rtk_gps.md) + - [ARK G5 RTK GPS](dronecan/ark_g5_rtk_gps.md) + - [ARK G5 RTK HEADING GPS](dronecan/ark_g5_rtk_heading_gps.md) - [ARK RTK GPS (CAN)](dronecan/ark_rtk_gps.md) - [ARK RTK GPS L1 L5 (CAN)](dronecan/ark_rtk_gps_l1_l2.md) - [ARK X20 RTK GPS (CAN)](dronecan/ark_x20_rtk_gps.md) @@ -820,9 +822,11 @@ - [Camera Integration/Architecture](camera/camera_architecture.md) - [Computer Vision](advanced/computer_vision.md) - [Motion Capture (VICON, Optitrack, NOKOV)](tutorials/motion-capture.md) - - [Neural Networks](advanced/neural_networks.md) - - [Neural Network Module Utilities](advanced/nn_module_utilities.md) - - [TensorFlow Lite Micro (TFLM)](advanced/tflm.md) + - [Neural Networks](neural_networks/index.md) + - [MC NN Control Module (Generic)](neural_networks/mc_neural_network_control.md) + - [Neural Network Module Utilities](neural_networks/nn_module_utilities.md) + - [TensorFlow Lite Micro (TFLM)](neural_networks/tflm.md) + - [RAPTOR Adaptive RL NN Module](neural_networks/raptor.md) - [Installing driver for Intel RealSense R200](advanced/realsense_intel_driver.md) - [Switching State Estimators](advanced/switching_state_estimators.md) - [Out-of-Tree Modules](advanced/out_of_tree_modules.md) @@ -902,6 +906,7 @@ - [Licenses](contribute/licenses.md) - [Releases](releases/index.md) - [main (alpha)](releases/main.md) + - [1.17 (alpha)](releases/1.17.md) - [1.16 (stable)](releases/1.16.md) - [1.15](releases/1.15.md) - [1.14](releases/1.14.md) diff --git a/docs/en/advanced/gimbal_control.md b/docs/en/advanced/gimbal_control.md index b5a7596ec2..f449b74587 100644 --- a/docs/en/advanced/gimbal_control.md +++ b/docs/en/advanced/gimbal_control.md @@ -74,7 +74,7 @@ For example, you might have the following settings to assign the gimbal roll, pi ![Gimbal Actuator config](../../assets/config/actuators/qgc_actuators_gimbal.png) -The PWM values to use for the disarmed, maximum and minimum values can be determined in the same way as other servo, using the [Actuator Test sliders](../config/actuators.md#actuator-testing) to confirm that each slider moves the appropriate axis, and changing the values so that the gimbal is in the appropriate position at the disarmed, low and high position in the slider. +The PWM values to use for the disarmed, maximum, center and minimum values can be determined in the same way as other servo, using the [Actuator Test sliders](../config/actuators.md#actuator-testing) to confirm that each slider moves the appropriate axis, and changing the values so that the gimbal is in the appropriate position at the disarmed, low, center and high position in the slider. The values may also be provided in gimbal documentation. ## Gimbal Control in Missions diff --git a/docs/en/advanced/neural_networks.md b/docs/en/advanced/neural_networks.md index 91829f3a88..039259fd5f 100644 --- a/docs/en/advanced/neural_networks.md +++ b/docs/en/advanced/neural_networks.md @@ -1,119 +1 @@ -# Neural Networks - - - -::: warning -This is an experimental module. -Use at your own risk. -::: - -The Multicopter Neural Network (NN) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) is an example module that allows you to experiment with using a pre-trained neural network on PX4. -It might be used, for example, to experiment with controllers for non-traditional drone morphologies, computer vision tasks, and so on. - -The module integrates a pre-trained neural network based on the [TensorFlow Lite Micro (TFLM)](../advanced/tflm.md) module. -The module is trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md) multicopter frame. -While the controller is fairly robust, and might work on other platforms, we recommend [Training your own Network](#training-your-own-network) if you use a different vehicle. -Note that after training the network you will need to update and rebuild PX4. - -TLFM is a mature inference library intended for use on embedded devices. -It has support for several architectures, so there is a high likelihood that you can build it for the board you want to use. -If not, there are other possible NN frameworks, such as [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) and [Executorch](https://pytorch.org/executorch-overview). - -This document explains how you can include the module in your PX4 build, and provides a broad overview of how it works. -The other documents in the section provide more information about the integration, allowing you to replace the NN with a version trained on different data, or even to replace the TLFM library altogether. - -If you are looking for more resources to learn about the module, a website has been created with links to a youtube video and a workshop paper. A full master's thesis will be added later. [A Neural Network Mode for PX4 on Embedded Flight Controllers](https://ntnu-arl.github.io/px4-nns/). - -## Neural Network PX4 Firmware - -::: warning -This module requires Ubuntu 24.04 or newer (it is not supported in Ubuntu 22.04). -::: - -The module has been tested on a number of configurations, which can be build locally using the commands: - -```sh -make px4_sitl_neural -``` - -```sh -make px4_fmu-v6c_neural -``` - -```sh -make mro_pixracerpro_neural -``` - -You can add the module to other board configurations by modifying their `default.px4board file` configuration to include these lines: - -```sh -CONFIG_LIB_TFLM=y -CONFIG_MODULES_MC_NN_CONTROL=y -``` - -:::tip -The `mc_nn_control` module takes up roughly 50KB, and many of the `default.px4board file` are already close to filling all the flash on their boards. To make room for the neural control module you can remove the include statements for other modules, such as FW, rover, VTOL and UUV. -::: - -## Example Module Overview - -The example module replaces the entire controller structure as well as the control allocator, as shown in the diagram below: - -![neural_control](../../assets/advanced/neural_control.png) - -In the [controller diagram](../flight_stack/controller_diagrams.md) you can see the [uORB message](../middleware/uorb.md) flow. -We hook into this flow by subscribing to messages at particular points, using our neural network to calculate outputs, and then publishing them into the next point in the flow. -We also need to stop the module publishing the topic to be replaced, which is covered in [Neural Network Module: System Integration](nn_module_utilities.md) - -### Input - -The input can be changed to whatever you want. -Set up the input you want to use during training and then provide the same input in PX4. -In the Neural Control module the input is an array of 15 numbers, and consists of these values in this order: - -- [3] Local position error. (goal position - current position) -- [6] The first 2 rows of a 3 dimensional rotation matrix. -- [3] Linear velocity -- [3] Angular velocity - -All the input values are collected from uORB topics and transformed into the correct representation in the `PopulateInputTensor()` function. -PX4 uses the NED frame representation, while the Aerial Gym Simulator, in which the NN was trained, uses the ENU representation. -Therefore two rotation matrices are created in the function and all the inputs are transformed from the NED representation to the ENU one. - -![ENU-NED](../../assets/advanced/ENU-NED.png) - -ENU and NED are just rotation representations, the translational difference is only there so both can be seen in the same figure. - -### Output - -The output consists of 4 values, the motor forces, one for each motor. -These are transformed in the `RescaleActions()` function. -This is done because PX4 expects normalized motor commands while the Aerial Gym Simulator uses physical values. -So the output from the network needs to be normalized before they can be sent to the motors in PX4. - -The commands are published to the [ActuatorMotors](../msg_docs/ActuatorMotors.md) topic. -The publishing is handled in `PublishOutput(float* command_actions)` function. - -:::tip -If the neural control mode is too aggressive or unresponsive the [MC_NN_THRST_COEF](../advanced_config/parameter_reference.md#MC_NN_THRST_COEF) parameter can be tuned. -Decrease it for more thrust. -::: - -## Training your own Network - -The network is currently trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md). -But the controller is somewhat robust, so it could work directly on other platforms, but performing system identification and training a new network is recommended. - -Since the Aerial Gym Simulator is open-source you can download it and train your own networks as long as you have access to an NVIDIA GPU. -If you want to train a control network optimized for your platform you can follow the instructions in the [Aerial Gym Documentation](https://ntnu-arl.github.io/aerial_gym_simulator/9_sim2real/). - -You should do one system identification flight for this and get an approximate inertia matrix for your platform. -On the `sys-id` flight you need ESC telemetry, you can read more about that in [DSHOT](../peripherals/dshot.md). - -Then do the following steps: - -- Do a hover flight -- Read of the logs what RPM is required for the drone to hover. -- Use the weight of each motor, length of the motor arms, total weight of the platform with battery to calculate an approximate inertia matrix for the platform. -- Insert these values into the Aerial Gym configuration and train your network. -- Convert the network as explained in [TFLM](tflm.md). + diff --git a/docs/en/can/index.md b/docs/en/can/index.md index b5ca6a57b3..cf33b7a8c4 100644 --- a/docs/en/can/index.md +++ b/docs/en/can/index.md @@ -10,6 +10,10 @@ CAN it is designed to be democratic and uses differential signaling. For this reason it is very robust even over longer cable lengths (on large vehicles), and avoids a single point of failure. CAN also allows status feedback from peripherals and convenient firmware upgrades over the bus. +PX4 has the ability to track and log detailed information from CAN devices, including firmware versions, hardware versions, and serial numbers. +This enables unique identification and lifecycle tracking of hardware connected to the flight controller. +See [Asset Tracking](../debug/asset_tracking.md) for more information. + PX4 supports two software protocols for communicating with CAN devices: - [DroneCAN](../dronecan/index.md): PX4 recommends this for most common setups. diff --git a/docs/en/config/safety.md b/docs/en/config/safety.md index 5640f1dcd7..bef0e0ea18 100644 --- a/docs/en/config/safety.md +++ b/docs/en/config/safety.md @@ -121,21 +121,22 @@ PX4 and the receiver may also need to be configured in order to _detect RC loss_ ![Safety - RC Loss (QGC)](../../assets/qgc/setup/safety/safety_rc_loss.png) -The QGCroundControl Safety UI allows you to set the [failsafe action](#failsafe-actions) and [RC Loss timeout](#COM_RC_LOSS_T). -Users that want to disable the RC loss failsafe in specific automatic modes (mission, hold, offboard) can do so using the parameter [COM_RCL_EXCEPT](#COM_RCL_EXCEPT). +The QGCroundControl Safety UI allows you to set the [failsafe action](#failsafe-actions) and [manual control loss timeout](#COM_RC_LOSS_T). +Users that want to disable this failsafe in specific modes can do so using the parameter [COM_RCL_EXCEPT](#COM_RCL_EXCEPT). Additional (and underlying) parameter settings are shown below. | Parameter | Setting | Description | | ----------------------------------------------------------------------------------------------------- | --------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| [COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T) | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the old manual control setpoint until the timeout triggers. | +| [COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T) | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the last known stick position until the timeout triggers. | | [COM_FAIL_ACT_T](../advanced_config/parameter_reference.md#COM_FAIL_ACT_T) | Failsafe Reaction Delay | Delay in seconds between failsafe condition being triggered (`COM_RC_LOSS_T`) and failsafe action (RTL, Land, Hold). In this state the vehicle waits in hold mode for the manual control source to reconnect. This might be set longer for long-range flights so that intermittent connection loss doesn't immediately invoke the failsafe. It can be to zero so that the failsafe triggers immediately. | | [NAV_RCL_ACT](../advanced_config/parameter_reference.md#NAV_RCL_ACT) | Failsafe Action | Disabled, Loiter, Return, Land, Disarm, Terminate. | -| [COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions | Set the modes in which manual control loss is ignored: Mission, Hold, Offboard. | +| [COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions | Set modes in which manual control loss is ignored. | ## Data Link Loss Failsafe -The Data Link Loss failsafe is triggered if a telemetry link (connection to ground station) is lost. +The Data Link Loss failsafe is triggered if the connection to the last MAVLink ground station like QGroundControl is lost. +Users that want to disable this failsafe in specific modes can do so using the parameter [COM_DLL_EXCEPT](#COM_DLL_EXCEPT). ![Safety - Data Link Loss (QGC)](../../assets/qgc/setup/safety/safety_data_link_loss.png) @@ -145,12 +146,7 @@ The settings and underlying parameters are shown below. | ---------------------- | ------------------------------------------------------------------------ | --------------------------------------------------------------------------------- | | Data Link Loss Timeout | [COM_DL_LOSS_T](../advanced_config/parameter_reference.md#COM_DL_LOSS_T) | Amount of time after losing the data connection before the failsafe will trigger. | | Failsafe Action | [NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT) | Disabled, Hold mode, Return mode, Land mode, Disarm, Terminate. | - -The following settings also apply, but are not displayed in the QGC UI. - -| Setting | Parameter | Description | -| ----------------------------------------------------------- | -------------------------------------------------------------------------- | ---------------------------------------------------- | -| Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes where DL loss will not trigger a failsafe. | +| Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes in which data link loss is ignored. | ## Geofence Failsafe diff --git a/docs/en/debug/asset_tracking.md b/docs/en/debug/asset_tracking.md new file mode 100644 index 0000000000..e933596944 --- /dev/null +++ b/docs/en/debug/asset_tracking.md @@ -0,0 +1,68 @@ +# Asset Tracking + + + +PX4 can track and log detailed information about external hardware devices connected to the flight controller. +This enables unique identification of vehicle parts throughout their operational lifetime using device IDs, serial numbers, and version information. + +::: info +Asset tracking is currently implemented for [DroneCAN](../dronecan/index.md) devices only. +::: + +## Overview + +Asset tracking allows you to determine exactly which hardware is installed on a vehicle, providing serial number, version, and other information. +This makes it easier to track and maintain specific vehicle parts across multiple vehicles, to quickly see what versions you're running when debugging, and log component information for regulatory audits. + +Asset tracking automatically collects and logs the following metadata from external devices: + +- **Device identification**: Vendor name, model name, device type +- **Version information**: Firmware version, hardware version +- **Unique identifiers**: Serial number, device ID +- **Device capabilities**: ESC, GPS, magnetometer, barometer, etc. + +This information is published via the [`device_information`](../msg_docs/DeviceInformation.md) uORB topic and logged to flight logs. +This enables fleet management, maintenance tracking, and troubleshooting. + +## Viewing Device Information + +### Real-Time Monitoring + +You can view device information in real-time using the [MAVLink Shell](../debug/mavlink_shell.md) or console: + +```sh +listener device_information +``` + +Example output for a CAN GPS module: + +```plain +TOPIC: device_information + device_information + timestamp: 16258961403 (0.216525 seconds ago) + device_id: 8944643 (Type: 0x88, UAVCAN:0 (0x7C)) + device_type: 5 + vendor_name: "cubepilot" + model_name: "here4" + firmware_version: "1.14.3006590" + hardware_version: "4.19" + serial_number: "1c00410018513331" +``` + +Device information is published in a round-robin fashion for each detected device, at a rate of approximately 1 Hz. + +### Multi-Capability Devices + +Devices with multiple sensors (e.g., a CAN GPS/magnetometer combo module like the HERE4) register separate device information entries for each capability. +Each entry shares the same serial number and base metadata but has a different `device_id` corresponding to the specific sensor capability. + +## Flight Log Analysis + +Device information is automatically logged to flight logs. +You can extract it using [pyulog](../log/flight_log_analysis.md#pyulog), though note that fields like vendor name, model name, and serial number are stored as `char` arrays and require additional parsing. + +## See Also + +- [CAN (DroneCAN & Cyphal)](../can/index.md) — CAN bus configuration and setup +- [DroneCAN](../dronecan/index.md) — DroneCAN-specific documentation +- [Flight Log Analysis](../log/flight_log_analysis.md) — Flight log analysis diff --git a/docs/en/dronecan/ark_g5_rtk_gps.md b/docs/en/dronecan/ark_g5_rtk_gps.md new file mode 100644 index 0000000000..0a7a1d8f4d --- /dev/null +++ b/docs/en/dronecan/ark_g5_rtk_gps.md @@ -0,0 +1,112 @@ +# ARK G5 RTK GPS + +::: info +This GPS module is made in the USA and NDAA compliant. +::: + +[ARK G5 RTK GPS](https://arkelectron.com/product/ark-g5-rtk-gps/) is a [DroneCAN](index.md) quad-band [RTK GPS](../gps_compass/rtk_gps.md). + +The module incorporates the [Septentrio mosaic-G5 P3 Ultra-compact high-precision GPS/GNSS receiver module](https://www.u-blox.com/en/product/zed-x20p-module), magnetometer, barometer, IMU, and buzzer module. + +![ARK G5 RTK GPS](../../assets/hardware/gps/ark/ark_g5_rtk_gps.png) + +## Where to Buy + +Order this module from: + +- [ARK Electronics](https://arkelectron.com/product/ark-g5-rtk-gps/) (US) + +## Hardware Specifications + +- [DroneCAN](index.md) RTK GNSS, Magnetometer, Barometer, IMU, and Buzzer Module +- [Dronecan Firmware Updating](../dronecan/index.md#firmware-update) +- Sensors + - [Septentrio mosaic-G5 P3 Ultra-compact high-precision GPS/GNSS receiver module](https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3) + - All-band all constellation GNSS receiver + - All-in-view satellite tracking: multi-constellation, quad-band GNSS module receiver + - Full raw data with positioning measurements and Galileo HAS positioning service compatibility + - Best-in-class RTK cm-level positioning accuracy + - Advanced GNSS+ algorithms + - 20Hz update rate + - [ST IIS2MDC Magnetometer](https://www.st.com/en/mems-and-sensors/iis2mdc.html) + - [Bosch BMP390 Barometer](https://www.bosch-sensortec.com/products/environmental-sensors/pressure-sensors/bmp390/) + - [Invensense ICM-42688-P 6-Axis IMU](https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42688-p/) +- STM32F412VGH6 MCU +- Safety Button +- Buzzer +- Two CAN Connectors (Pixhawk Connector Standard 4-pin JST GH) +- G5 "UART 2" Connector + - 4-pin JST GH + - TX, RX, PPS, GND +- G5 USB C +- Debug Connector (Pixhawk Connector Standard 6-pin JST SH) +- LED Indicators + - GPS Fix + - RTK Status + - RGB system status +- USA Built +- NDAA Compliant +- Power Requirements + - 5V + - 270mA +- Dimensions + - Without Antenna + - 48.0mm x 40.0mm x 15.4mm + - 13.0g + - With Antenna + - 48.0mm x 40.0mm x 51.0mm + - 43.5g +- Includes + - CAN Cable (Pixhawk Connector Standard 4-pin) + - Full-Frequency Helical GPS Antenna + +## Hardware Setup + +### Wiring + +The ARK G5 RTK GPS is connected to the CAN bus using a [Pixhawk connector standard](https://github.com/pixhawk/Pixhawk-Standards/blob/master/DS-009%20Pixhawk%20Connector%20Standard.pdf) 4-pin JST GH cable. +For more information, refer to the [CAN Wiring](../can/index.md#wiring) instructions. + +### Mounting + +The recommended mounting orientation is with the connectors on the board pointing towards the **back of vehicle**. + +The sensor can be mounted anywhere on the frame, but you will need to specify its position, relative to vehicle centre of gravity, during [PX4 Configuration](#px4-configuration). + +## Firmware Setup + +The Septentrio G5 module firmware can be updated using the Septentrio [RxTools](https://www.septentrio.com/en/products/gps-gnss-receiver-software/rxtools) application. + +## Flight Controller Setup + +### Enabling DroneCAN + +In order to use the ARK G5 RTK GPS, connect it to the Pixhawk CAN bus and enable the DroneCAN driver by setting parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` for dynamic node allocation (or `3` if using [DroneCAN ESCs](../dronecan/escs.md)). + +The steps are: + +- In _QGroundControl_ set the parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` or `3` and reboot (see [Finding/Updating Parameters](../advanced_config/parameters.md)). +- Connect ARK G5 RTK GPS CAN to the Pixhawk CAN. + +Once enabled, the module will be detected on boot. + +There is also CAN built-in bus termination via [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) + +### PX4 Configuration + +You need to set necessary [DroneCAN](index.md) parameters and define offsets if the sensor is not centred within the vehicle: + +- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO). +- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK G5 RTK GPS from the vehicle's centre of gravity. + +## LED Meanings + +The GPS status lights are located to the right of the connectors: + +- Blinking green is GPS fix +- Blinking blue is received corrections and RTK Float +- Solid blue is RTK Fixed + +## See Also + +- [ARK G5 RTK GPS Documentation](https://docs.arkelectron.com/gps/ark-g5-rtk-gps) (ARK Docs) diff --git a/docs/en/dronecan/ark_g5_rtk_heading_gps.md b/docs/en/dronecan/ark_g5_rtk_heading_gps.md new file mode 100644 index 0000000000..80505235ce --- /dev/null +++ b/docs/en/dronecan/ark_g5_rtk_heading_gps.md @@ -0,0 +1,150 @@ +# ARK G5 RTK HEADING GPS + +::: info +This GPS module is made in the USA and NDAA compliant. +::: + +[ARK G5 RTK HEADING GPS](https://arkelectron.com/product/ark-g5-rtk-gps/) is a [DroneCAN](index.md) quad-band dual antenna [RTK GPS](../gps_compass/rtk_gps.md) that additionally provides vehicle yaw information from GPS. + +The module incorporates the [Septentrio mosaic-G5 P3H Ultra-compact high-precision GPS/GNSS receiver module with heading capability](https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3H), magnetometer, barometer, IMU, and buzzer module. + +![ARK G5 RTK HEADING GPS](../../assets/hardware/gps/ark/ark_g5_rtk_gps.png) + +## Where to Buy + +Order this module from: + +- [ARK Electronics](https://arkelectron.com/product/ark-g5-rtk-heading-gps/) (US) + +## Hardware Specifications + +- [DroneCAN](index.md) RTK GNSS, Magnetometer, Barometer, IMU, and Buzzer Module +- [Dronecan Firmware Updating](../dronecan/index.md#firmware-update) +- Sensors + - [Septentrio mosaic-G5 P3H Ultra-compact high-precision GPS/GNSS receiver module with heading capability](https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3H) + - All-band all constellation GNSS receiver + - All-in-view satellite tracking: multi-constellation, quad-band GNSS module receiver + - Full raw data with positioning measurements and Galileo HAS positioning service compatibility + - Best-in-class RTK cm-level positioning accuracy + - Advanced GNSS+ algorithms + - 20Hz update rate + - [ST IIS2MDC Magnetometer](https://www.st.com/en/mems-and-sensors/iis2mdc.html) + - [Bosch BMP390 Barometer](https://www.bosch-sensortec.com/products/environmental-sensors/pressure-sensors/bmp390/) + - [Invensense ICM-42688-P 6-Axis IMU](https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42688-p/) +- STM32F412VGH6 MCU +- Safety Button +- Buzzer +- Two CAN Connectors (Pixhawk Connector Standard 4-pin JST GH) +- G5 "UART 2" Connector + - 4-pin JST GH + - TX, RX, PPS, GND +- G5 USB C +- Debug Connector (Pixhawk Connector Standard 6-pin JST SH) +- LED Indicators + - GPS Fix + - RTK Status + - RGB system status +- USA Built +- NDAA Compliant +- Power Requirements + - 5V + - 270mA +- Dimensions + - Without Antenna + - 48.0mm x 40.0mm x 15.4mm + - 13.0g + - With Antenna + - 48.0mm x 40.0mm x 51.0mm + - 43.5g +- Includes + - CAN Cable (Pixhawk Connector Standard 4-pin) + - Full-Frequency Helical GPS Antenna + +## Hardware Setup + +### Wiring + +The ARK G5 RTK HEADING GPS is connected to the CAN bus using a [Pixhawk connector standard](https://github.com/pixhawk/Pixhawk-Standards/blob/master/DS-009%20Pixhawk%20Connector%20Standard.pdf) 4-pin JST GH cable. +For more information, refer to the [CAN Wiring](../can/index.md#wiring) instructions. + +### Mounting + +The recommended mounting orientation is with the connectors on the board pointing towards the **back of vehicle**. + +The sensor can be mounted anywhere on the frame, but you will need to specify its position, relative to vehicle centre of gravity, during [PX4 configuration](#px4-configuration). + +## Firmware Setup + +The Septentrio G5 module firmware can be updated using the Septentrio [RxTools](https://www.septentrio.com/en/products/gps-gnss-receiver-software/rxtools) application. + +## Flight Controller Setup + +### Enabling DroneCAN + +In order to use the ARK G5 RTK HEADING GPS, connect it to the Pixhawk CAN bus and enable the DroneCAN driver by setting parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` for dynamic node allocation (or `3` if using [DroneCAN ESCs](../dronecan/escs.md)). + +The steps are: + +- In _QGroundControl_ set the parameter [UAVCAN_ENABLE](../advanced_config/parameter_reference.md#UAVCAN_ENABLE) to `2` or `3` and reboot (see [Finding/Updating Parameters](../advanced_config/parameters.md)). +- Connect ARK G5 RTK HEADING GPS CAN to the Pixhawk CAN. + +Once enabled, the module will be detected on boot. + +There is also CAN built-in bus termination via [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) + +### PX4 Configuration + +You need to set necessary [DroneCAN](index.md) parameters and define offsets if the sensor is not centred within the vehicle: + +- Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true. +- Enable GPS blending to ensure the heading is always published by setting [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) to 7 (all three bits checked). +- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO). +- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK G5 RTK HEADING GPS from the vehicle's centre of gravity. + +### Parameter references + +This GPS is using ARK's private driver, the prameters below only exist on the firmware we ship the GPS with. You can set these params either in QGC or using the DroneCAN GUI Tool. + +#### SEP_OFFS_YAW (float) + +Heading offset angle for dual antenna GPS setups that support heading estimation. +Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the Rover/ANT2 antenna is in front. +The offset angle increases clockwise. +Set this to 90 if the ANT2 antenna is placed on the right side of the vehicle and the Moving Base/MAIN antenna is on the left side. + +- Default: 0 +- Min: -360 +- Max: 360 +- Unit: degree + +#### SEP_OFFS_PITCH (float) + +Vertical offsets can be compensated for by adjusting the Pitch offset. +Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis. This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame. Since pitch is defined as the right-handed rotation about the vehicle Y axis, a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch. + +- Default: 0 +- Min: -90 +- Max: 90 +- Unit: degree + +#### SEP_OUT_RATE (enum) + +Configures the output rate for GNSS data messages. + +- -1: OnChange (Default) +- 50: 50 ms +- 100: 100 ms +- 200: 200 ms +- 500: 500 ms + +## LED Meanings + +The GPS status lights are located to the right of the connectors: + +- Blinking green is GPS fix +- Blinking blue is received corrections and RTK Float +- Solid blue is RTK Fixed + +## See Also + +- [ARK G5 RTK HEADING GPS Documentation](https://docs.arkelectron.com/gps/ark-g5-rtk-gps) (ARK Docs) diff --git a/docs/en/dronecan/index.md b/docs/en/dronecan/index.md index 04dd9e3aa7..13898c9dc3 100644 --- a/docs/en/dronecan/index.md +++ b/docs/en/dronecan/index.md @@ -27,6 +27,8 @@ Connecting peripherals over DroneCAN has many benefits: - Wiring is less complicated as you can have a single bus for connecting all your ESCs and other DroneCAN peripherals. - Setup is easier as you configure ESC numbering by manually spinning each motor. - It allows users to configure and update the firmware of all CAN-connected devices centrally through PX4. +- PX4 automatically tracks device information (vendor, model, versions, serial numbers) for maintenance and fleet management. + See [Asset Tracking](../debug/asset_tracking.md). ## Supported Hardware diff --git a/docs/en/features_fw/gain_compression.md b/docs/en/features_fw/gain_compression.md index 7efb48f59c..25259fd05e 100644 --- a/docs/en/features_fw/gain_compression.md +++ b/docs/en/features_fw/gain_compression.md @@ -1,6 +1,6 @@ # Gain compression - + Automatic gain compression reduces the gains of the angular-rate PID whenever oscillations are detected. It monitors the angular-rate controller output through a band-pass filter to identify these oscillations. diff --git a/docs/en/flight_controller/micoair743-lite.md b/docs/en/flight_controller/micoair743-lite.md index 4f7ac36bc6..7b3ed25128 100644 --- a/docs/en/flight_controller/micoair743-lite.md +++ b/docs/en/flight_controller/micoair743-lite.md @@ -1,6 +1,6 @@ # MicoAir743-Lite - + :::warning PX4 does not manufacture this (or any) autopilot. diff --git a/docs/en/flight_controller/radiolink_pix6.md b/docs/en/flight_controller/radiolink_pix6.md index 98e5dae7a2..1574314338 100644 --- a/docs/en/flight_controller/radiolink_pix6.md +++ b/docs/en/flight_controller/radiolink_pix6.md @@ -1,6 +1,6 @@ # RadiolinkPIX6 Flight Controller - + :::warning PX4 does not manufacture this (or any) autopilot. diff --git a/docs/en/flight_controller/x-mav_ap-h743r1.md b/docs/en/flight_controller/x-mav_ap-h743r1.md index 435d39b640..f527e9876d 100644 --- a/docs/en/flight_controller/x-mav_ap-h743r1.md +++ b/docs/en/flight_controller/x-mav_ap-h743r1.md @@ -1,6 +1,6 @@ -# AP-H743-R1 +# AP-H743-R1 Flight Controller - + :::warning PX4 does not manufacture this (or any) autopilot. @@ -50,6 +50,7 @@ These flight controllers are [manufacturer supported](../flight_controller/autop Order from [X-MAV](https://www.x-mav.cn/). ## Radio Control + A Radio Control (RC) system is required if you want to manually control your vehicle (PX4 does not require a radio system for autonomous flight modes). You will need to select a compatible transmitter/receiver and then bind them so that they communicate (read the instructions that come with your specific transmitter/receiver). @@ -59,14 +60,14 @@ CRSF receiver must be wired to a spare port (UART) on the Flight Controller. The ## Serial Port Mapping -| UART | Device | Port | -| ------ | ---------- | ------------- | -| USART1 | /dev/ttyS0 | GPS | -| USART2 | /dev/ttyS1 | GPS2 | -| USART3 | /dev/ttyS2 | TELEM1 | -| UART4 | /dev/ttyS3 | TELEM2 | -| UART7 | /dev/ttyS4 | TELEM3 | -| UART8 | /dev/ttyS5 | SERIAL4 | +| UART | Device | Port | +| ------ | ---------- | ------- | +| USART1 | /dev/ttyS0 | GPS | +| USART2 | /dev/ttyS1 | GPS2 | +| USART3 | /dev/ttyS2 | TELEM1 | +| UART4 | /dev/ttyS3 | TELEM2 | +| UART7 | /dev/ttyS4 | TELEM3 | +| UART8 | /dev/ttyS5 | SERIAL4 | ## PWM Output @@ -133,13 +134,14 @@ The complete set of supported configurations can be found in the [Airframe Refer ## Debug Port ### SWD + The [SWD interface](../debug/swd_debug.md) operate on the **FMU-DEBUG** port (`FMU-DEBUG`). The debug port (`FMU-DEBUG`) uses a [JST SM04B-GHS-TB](https://www.digikey.com/en/products/detail/jst-sales-america-inc/SM04B-GHS-TB/807788) connector and has the following pinout: -| Pin | Signal | Volt | -| ------- | -------------- | ----- | -| 1 (red) | 5V+ | +5V | -| 2 (blk) | FMU_SWDIO | +3.3V | -| 3 (blk) | FMU_SWCLK | +3.3V | -| 4 (blk) | GND | GND | +| Pin | Signal | Volt | +| ------- | --------- | ----- | +| 1 (red) | 5V+ | +5V | +| 2 (blk) | FMU_SWDIO | +3.3V | +| 3 (blk) | FMU_SWCLK | +3.3V | +| 4 (blk) | GND | GND | diff --git a/docs/en/flight_modes_fw/hold.md b/docs/en/flight_modes_fw/hold.md index 3691bb3441..9265479c87 100644 --- a/docs/en/flight_modes_fw/hold.md +++ b/docs/en/flight_modes_fw/hold.md @@ -2,11 +2,14 @@ -The _Hold_ flight mode causes the vehicle to loiter (circle) around its current GPS position and maintain its current altitude. +The _Hold_ flight mode causes the vehicle to loiter around its current GPS position and maintain its current altitude. + +The mode supports a [number of distinct loiter modes](#loiter-modes), which are triggered using different QGC controls or MAVLink commands. +These allow loitering with circular and figure 8 flight paths. :::tip _Hold mode_ can be used to pause a mission or to help you regain control of a vehicle in an emergency. -It is usually activated with a pre-programmed switch. +It is usually activated with a pre-programmed RC switch. ::: ::: info @@ -24,24 +27,80 @@ It is usually activated with a pre-programmed switch. ::: -## Technical Summary +## Loiter modes -The aircraft circles around the GPS hold position at the current altitude. -The vehicle will first ascend to [NAV_MIN_LTR_ALT](#NAV_MIN_LTR_ALT) if the mode is engaged below this altitude. +### Default Loiter -RC stick movement is ignored. +The aircraft circles around the position at which the mode was triggered and maintain its current altitude. +The loiter radius is set by the parameter [NAV_LOITER_RAD](#NAV_LOITER_RAD). +Note that if the vehicle altitude is below [NAV_MIN_LTR_ALT](#NAV_MIN_LTR_ALT), it will ascend to that minimum altitude before circling. -### Parameters +The default loiter mode is entered when you switch to Hold mode without explicitly specifying any loiter behaviour. +For example, if you switch to Hold mode using an RC switch, select **Hold** on the QGC flight mode selector, or activate the mode using the MAVLink [MAV_CMD_DO_SET_MODE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_MODE) command. + +### Orbit Loiter Mode + + + +The aircraft travels towards a _specified_ orbit center position, then circles it with a given direction and radius. + +This behaviour can be accessed in QGroundControl by clicking on the map in Fly view, selecting **Orbit at Location**, and configuring the radius. + +The behavior can be triggered using the MAVLink [MAV_CMD_DO_ORBIT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_ORBIT) command. +Note that PX4 respects the specified centre point (`param5`, `param6`, `param7`), and the radius and direction (`param1`). +PX4 ignores `param3` (Yaw behaviour) and `param4` (Orbits). +The value of `param2` (velocity) is also ignored, but the speed can be controlled using the [MAV_CMD_DO_CHANGE_SPEED](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_CHANGE_SPEED) command (constrained between `FW_AIRSPD_MAX` and `FW_AIRSPD_MIN`). +PX4 outputs orbit status using the [ORBIT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#ORBIT_EXECUTION_STATUS) message. + +### Figure 8 Loiter Mode + + + +The aircraft flys towards the closest point on a specified figure 8 path and then follows it. +The path is defined by the figure 8 centre position, orientation, and radius of two circles. + +The feature is experimental, and is not present in PX4 firmware by default (on most flight controller boards). +It can be included by setting the `CONFIG_FIGURE_OF_EIGHT` key in the [PX4 board configuration](../hardware/porting_guide_config.md#px4-board-configuration-kconfig) for your board and rebuilding. +For example, this is enabled on the [default.px4board](https://github.com/PX4/PX4-Autopilot/blob/main/boards/auterion/fmu-v6s/default.px4board#L46) file for the `auterion/fmu-v6s` board. + +The behavior can be triggered using the MAVLink [MAV_CMD_DO_FIGURE_EIGHT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_FIGURE_EIGHT) command (PX4 respects all the parameters). +PX4 outputs the figure 8 status using the [FIGURE_EIGHT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#FIGURE_EIGHT_EXECUTION_STATUS) message. + +::: info +Figure 8 loitering is not currently supported by QGC: [QGC#12778: Need Support Figure of eight (8 figure) loitering by QGC](https://github.com/mavlink/qgroundcontrol/issues/12778). +::: + +Figure 8 loitering is also available in the simulator. +You can test it in [Gazebo](../sim_gazebo_gz/index.md) using a fixed wing frame: + +```sh +make px4_sitl gz_rc_cessna +``` + +## Parameters Hold mode behaviour can be configured using the parameters below. | Parameter | Description | | -------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------- | -| [NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD) | The radius of the loiter circle. | +| [NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD) | The radius of the loiter circle. | | [NAV_MIN_LTR_ALT](../advanced_config/parameter_reference.md#NAV_MIN_LTR_ALT) | Minimum height for loiter mode (vehicle will ascend to this altitude if mode is engaged at a lower altitude). | +## MAVLink Commands + +The following commands are relevant to this mode: + +- [MAV_CMD_DO_ORBIT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_ORBIT) - Switch to Hold mode and start the specified [Orbit loiter](#orbit-loiter-mode). + Params 2 (velocity), 3 (yaw), 4 (orbits) are ignored. + [ORBIT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#ORBIT_EXECUTION_STATUS) is emitted. +- [MAV_CMD_DO_FIGURE_EIGHT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_FIGURE_EIGHT) - Switch to Hold mode and start the specified [Figure 8 loiter](#figure-8-loiter-mode). + All params are respected. + [FIGURE_EIGHT_EXECUTION_STATUS](https://mavlink.io/en/messages/common.html#FIGURE_EIGHT_EXECUTION_STATUS) is emitted. + +Note, other commands may be supported. + ## See Also -[Hold Mode (MC)](../flight_modes_mc/hold.md) +- [Hold Mode (MC)](../flight_modes_mc/hold.md) diff --git a/docs/en/flight_modes_fw/takeoff.md b/docs/en/flight_modes_fw/takeoff.md index 021a79df53..265c09f4b4 100644 --- a/docs/en/flight_modes_fw/takeoff.md +++ b/docs/en/flight_modes_fw/takeoff.md @@ -49,8 +49,8 @@ If the local position is invalid or becomes invalid while executing the takeoff, ::: info -- Takeoff towards a target position was added in . -- Holding wings level and ascending to clearance attitude when local position is invalid during takeoff was added in . +- Takeoff towards a target position was added in . +- Holding wings level and ascending to clearance attitude when local position is invalid during takeoff was added in . - QGroundControl does not support `MAV_CMD_NAV_TAKEOFF` (at time of writing). ::: diff --git a/docs/en/gps_compass/rtk_gps.md b/docs/en/gps_compass/rtk_gps.md index c0e192c4c3..29e2fe5358 100644 --- a/docs/en/gps_compass/rtk_gps.md +++ b/docs/en/gps_compass/rtk_gps.md @@ -20,52 +20,59 @@ The RTK compatible devices below that are expected to work with PX4 (it omits di The table indicates devices that also output yaw, and that can provide yaw when two on-vehicle units are used. It also highlights devices that connect via the CAN bus, and those which support PPK (Post-Processing Kinematic). -| Device | GPS | Compass | [DroneCAN](../dronecan/index.md) | [GPS Yaw](#configuring-gps-as-yaw-heading-source) | PPK | -| :-------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------: | :------: | :------------------------------: | :-----------------------------------------------: | :-: | -| [ARK RTK GPS](../dronecan/ark_rtk_gps.md) | F9P | BMM150 | ✓ | [Dual F9P][DualF9P] | -| [ARK RTK GPS L1 L5](../dronecan/ark_rtk_gps_l1_l2.md) | F9P | BMM150 | ✓ | | -| [ARK MOSAIC-X5 RTK GPS](../dronecan/ark_mosaic__rtk_gps.md) | Mosaic-X5 | IIS2MDC | ✓ | [Septentrio Dual Antenna][SeptDualAnt] | -| [ARK X20 RTK GPS](../dronecan/ark_x20_rtk_gps.md) | X20P | BMP390 | ✓ | | -| [CUAV C-RTK GPS](../gps_compass/rtk_gps_cuav_c-rtk.md) | M8P/M8N | ✓ | | | -| [CUAV C-RTK2](../gps_compass/rtk_gps_cuav_c-rtk2.md) | F9P | ✓ | | [Dual F9P][DualF9P] | -| [CUAV C-RTK 9Ps GPS](../gps_compass/rtk_gps_cuav_c-rtk-9ps.md) | F9P | RM3100 | | [Dual F9P][DualF9P] | -| [CUAV C-RTK2 PPK/RTK GNSS](../gps_compass/rtk_gps_cuav_c-rtk.md) | F9P | RM3100 | | | ✓ | -| [CubePilot Here+ RTK GPS](../gps_compass/rtk_gps_hex_hereplus.md) | M8P | HMC5983 | | | -| [CubePilot Here3 CAN GNSS GPS (M8N)](https://www.cubepilot.org/#/here/here3) | M8P | ICM20948 | ✓ | | -| [Drotek SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html) | F9P | RM3100 | | [Dual F9P][DualF9P] | -| [DATAGNSS NANO HRTK Receiver](../gps_compass/rtk_gps_datagnss_nano_hrtk.md) | [D10P](https://docs.datagnss.com/gnss/gnss_module/D10P_RTK) | IST8310 | | ✘ | -| [DATAGNSS GEM1305 RTK Receiver](../gps_compass/rtk_gps_gem1305.md) | TAU951M | IST8310 | | ✘ | -| [Femtones MINI2 Receiver](../gps_compass/rtk_gps_fem_mini2.md) | FB672, FB6A0 | ✓ | | | -| [Freefly RTK GPS](../gps_compass/rtk_gps_freefly.md) | F9P | IST8310 | | | -| [Holybro H-RTK ZED-F9P RTK Rover (DroneCAN variant)](../dronecan/holybro_h_rtk_zed_f9p_gps.md) | F9P | RM3100 | ✓ | [Dual F9P][DualF9P] | -| [Holybro H-RTK ZED-F9P RTK Rover](https://holybro.com/collections/h-rtk-gps/products/h-rtk-zed-f9p-rover) | F9P | RM3100 | | [Dual F9P][DualF9P] | -| [Holybro H-RTK F9P Ultralight](https://holybro.com/products/h-rtk-f9p-ultralight) | F9P | IST8310 | | [Dual F9P][DualF9P] | -| [Holybro H-RTK F9P Helical or Base](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md) | F9P | IST8310 | | [Dual F9P][DualF9P] | -| [Holybro DroneCAN H-RTK F9P Helical](https://holybro.com/products/dronecan-h-rtk-f9p-helical) | F9P | BMM150 | ✓ | [Dual F9P][DualF9P] | -| [Holybro H-RTK F9P Rover Lite](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md) | F9P | IST8310 | | | | -| [Holybro DroneCAN H-RTK F9P Rover](https://holybro.com/products/dronecan-h-rtk-f9p-rover) | F9P | BMM150 | | [Dual F9P][DualF9P] | -| [Holybro H-RTK M8P GNSS](../gps_compass/rtk_gps_holybro_h-rtk-m8p.md) | M8P | IST8310 | | -| [Holybro H-RTK Unicore UM982 GPS](../gps_compass/rtk_gps_holybro_unicore_um982.md) | UM982 | IST8310 | | [Unicore Dual Antenna][UnicoreDualAnt] | -| [LOCOSYS Hawk R1](../gps_compass/rtk_gps_locosys_r1.md) | MC-1612-V2b | | | | -| [LOCOSYS Hawk R2](../gps_compass/rtk_gps_locosys_r2.md) | MC-1612-V2b | IST8310 | | | -| [mRo u-blox ZED-F9 RTK L1/L2 GPS](https://store.mrobotics.io/product-p/m10020d.htm) | F9P | ✓ | | [Dual F9P][DualF9P] | -| [Navisys L1/L2 ZED-F9P RTK - Base only](https://www.navisys.com.tw/productdetail?name=GR901&class=RTK) | F9P | | | | -| [RaccoonLab L1/L2 ZED-F9P][RaccoonLab L1/L2 ZED-F9P] | F9P | RM3100 | ✓ | | | -| [RaccoonLab L1/L2 ZED-F9P with external antenna][RaccnLabL1L2ZED-F9P ext_ant] | F9P | RM3100 | ✓ | | -| [Septentrio AsteRx-m3 Pro](../gps_compass/septentrio_asterx-rib.md) | AsteRx | ✓ | | [Septentrio Dual Antenna][SeptDualAnt] | ✓ | -| [Septentrio mosaic-go](../gps_compass/septentrio_mosaic-go.md) | mosaic X5 / mosaic H | ✓ | | [Septentrio Dual Antenna][SeptDualAnt] | ✓ | -| [SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html) | F9P | ✓ | | [Dual F9P][DualF9P] | -| [SparkFun GPS-RTK2 Board - ZED-F9P](https://www.sparkfun.com/products/15136) | F9P | ✓ | | [Dual F9P][DualF9P] | -| [Trimble MB-Two](../gps_compass/rtk_gps_trimble_mb_two.md) | F9P | ✓ | | ✓ | | +| Device | GPS | Compass | [DroneCAN] | [GPS Yaw] | PPK | +| :-------------------------------------------------------------------------------------------------------- | :------------------: | :------: | :--------: | :-----------------------: | :-: | +| [ARK G5 RTK GPS](../dronecan/ark_g5_rtk_gps.md) | [mosaic-G5 P3] | IIS2MDC | ✓ | | | +| [ARK G5 RTK HEADING GPS](../dronecan/ark_g5_rtk_heading_gps.md) | [mosaic-G5 P3H] | IIS2MDC | ✓ | [Heading Capability][mosaic-G5 P3H] | | +| [ARK RTK GPS](../dronecan/ark_rtk_gps.md) | F9P | BMM150 | ✓ | [Dual F9P] | | +| [ARK RTK GPS L1 L5](../dronecan/ark_rtk_gps_l1_l2.md) | F9P | BMM150 | ✓ | | | +| [ARK MOSAIC-X5 RTK GPS](../dronecan/ark_mosaic__rtk_gps.md) | Mosaic-X5 | IIS2MDC | ✓ | [Septentrio Dual Antenna] | | +| [ARK X20 RTK GPS](../dronecan/ark_x20_rtk_gps.md) | X20P | IIS2MDC | ✓ | | | +| [CUAV C-RTK GPS](../gps_compass/rtk_gps_cuav_c-rtk.md) | M8P/M8N | ✓ | | | | +| [CUAV C-RTK2](../gps_compass/rtk_gps_cuav_c-rtk2.md) | F9P | ✓ | | [Dual F9P] | | +| [CUAV C-RTK 9Ps GPS](../gps_compass/rtk_gps_cuav_c-rtk-9ps.md) | F9P | RM3100 | | [Dual F9P] | | +| [CUAV C-RTK2 PPK/RTK GNSS](../gps_compass/rtk_gps_cuav_c-rtk.md) | F9P | RM3100 | | | ✓ | +| [CubePilot Here+ RTK GPS](../gps_compass/rtk_gps_hex_hereplus.md) | M8P | HMC5983 | | | | +| [CubePilot Here3 CAN GNSS GPS (M8N)](https://www.cubepilot.org/#/here/here3) | M8P | ICM20948 | ✓ | | | +| [Drotek SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html) | F9P | RM3100 | | [Dual F9P] | | +| [DATAGNSS NANO HRTK Receiver](../gps_compass/rtk_gps_datagnss_nano_hrtk.md) | [D10P] | IST8310 | | ✘ | | +| [DATAGNSS GEM1305 RTK Receiver](../gps_compass/rtk_gps_gem1305.md) | TAU951M | IST8310 | | ✘ | | +| [Femtones MINI2 Receiver](../gps_compass/rtk_gps_fem_mini2.md) | FB672, FB6A0 | ✓ | | | | +| [Freefly RTK GPS](../gps_compass/rtk_gps_freefly.md) | F9P | IST8310 | | | | +| [Holybro H-RTK ZED-F9P RTK Rover (DroneCAN variant)](../dronecan/holybro_h_rtk_zed_f9p_gps.md) | F9P | RM3100 | ✓ | [Dual F9P] | | +| [Holybro H-RTK ZED-F9P RTK Rover](https://holybro.com/collections/h-rtk-gps/products/h-rtk-zed-f9p-rover) | F9P | RM3100 | | [Dual F9P] | | +| [Holybro H-RTK F9P Ultralight](https://holybro.com/products/h-rtk-f9p-ultralight) | F9P | IST8310 | | [Dual F9P] | | +| [Holybro H-RTK F9P Helical or Base](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md) | F9P | IST8310 | | [Dual F9P] | | +| [Holybro DroneCAN H-RTK F9P Helical](https://holybro.com/products/dronecan-h-rtk-f9p-helical) | F9P | BMM150 | ✓ | [Dual F9P] | | +| [Holybro H-RTK F9P Rover Lite](../gps_compass/rtk_gps_holybro_h-rtk-f9p.md) | F9P | IST8310 | | | | +| [Holybro DroneCAN H-RTK F9P Rover](https://holybro.com/products/dronecan-h-rtk-f9p-rover) | F9P | BMM150 | | [Dual F9P] | | +| [Holybro H-RTK M8P GNSS](../gps_compass/rtk_gps_holybro_h-rtk-m8p.md) | M8P | IST8310 | | | | +| [Holybro H-RTK Unicore UM982 GPS](../gps_compass/rtk_gps_holybro_unicore_um982.md) | UM982 | IST8310 | | [Unicore Dual Antenna] | | +| [LOCOSYS Hawk R1](../gps_compass/rtk_gps_locosys_r1.md) | MC-1612-V2b | | | | | +| [LOCOSYS Hawk R2](../gps_compass/rtk_gps_locosys_r2.md) | MC-1612-V2b | IST8310 | | | | +| [mRo u-blox ZED-F9 RTK L1/L2 GPS](https://store.mrobotics.io/product-p/m10020d.htm) | F9P | ✓ | | [Dual F9P] | | +| [Navisys L1/L2 ZED-F9P RTK - Base only](https://www.navisys.com.tw/productdetail?name=GR901&class=RTK) | F9P | | | | | +| [RaccoonLab L1/L2 ZED-F9P][RaccoonLab L1/L2 ZED-F9P] | F9P | RM3100 | ✓ | | | +| [RaccoonLab L1/L2 ZED-F9P with external antenna][RaccnLabL1L2ZED-F9P ext_ant] | F9P | RM3100 | ✓ | | | +| [Septentrio AsteRx-m3 Pro](../gps_compass/septentrio_asterx-rib.md) | AsteRx | ✓ | | [Septentrio Dual Antenna] | ✓ | +| [Septentrio mosaic-go](../gps_compass/septentrio_mosaic-go.md) | mosaic X5 / mosaic H | ✓ | | [Septentrio Dual Antenna] | ✓ | +| [SIRIUS RTK GNSS ROVER (F9P)](https://store-drotek.com/911-sirius-rtk-gnss-rover-f9p.html) | F9P | ✓ | | [Dual F9P] | | +| [SparkFun GPS-RTK2 Board - ZED-F9P](https://www.sparkfun.com/products/15136) | F9P | ✓ | | [Dual F9P] | | +| [Trimble MB-Two](../gps_compass/rtk_gps_trimble_mb_two.md) | F9P | ✓ | | ✓ | | [RaccnLabL1L2ZED-F9P ext_ant]: https://docs.raccoonlab.co/guide/gps_mag_baro/gnss_external_antenna_f9p_v320.html [RaccoonLab L1/L2 ZED-F9P]: https://docs.raccoonlab.co/guide/gps_mag_baro/gps_l1_l2_zed_f9p.html -[DualF9P]: ../gps_compass/u-blox_f9p_heading.md -[SeptDualAnt]: ../gps_compass/septentrio.md#gnss-based-heading -[UnicoreDualAnt]: ../gps_compass/rtk_gps_holybro_unicore_um982.md#enable-gps-heading-yaw +[Dual F9P]: ../gps_compass/u-blox_f9p_heading.md +[Septentrio Dual Antenna]: ../gps_compass/septentrio.md#gnss-based-heading +[Unicore Dual Antenna]: ../gps_compass/rtk_gps_holybro_unicore_um982.md#enable-gps-heading-yaw [DATAGNSS GEM1305 RTK]: ../gps_compass/rtk_gps_gem1305.md +[DroneCAN]: ../dronecan/index.md +[GPS Yaw]: #configuring-gps-as-yaw-heading-source +[mosaic-G5 P3]: https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3 +[mosaic-G5 P3H]: https://www.septentrio.com/en/products/gnss-receivers/gnss-receiver-modules/mosaic-G5-P3H +[D10P]: https://docs.datagnss.com/gnss/gnss_module/D10P_RTK Notes: @@ -143,6 +150,7 @@ The RTK GPS connection is essentially plug and play: ![survey-in](../../assets/qgc/setup/rtk/qgc_rtk_survey-in.png) 1. Once Survey-in completes: + - The RTK GPS icon changes to white and _QGroundControl_ starts to stream position data to the vehicle: ![RTK streaming](../../assets/qgc/setup/rtk/qgc_rtk_streaming.png) diff --git a/docs/en/middleware/uxrce_dds.md b/docs/en/middleware/uxrce_dds.md index eb568e5a58..3e1a38d226 100644 --- a/docs/en/middleware/uxrce_dds.md +++ b/docs/en/middleware/uxrce_dds.md @@ -321,7 +321,7 @@ The configuration can be done using the [UXRCE-DDS parameters](../advanced_confi - [UXRCE_DDS_SYNCT](../advanced_config/parameter_reference.md#UXRCE_DDS_SYNCT): Bridge time synchronization enable. The uXRCE-DDS client module can synchronize the timestamp of the messages exchanged over the bridge. This is the default configuration. In certain situations, for example during [simulations](../ros2/user_guide.md#ros-gazebo-and-px4-time-synchronization), this feature may be disabled. - - [`UXRCE_DDS_NS_IDX`](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX): Index-based namespace definition + - [UXRCE_DDS_NS_IDX](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX) : Index-based namespace definition Setting this parameter to any value other than `-1` creates a namespace with the prefix `uav_` and the specified value, e.g. `uav_0`, `uav_1`, etc. See [namespace](#customizing-the-namespace) for methods to define richer or arbitrary namespaces. @@ -426,7 +426,7 @@ will generate topics under the namespaces: ::: -- A simple index-based namespace can be applied by setting the parameter [`UXRCE_DDS_NS_IDX`](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX) to a value between 0 and 9999. +- A simple index-based namespace can be applied by setting the parameter [`UXRCE_DDS_NS_IDX`](../advanced_config/parameter_reference.md#UXRCE_DDS_NS_IDX) to a value between 0 and 9999. This will generate a namespace such as `/uav_0`, `/uav_1`, and so on. This technique is ideal if vehicles must be persistently associated with namespaces because their clients are automatically started through PX4. diff --git a/docs/en/neural_networks/index.md b/docs/en/neural_networks/index.md new file mode 100644 index 0000000000..4379f079f9 --- /dev/null +++ b/docs/en/neural_networks/index.md @@ -0,0 +1,21 @@ +# Neural Network Control + +PX4 supports the following mechanisms for using neural networks for multirotor control: + +- [MC Neural Networks Control](../neural_networks/mc_neural_network_control.md) — A generic neural network module that you can modify to use different underlying neural network and training models and compile into the firmware. +- [RAPTOR: A Neural Network Module for Adaptive Quadrotor Control](../neural_networks/raptor.md) — An adaptive RL NN module that works well with different Quad configurations without additional training. + +Generally you will select the former if you wish to experiment with custom neural network architectures and train them using PyTorch or TensorFlow, and the latter if you want to use a pre-trained neural-network controller that works out-of-the-box (without training for your particular platform) or if you train your own policies using [RLtools](https://rl.tools). + +Note that both modules are experimental and provided for experimentation. +The table below provides more detail on the differences. + +| Use Case | [`mc_raptor`](../neural_networks/raptor.md) | [`mc_nn_control`](../neural_networks/mc_neural_network_control.md) | +| ---------------------------------------------------------------- | ------------------------------------------- | ------------------------------------------------------------------ | +| Pre-trained policy that adapts to any quadrotor without training | ✓ RAPTOR | ✘ | +| Train policy in PyTorch/TF | ✘ | ✓ TF Lite | +| Train policy in RLtools | ✓ | ✘ | +| Use manual control (remote) with NN policy | ✘ GPS/MoCap | ✓ Manual attitude commands | +| Load policy checkpoints from SD card | ✓ Upload via MAVLink FTP | ✘ Compiled into firmware | +| Offboard setpoints | ✓ MAVLink | ✘ | +| Internal Trajectory Generator | ✓ (Position, Lissajous) | ✘ | diff --git a/docs/en/neural_networks/mc_neural_network_control.md b/docs/en/neural_networks/mc_neural_network_control.md new file mode 100644 index 0000000000..6f83a67cb3 --- /dev/null +++ b/docs/en/neural_networks/mc_neural_network_control.md @@ -0,0 +1,119 @@ +# MC Neural Networks Control + + + +::: warning +This is an experimental module. +Use at your own risk. +::: + +The Multicopter Neural Network (NN) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) is an example module that allows you to experiment with using a pre-trained neural network on PX4. +It might be used, for example, to experiment with controllers for non-traditional drone morphologies, computer vision tasks, and so on. + +The module integrates a pre-trained neural network based on the [TensorFlow Lite Micro (TFLM)](./tflm.md) module. +The module is trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md) multicopter frame. +While the controller is fairly robust, and might work on other platforms, we recommend [Training your own Network](#training-your-own-network) if you use a different vehicle. +Note that after training the network you will need to update and rebuild PX4. + +TLFM is a mature inference library intended for use on embedded devices. +It has support for several architectures, so there is a high likelihood that you can build it for the board you want to use. +If not, there are other possible NN frameworks, such as [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) and [Executorch](https://pytorch.org/executorch-overview). + +This document explains how you can include the module in your PX4 build, and provides a broad overview of how it works. +The other documents in the section provide more information about the integration, allowing you to replace the NN with a version trained on different data, or even to replace the TLFM library altogether. + +If you are looking for more resources to learn about the module, a website has been created with links to a youtube video and a workshop paper. A full master's thesis will be added later. [A Neural Network Mode for PX4 on Embedded Flight Controllers](https://ntnu-arl.github.io/px4-nns/). + +## Neural Network PX4 Firmware + +::: warning +This module requires Ubuntu 24.04 or newer (it is not supported in Ubuntu 22.04). +::: + +The module has been tested on a number of configurations, which can be build locally using the commands: + +```sh +make px4_sitl_neural +``` + +```sh +make px4_fmu-v6c_neural +``` + +```sh +make mro_pixracerpro_neural +``` + +You can add the module to other board configurations by modifying their `default.px4board file` configuration to include these lines: + +```sh +CONFIG_LIB_TFLM=y +CONFIG_MODULES_MC_NN_CONTROL=y +``` + +:::tip +The `mc_nn_control` module takes up roughly 50KB, and many of the `default.px4board file` are already close to filling all the flash on their boards. To make room for the neural control module you can remove the include statements for other modules, such as FW, rover, VTOL and UUV. +::: + +## Example Module Overview + +The example module replaces the entire controller structure as well as the control allocator, as shown in the diagram below: + +![neural_control](../../assets/advanced/neural_control.png) + +In the [controller diagram](../flight_stack/controller_diagrams.md) you can see the [uORB message](../middleware/uorb.md) flow. +We hook into this flow by subscribing to messages at particular points, using our neural network to calculate outputs, and then publishing them into the next point in the flow. +We also need to stop the module publishing the topic to be replaced, which is covered in [Neural Network Module: System Integration](nn_module_utilities.md) + +### Input + +The input can be changed to whatever you want. +Set up the input you want to use during training and then provide the same input in PX4. +In the Neural Control module the input is an array of 15 numbers, and consists of these values in this order: + +- [3] Local position error. (goal position - current position) +- [6] The first 2 rows of a 3 dimensional rotation matrix. +- [3] Linear velocity +- [3] Angular velocity + +All the input values are collected from uORB topics and transformed into the correct representation in the `PopulateInputTensor()` function. +PX4 uses the NED frame representation, while the Aerial Gym Simulator, in which the NN was trained, uses the ENU representation. +Therefore two rotation matrices are created in the function and all the inputs are transformed from the NED representation to the ENU one. + +![ENU-NED](../../assets/advanced/ENU-NED.png) + +ENU and NED are just rotation representations, the translational difference is only there so both can be seen in the same figure. + +### Output + +The output consists of 4 values, the motor forces, one for each motor. +These are transformed in the `RescaleActions()` function. +This is done because PX4 expects normalized motor commands while the Aerial Gym Simulator uses physical values. +So the output from the network needs to be normalized before they can be sent to the motors in PX4. + +The commands are published to the [ActuatorMotors](../msg_docs/ActuatorMotors.md) topic. +The publishing is handled in `PublishOutput(float* command_actions)` function. + +:::tip +If the neural control mode is too aggressive or unresponsive the [MC_NN_THRST_COEF](../advanced_config/parameter_reference.md#MC_NN_THRST_COEF) parameter can be tuned. +Decrease it for more thrust. +::: + +## Training your own Network + +The network is currently trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md). +But the controller is somewhat robust, so it could work directly on other platforms, but performing system identification and training a new network is recommended. + +Since the Aerial Gym Simulator is open-source you can download it and train your own networks as long as you have access to an NVIDIA GPU. +If you want to train a control network optimized for your platform you can follow the instructions in the [Aerial Gym Documentation](https://ntnu-arl.github.io/aerial_gym_simulator/9_sim2real/). + +You should do one system identification flight for this and get an approximate inertia matrix for your platform. +On the `sys-id` flight you need ESC telemetry, you can read more about that in [DSHOT](../peripherals/dshot.md). + +Then do the following steps: + +- Do a hover flight +- Read of the logs what RPM is required for the drone to hover. +- Use the weight of each motor, length of the motor arms, total weight of the platform with battery to calculate an approximate inertia matrix for the platform. +- Insert these values into the Aerial Gym configuration and train your network. +- Convert the network as explained in [TFLM](tflm.md). diff --git a/docs/en/advanced/nn_module_utilities.md b/docs/en/neural_networks/nn_module_utilities.md similarity index 97% rename from docs/en/advanced/nn_module_utilities.md rename to docs/en/neural_networks/nn_module_utilities.md index d00f8aff63..b1df217ded 100644 --- a/docs/en/advanced/nn_module_utilities.md +++ b/docs/en/neural_networks/nn_module_utilities.md @@ -2,7 +2,7 @@ The neural control module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) implements an end-to-end controller utilizing neural networks. -The parts of the module directly concerned with generating the code for the trained neural network and integrating it into the module are covered in [TensorFlow Lite Micro (TFLM)](../advanced/tflm.md). +The parts of the module directly concerned with generating the code for the trained neural network and integrating it into the module are covered in [TensorFlow Lite Micro (TFLM)](./tflm.md). This page covers the changes that were made to integrate the module into PX4, both within the module, and in larger system configuration. ::: tip @@ -75,7 +75,7 @@ Which timing library is included and used is based on wether PX4 is built with N ## Changing the setpoint -The module uses the [TrajectorySetpoint](../msg_docs/TrajectorySetpoint.md) message’s position fields to define its target. +The module uses the [TrajectorySetpoint](../msg_docs/TrajectorySetpoint.md) message's position fields to define its target. To follow a trajectory, you can send updated setpoints. For an example of how to do this in a PX4 module, see the [mc_nn_testing](https://github.com/SindreMHegre/PX4-Autopilot-public/tree/main/src/modules/mc_nn_testing) module in this fork. Note that this is not included in upstream PX4. diff --git a/docs/en/neural_networks/raptor.md b/docs/en/neural_networks/raptor.md new file mode 100644 index 0000000000..3867908fd8 --- /dev/null +++ b/docs/en/neural_networks/raptor.md @@ -0,0 +1,221 @@ +# RAPTOR: A Neural Network Module for Adaptive Quadrotor Control + + + +::: warning +This is an experimental module. +Use at your own risk. +::: + +RAPTOR is a tiny reinforcement-learning based neural network module for quadrotor control that can be used to control a wide variety of quadrotors without retuning. + +This topic provides an overview of the fundamental concepts, and explains how you can use the module in simulation and real hardware. + +## Overview + +![Visual Abstract](../../assets/advanced/neural_networks/raptor/visual_abstract.jpg) + +RAPTOR is an adaptive policy for end-to-end quadrotor control. +It is motivated by the human ability to adapt learned behaviours to similar situations. +For example, while humans may initially require many hours of driving experience to be able to smoothly control the car and blend into traffic, when faced with a new vehicle they do not need to re-learn how to drive — they only need to experience a few rough braking/acceleration/steering responses to adjust their previously learned behavior. + +Reinforcement Learning (RL) is a machine learning technique that uses trial and error to learn decision making/control behaviors, which is similar to the way that humans learn to drive. +RL is interesting for controlling robots (and particularly UAVs) because it overcomes some fundamental limitations of classic, modular control architectures (information loss at module boundaries, requirement for expert tuning, etc). +RL has been very successful in [high-performance quadrotor flight](https://doi.org/10.1038/s41586-023-06419-4), but previous designs have not been particularly adaptable to new frames and vehicle types. + +RAPTOR fills this gap and demonstrates a single, tiny neural-network control policy that can control a wide variety of quadrotors (tested on real quadrotors from 32 g to 2.4 kg). + +For more details please refer to this video: + + + +The method we developed for training the RAPTOR policy is called Meta-Imitation Learning: + +![Diagram showing the Method Overview](../../assets/advanced/neural_networks/raptor/method.jpg) + +You can torture test the RAPTOR policy in your browser at [https://raptor.rl.tools](https://raptor.rl.tools) or in the embedded app here: + + + +For more information please refer to the paper at [https://arxiv.org/abs/2509.11481](https://arxiv.org/abs/2509.11481). + +## Structure + +The RAPTOR control policy is an end-to-end policy that takes position, orientation, linear velocity and angular velocity as inputs and outputs motor commands (`actuator_motors`). +To integrate it into PX4 we use the external mode registration facilities in PX4 (which also works well for internal modes as demonstrated in `mc_nn_control`). +Because of this architecture the `mc_raptor` module is completely decoupled from all other PX4 logic. + +By default, the RAPTOR module expects setpoints via `trajectory_setpoint` messages. +If no `trajectory_setpoint` messages are received or if no `trajectory_setpoint` is received within 200 ms, the current position and orientation (with zero velocity) is used as the setpoint. +Since feeding setpoints reliably via telemetry is still a challenge, we also implement a simple option to generate internal reference trajectories (controlled through the `MC_RAPTOR_INTREF` parameter) for demonstration and benchmarking purposes. + +## Features + +- Tiny neural network (just 2084 parameters) => minimal CPU usage +- Easily maintainable + - Simple CMake setup + - Self-contained (no interference with other modules) + - Single, simple and well-maintained dependency (RLtools) +- Loading neural network parameters from SD card + - Minimal flash usage (for possible inclusion into default build configurations) + - Easy development: Train new neural network and just upload it via MAVLink FTP without requiring to re-flash the firmware +- Tested on 10+ different real platforms (including flexible frames, brushed motors) +- Actively developed and maintained + +## Usage + +### SITL + +Build PX4 SITL with Raptor, disable QGC requirement, and adjust the `IMU_GYRO_RATEMAX` to match the simulation IMU rate + +```sh +make px4_sitl_raptor gz_x500 +param set NAV_DLL_ACT 0 +param set COM_DISARM_LAND -1 # When taking off in offboard the landing detector can cause mid-air disarms +param set IMU_GYRO_RATEMAX 250 # Just for SITL. Tested with IMU_GYRO_RATEMAX=400 on real FCUs +param set MC_RAPTOR_ENABLE 1 # Enable the mc_raptor module +param save +``` + +Upload the RAPTOR checkpoint to the "SD card": Separate terminal + +```bash +mavproxy.py --master udp:127.0.0.1:14540 +ftp mkdir /raptor # for the real FMU use: /fs/microsd/raptor +ftp put src/modules/mc_raptor/blob/policy.tar /raptor/policy.tar +``` + +Restart (Ctrl+C) + +```sh +make px4_sitl_raptor gz_x500 +commander takeoff +commander status +``` + +Note the external mode ID of `RAPTOR` in the status report + +```sh +commander mode ext{RAPTOR_MODE_ID} +``` + +#### Internal Reference Trajectory Generation + +In our experience, feeding the `trajectory_setpoint` via MAVLink (even via WiFi telemetry) is unreliable. +But we do not want to constrain this module to only platforms that have a companion board. + +For this reason we have integrated a simple internal reference trajectory generator for testing and benchmarking purposes. +It supports position (constant position and yaw setpoint) as well as configurable [Lissajous trajectories](https://en.wikipedia.org/wiki/Lissajous_curve). + +The Lissajous generator can, for example, generate smooth figure-eight trajectories that contain interesting accelerations for benchmarking and testing purposes. +Please refer to the embedded configurator later in this section to explore the Lissajous parameters and view the resulting trajectories. + +To use the internal reference generator, select the mode: `0`: Off/activation position tracking, `1`: Lissajous + +```sh +param set MC_RAPTOR_INTREF 1 +``` + +Restart (ctrl+c) + +```sh +commander takeoff +commander mode ext{RAPTOR_MODE_ID} +mc_raptor intref lissajous 0.5 1 0 2 1 1 10 3 +``` + +The trajectory is relative to the position and yaw of the vehicle at the point where the RAPTOR mode is activated (or the position and yaw where the parameters are changed if it is already activated). + +You can adjust the parameters of the trajectory with the following tool. +Make sure to copy the generated CLI string at the end: + + + +### Real-World + +#### Setup + +The `mc_raptor` module has been mostly tested with the Holybro X500 V2 but it should also work out-of-the-box with other platforms (see the [Other Platforms](#other-platforms) section). + +```sh +make px4_fmu-v6c_raptor upload +``` + +We recommend initially testing the RAPTOR mode using a dead man's switch. +For this we configure the mode selection to be connected to a push button or a switch with a spring that automatically switches back. +In the default position we configure e.g. `Stabilized Mode` and in the pressed configuration we select `External Mode 1` (since the name of the external mode is only transmitted at runtime). +This allows to take off manually and then just trigger the RAPTOR mode for a split-second to see how it behaves. +In our experiments it has been exceptionally stable (zero crashes) but we still think progressively activating it for longer is the safest way to build confidence. + +::: warning +Make sure that your platform uses the standard PX4 quadrotor motor layout: + +1: front-right, 2: back-left, 3: front-left, 4: back-right +::: + +##### Other Platforms + +To enable the `mc_raptor` module in other platforms, just add `CONFIG_MODULES_MC_RAPTOR=y` and `CONFIG_LIB_RL_TOOLS=y` + +```diff ++++ b/boards/px4/fmu-v6c/raptor.px4board +@@ -35,2 +35,3 @@ + CONFIG_DRIVERS_UAVCAN=y ++CONFIG_LIB_RL_TOOLS=y + CONFIG_MODULES_AIRSPEED_SELECTOR=y +@@ -64,2 +65,3 @@ + CONFIG_MODULES_MC_POS_CONTROL=y ++CONFIG_MODULES_MC_RAPTOR=y + CONFIG_MODULES_MC_RATE_CONTROL=y +``` + +#### Results + +Even though there were moderate winds (~ 5 m/s) during the test, we found good figure-eight tracking performance at velocities up to 12 m/s: + +![Lissajous](../../assets/advanced/neural_networks/raptor/results_figure_eight.svg) + +We also tested the linear velocity in a straight line and found that the RAPTOR policy can reliably fly at > 17 m/s (the wind direction was orthogonal to the line): + +![Linear Oscillation](../../assets/advanced/neural_networks/raptor/results_line.svg) + +### Troubleshooting + +#### Logging + +Use this logging configuration to log all relevant topics at maximum rate: + +```sh +cat > logger_topics.txt << EOF +raptor_status 0 +raptor_input 0 +trajectory_setpoint 0 +vehicle_local_position 0 +vehicle_angular_velocity 0 +vehicle_attitude 0 +vehicle_status 0 +actuator_motors 0 +EOF +``` + +Use mavproxy FTP to upload it: + +```sh +mavproxy.py +``` + +##### Real + +```sh +ftp mkdir /fs/microsd/etc +ftp mkdir /fs/microsd/etc/logging +ftp put logger_topics.txt /fs/microsd/etc/logging/logger_topics.txt +``` + +##### SITL + +```sh +ftp mkdir etc +ftp mkdir logging +ftp put logger_topics.txt etc/logging/logger_topics.txt +``` diff --git a/docs/en/advanced/tflm.md b/docs/en/neural_networks/tflm.md similarity index 90% rename from docs/en/advanced/tflm.md rename to docs/en/neural_networks/tflm.md index 2c30ac5216..57b2847867 100644 --- a/docs/en/advanced/tflm.md +++ b/docs/en/neural_networks/tflm.md @@ -1,6 +1,6 @@ # TensorFlow Lite Micro (TFLM) -The PX4 [Multicopter Neural Network](../advanced/neural_networks.md) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) integrates a neural network that uses the [TensorFlow Lite Micro (TFLM)](https://github.com/tensorflow/tflite-micro) inference library. +The PX4 [MC Neural Networks Control](../neural_networks/mc_neural_network_control.md) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) integrates a neural network that uses the [TensorFlow Lite Micro (TFLM)](https://github.com/tensorflow/tflite-micro) inference library. This is a mature inference library intended for use on embedded devices, and is hence a suitable choice for PX4. @@ -68,7 +68,7 @@ The `_input_tensor` is also defined, it is fetched from `_control_interpreter->i The `_input_tensor` is filled in the `PopulateInputTensor()` function. `_input_tensor` works by accessing the `->data.f` member array and fill in the required inputs for your network. -The inputs used in the control network is covered in [Neural Networks](../advanced/neural_networks.md). +The inputs used in the control network is covered in [MC Neural Networks Control](../neural_networks/mc_neural_network_control.md). ### Outputs diff --git a/docs/en/peripherals/esc_motors.md b/docs/en/peripherals/esc_motors.md index 77a02eba5b..7fddeb999d 100644 --- a/docs/en/peripherals/esc_motors.md +++ b/docs/en/peripherals/esc_motors.md @@ -30,7 +30,7 @@ The following list is non-exhaustive. [PWM]: ../peripherals/pwm_escs_and_servo.md [Holybro Kotleta 20]: ../dronecan/holybro_kotleta.md [Vertiq Motor & ESC modules]: ../peripherals/vertiq.md -[RaccoonLab CAN PWM nodes]: ../dronecan/raccoonlab_nodes.md +[RaccoonLab CAN PWM ESC nodes]: ../dronecan/raccoonlab_nodes.md [Zubax Telega]: ../dronecan/zubax_telega.md ## See Also diff --git a/docs/en/releases/1.16.md b/docs/en/releases/1.16.md index 0fac823996..42019f52d1 100644 --- a/docs/en/releases/1.16.md +++ b/docs/en/releases/1.16.md @@ -129,6 +129,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide). ### uXRCE-DDS / ROS2 - [PX4-Autopilot#24113](https://github.com/PX4/PX4-Autopilot/pull/24113): [ROS 2 Message Translation Node](../ros2/px4_ros2_msg_translation_node.md) to translate PX4 messages from one definition version to another dynamically +- [PX4 ROS 2 Interface Library](../ros2/px4_ros2_control_interface.md) support for [ROS-based waypoint missions](../ros2/px4_ros2_waypoint_missions.md). - dds_topics: add vtol_vehicle_status ([PX4-Autopilot#24582](https://github.com/PX4/PX4-Autopilot/pull/24582)) - dds_topics: add home_position ([PX4-Autopilot#24583](https://github.com/PX4/PX4-Autopilot/pull/24583)) @@ -138,6 +139,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide). - Parameter to always start mavlink stream via USB. ([PX4-Autopilot#22234](https://github.com/PX4/PX4-Autopilot/pull/22234)) - Refactor: MAVLink message handling in one function, reference instead of pointer to main instance ([PX4-Autopilo#23219](https://github.com/PX4/PX4-Autopilot/pull/22234)) - mavlink log handler rewrite for improved effeciency ([PX4-Autopilo#23219](https://github.com/PX4/PX4-Autopilot/pull/22234)) + ### Multi-Rotor - [Multirotor] add yaw torque low pass filter ([PX4-Autopilot#24173](https://github.com/PX4/PX4-Autopilot/pull/24173)) diff --git a/docs/en/releases/1.17.md b/docs/en/releases/1.17.md new file mode 100644 index 0000000000..67728cca98 --- /dev/null +++ b/docs/en/releases/1.17.md @@ -0,0 +1,134 @@ +# PX4-Autopilot v1.17.0 Release Notes + + + + + +
+
+

This page is on a release branch, and hence probably out of date. See the latest version.

+
+
+ +This contains changes to PX4 planned for PX4 v1.17 (since the last major release [PX v1.16](../releases/1.16.md)). + +::: warning +PX4 v1.17 is in alpha/beta testing. +Update these notes with features that are going to be in PX4 v1.17 release. +New features that are not expected to go into the v1.17 release are in [PX4-Autopilot `main` Release Notes](../releases/main.md). +::: + +## Read Before Upgrading + +TBD … + +Please continue reading for [upgrade instructions](#upgrade-guide). + +## Major Changes + +- TBD + +## Upgrade Guide + +## Other changes + +### Hardware Support + +- **[New Hardware]** boards: [MicoAir743-Lite FC](../flight_controller/micoair743-lite.md) +- **[New Hardware]** boards: [RadiolinkPIX6 FC](../flight_controller/radiolink_pix6.md) +- **[New Hardware]** boards: [AP-H743-R1 FC](../flight_controller/x-mav_ap-h743r1.md) + + + +### Control + + + +- [MC Neural Network Module](../advanced/neural_networkss.md) + +### Estimation + +- TBD + + + +### Simulation + +- Overhaul rover simulation: + - Add synthetic differential rover model: [PX4-gazebo-models#107](https://github.com/PX4/PX4-gazebo-models/pull/107) + - Add synthetic mecanum rover model: [PX4-gazebo-models#113](https://github.com/PX4/PX4-gazebo-models/pull/113) + - Update synthetic ackermann rover model: [PX4-gazebo-models#117](https://github.com/PX4/PX4-gazebo-models/pull/117) + +- [Simulation-in-Hardware (SIH)](../sim_sih/index.md#compatibility) + - New simulation: MC Hexacopter X + - New simulation: Ackermann Rover + +### Debug & Logging + +- TBD + +### Ethernet + +- TBD + +### uXRCE-DDS / Zenoh / ROS2 + +- [PX4 ROS 2 Interface Library](../ros2/px4_ros2_control_interface.md) support for [Fixed Wing lateral/longitudinal setpoint](../ros2/px4_ros2_control_interface.md#fixed-wing-lateral-and-longitudinal-setpoint-fwlaterallongitudinalsetpointtype) (`FwLateralLongitudinalSetpointType`) and [VTOL transitions](../ros2/px4_ros2_control_interface.md#controlling-a-vtol). ([PX4-Autopilot#24056](https://github.com/PX4/PX4-Autopilot/pull/24056)). +- [UXRCE_DDS: Simple index based namespace (UXRCE_DDS_NS_IDX)](../middleware/uxrce_dds.md#customizing-the-namespace) +- [Zenoh (PX4 ROS 2 rmw_zenoh)](../middleware/zenoh.md) + +### MAVLink + +- TBD + + + +### VTOL + +- TBD + +### Fixed-wing + +- [Fixed Wing Takeoff mode](../flight_modes_fw/takeoff.md) will now keep climbing with level wings on position loss. + A target takeoff waypoint can be set to control takeoff course and loiter altitude. ([PX4-Autopilot#25083](https://github.com/PX4/PX4-Autopilot/pull/25083)). +- Automatically suppress angular rate oscillations using [Gain compression](../features_fw/gain_compression.md). ([PX4-Autopilot#25840: FW rate control: add gain compression algorithm](https://github.com/PX4/PX4-Autopilot/pull/25840)) + +### Rover + +- Removed deprecated rover module ([PX4-Autopilot#25054](https://github.com/PX4/PX4-Autopilot/pull/25054)). +- Add support for [Apps & API](../flight_modes_rover/api.md) including [Rover Setpoints](../ros2/px4_ros2_control_interface.md#rover-setpoints) ([PX4-Autopilot#25074](https://github.com/PX4/PX4-Autopilot/pull/25074), [PX4-ROS2-Interface-Lib#140](https://github.com/Auterion/px4-ros2-interface-lib/pull/140)). +- Update [rover simulation](../frames_rover/index.md#simulation) ([PX4-Autopilot#25644](https://github.com/PX4/PX4-Autopilot/pull/25644)) (see [Simulation](#simulation) release note for details). + +### ROS 2 + +- TBD diff --git a/docs/en/releases/index.md b/docs/en/releases/index.md index 7c5aa295c5..69986e1ea4 100644 --- a/docs/en/releases/index.md +++ b/docs/en/releases/index.md @@ -2,7 +2,8 @@ A list of PX4 release notes, they contain a list of the changes that went into each release, explaining the included features, bug fixes, deprecations and updates in detail. -- [main](../releases/main.md) (changes since v1.16) +- [main](../releases/main.md) (changes planned for v1.18 or later) +- [v1.17](../releases/1.17.md) (changes planned for v1.17, since v1.16) - [v1.16](../releases/1.16.md) - [v1.15](../releases/1.15.md) - [v1.14](../releases/1.14.md) diff --git a/docs/en/releases/main.md b/docs/en/releases/main.md index 5c0d07a31a..acee02d522 100644 --- a/docs/en/releases/main.md +++ b/docs/en/releases/main.md @@ -16,13 +16,13 @@ const { site } = useData(); This contains changes to PX4 `main` branch since the last major release ([PX v1.16](../releases/1.16.md)). ::: warning -PX4 v1.16 is in candidate-release testing, pending release. -Update these notes with features that are going to be in `main` but not the PX4 v1.16 release. +PX4 v1.17 is in alpha/beta testing. +Update these notes with features that are going to be in `main` (PX4 v1.18 or later) but not the PX4 v1.17 release. ::: ## Read Before Upgrading -TBD … +- TBD … Please continue reading for [upgrade instructions](#upgrade-guide). @@ -45,8 +45,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide). ### Control - Added new flight mode(s): [Altitude Cruise (MC)](../flight_modes_mc/altitude_cruise.md), Altitude Cruise (FW). - For fixed-wing the mode behaves the same as Altitude mode but you can disable the manual control loss failsafe. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise - ](https://github.com/PX4/PX4-Autopilot/pull/25435)). + For fixed-wing the mode behaves the same as Altitude mode but you can disable the manual control loss failsafe. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)). ### Estimation @@ -59,19 +58,34 @@ Please continue reading for [upgrade instructions](#upgrade-guide). ### Simulation +- TBD + + + +### Debug & Logging + +- [Asset Tracking](../debug/asset_tracking.md): Automatic tracking and logging of external device information including vendor name, firmware and hardware version, serial numbers. Currently supports DroneCAN devices. ([PX4-Autopilot#25617](https://github.com/PX4/PX4-Autopilot/pull/25617)) + ### Ethernet - TBD -### uXRCE-DDS / ROS2 +### uXRCE-DDS / Zenoh / ROS2 + +- TBD + + ### MAVLink @@ -92,16 +106,26 @@ Please continue reading for [upgrade instructions](#upgrade-guide). ### Fixed-wing +- TBD + + ### Rover +- TBD + + + ### ROS 2 - TBD diff --git a/docs/en/ros2/px4_ros2_control_interface.md b/docs/en/ros2/px4_ros2_control_interface.md index ebb54be960..81f5943882 100644 --- a/docs/en/ros2/px4_ros2_control_interface.md +++ b/docs/en/ros2/px4_ros2_control_interface.md @@ -341,9 +341,9 @@ The used types also define the compatibility with different vehicle types. The following sections provide a list of supported setpoint types: - [MulticopterGotoSetpointType](#go-to-setpoint-multicoptergotosetpointtype): Smooth position and (optionally) heading control -- [FwLateralLongitudinalSetpointType](#fixed-wing-lateral-and-longitudinal-setpoint-fwlaterallongitudinalsetpointtype): Direct control of lateral and longitudinal fixed wing dynamics +- [FwLateralLongitudinalSetpointType](#fixed-wing-lateral-and-longitudinal-setpoint-fwlaterallongitudinalsetpointtype): Direct control of lateral and longitudinal fixed wing dynamics - [DirectActuatorsSetpointType](#direct-actuator-control-setpoint-directactuatorssetpointtype): Direct control of motors and flight surface servo setpoints -- [Rover Setpoints](#rover-setpoints): Direct access to rover control setpoints (Position, Speed, Attitude, Rate, Throttle and Steering). +- [Rover Setpoints](#rover-setpoints): Direct access to rover control setpoints (Position, Speed, Attitude, Rate, Throttle and Steering). :::tip The other setpoint types are currently experimental, and can be found in: [px4_ros2/control/setpoint_types/experimental](https://github.com/Auterion/px4-ros2-interface-lib/tree/main/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental). @@ -410,7 +410,7 @@ _goto_setpoint->update( #### Fixed-Wing Lateral and Longitudinal Setpoint (FwLateralLongitudinalSetpointType) - + ::: info This setpoint type is supported for fixed-wing vehicles and for VTOLs in fixed-wing mode. @@ -552,7 +552,7 @@ If you want to control an actuator that does not control the vehicle's motion, b #### Rover Setpoints - + The rover modules use a hierarchical structure to propagate setpoints: @@ -586,7 +586,7 @@ An example for a rover specific drive mode using the `RoverSpeedAttitudeSetpoint ### Controlling a VTOL - + To control a VTOL in an external flight mode, ensure you're returning the correct setpoint type based on the current flight configuration: diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md index c134b8aca8..07767e72ef 100644 --- a/docs/en/sim_sih/index.md +++ b/docs/en/sim_sih/index.md @@ -27,8 +27,8 @@ The Desktop computer is only used to display the virtual vehicle. - SIH for FW (airplane) and VTOL tailsitter are supported from PX4 v1.13. - SIH as SITL (without hardware) from PX4 v1.14. - SIH for Standard VTOL from PX4 v1.16. -- SIH for MC Hexacopter X from `main` (expected to be PX4 v1.17). -- SIH for Ackermann Rover from `main`. +- SIH for MC Hexacopter X from PX4 v1.17. +- SIH for Ackermann Rover from PX4 v1.17. ### Benefits @@ -339,6 +339,7 @@ You can find a full list of available values for `PWM_MAIN_FUNCn` [here](../adva Alternatively, you can use the [`PWM_AUX_FUNCn`](../advanced_config/parameter_reference.md#PWM_AUX_FUNC1) parameters. You may also configure the output as desired: + - Disarmed PWM: ([`PWM_MAIN_DISn`](../advanced_config/parameter_reference.md#PWM_MAIN_DIS1) / [`PWM_AUX_DIS1`](../advanced_config/parameter_reference.md#PWM_AUX_DIS1)) - Minimum PWM ([`PWM_MAIN_MINn`](../advanced_config/parameter_reference.md#PWM_MAIN_MIN1) / [`PWM_AUX_MINn`](../advanced_config/parameter_reference.md#PWM_AUX_MIN1)) - Maximum PWM ([`PWM_MAIN_MAXn`](../advanced_config/parameter_reference.md#PWM_MAIN_MAX1) / [`PWM_AUX_MAXn`](../advanced_config/parameter_reference.md#PWM_AUX_MAX1)) diff --git a/docs/ko/SUMMARY.md b/docs/ko/SUMMARY.md index 468fe2206c..300979c54d 100644 --- a/docs/ko/SUMMARY.md +++ b/docs/ko/SUMMARY.md @@ -315,15 +315,17 @@ - [ADSB/FLARM (트래픽 회피)](config/actuators.md) - [ESC 보정](advanced_config/esc_calibration.md) - [ESC와 모터](peripherals/esc_motors.md) + - [ESC Protocols](esc/esc_protocols.md) - [PWM ESC와 서보](peripherals/pwm_escs_and_servo.md) - [DShot ESCs](peripherals/dshot.md) - [OneShot ESCs and Servos](peripherals/oneshot.md) - [DroneCAN ESCs](dronecan/escs.md) - - [Zubax Telega](dronecan/zubax_telega.md) - [PX4 Sapog ESC Firmware](dronecan/sapog.md) - - [Holybro Kotleta](dronecan/holybro_kotleta.md) - - [Vertiq](peripherals/vertiq.md) - - [VESC](peripherals/vesc.md) + - [ARK 4IN1 ESC](esc/ark_4in1_esc.md) + - [Holybro Kotleta](dronecan/holybro_kotleta.md) + - [Vertiq Motor/ESC Modules](peripherals/vertiq.md) + - [VESC Project ESCs](peripherals/vesc.md) + - [Zubax Telega ESCs](dronecan/zubax_telega.md) - [Radio Control (RC)](getting_started/rc_transmitter_receiver.md) - [무선 조종기 설정](config/radio.md) @@ -519,6 +521,7 @@ - [PPS Time Synchronization](advanced/pps_time_sync.md) - [미들웨어](middleware/index.md) - [uORB 메시지 전송](middleware/uorb.md) + - [uORB Docs Standard](uorb/uorb_documentation.md) - [uORB 그라프](middleware/uorb_graph.md) - [uORB Message Reference](msg_docs/index.md) - [Versioned](msg_docs/versioned_messages.md) @@ -581,6 +584,7 @@ - [DebugKeyValue](msg_docs/DebugKeyValue.md) - [DebugValue](msg_docs/DebugValue.md) - [DebugVect](msg_docs/DebugVect.md) + - [DeviceInformation](msg_docs/DeviceInformation.md) - [DifferentialPressure](msg_docs/DifferentialPressure.md) - [DistanceSensor](msg_docs/DistanceSensor.md) - [DistanceSensorModeChangeRequest](msg_docs/DistanceSensorModeChangeRequest.md) diff --git a/docs/ko/advanced_config/ethernet_setup.md b/docs/ko/advanced_config/ethernet_setup.md index ab0a88e48b..0b92e9d940 100644 --- a/docs/ko/advanced_config/ethernet_setup.md +++ b/docs/ko/advanced_config/ethernet_setup.md @@ -25,6 +25,7 @@ It may also be supported on other boards. Supported flight controllers include: +- [ARK Electronics ARKV6X](../flight_controller/ark_v6x.md) - [CUAV Pixhawk V6X](../flight_controller/cuav_pixhawk_v6x.md) - [Holybro Pixhawk 5X](../flight_controller/pixhawk5x.md) - [Holybro Pixhawk 6X](../flight_controller/pixhawk6x.md) diff --git a/docs/ko/assembly/_assembly.md b/docs/ko/assembly/_assembly.md index 496741776c..555111defd 100644 --- a/docs/ko/assembly/_assembly.md +++ b/docs/ko/assembly/_assembly.md @@ -285,7 +285,7 @@ A particular vehicle might have more/fewer motors and actuators, but the wiring The following sections explain each part in more detail. :::tip -If you're using [DroneCAN ESC](../peripherals/esc_motors.md#dronecan) the control signals will be connected to the CAN BUS instead of the PWM outputs as shown. +If you're using [DroneCAN ESC](../dronecan/escs.md) the control signals will be connected to the CAN BUS instead of the PWM outputs as shown. ::: ### Flight Controller Power @@ -426,7 +426,6 @@ They recommend sensors, power systems, and other components from the same manufa - [Drone Components & Parts](../getting_started/px4_basic_concepts.md#drone-components-parts) (Basic Concepts) - [Payloads](../getting_started/px4_basic_concepts.md#payloads) (Basic Concepts) - [Hardware Selection & Setup](../hardware/drone_parts.md) — information about connecting and configuring specific flight controllers, sensors and other peripherals (e.g. airspeed sensor for planes). - - [Mounting the Flight Controller](../assembly/mount_and_orient_controller.md) - [Vibration Isolation](../assembly/vibration_isolation.md) - [Mounting a Compass](../assembly/mount_gps_compass.md) diff --git a/docs/ko/can/index.md b/docs/ko/can/index.md index 137f639189..ee516e9c6c 100644 --- a/docs/ko/can/index.md +++ b/docs/ko/can/index.md @@ -1,7 +1,13 @@ -# CAN +# CAN (DroneCAN & Cyphal) [Controller Area Network (CAN)](https://en.wikipedia.org/wiki/CAN_bus) is a robust wired network that allows drone components such as flight controller, ESCs, sensors, and other peripherals, to communicate with each other. -Because it is designed to be democratic and uses differential signaling, it is very robust even over longer cable lengths (on large vehicles), and avoids a single point of failure. + +It is particularly recommended on larger vehicles. + +## 개요 + +CAN it is designed to be democratic and uses differential signaling. +For this reason it is very robust even over longer cable lengths (on large vehicles), and avoids a single point of failure. CAN also allows status feedback from peripherals and convenient firmware upgrades over the bus. PX4 supports two software protocols for communicating with CAN devices: @@ -18,29 +24,36 @@ In 2022 the project split into two: the original version of UAVCAN (UAVCAN v0) w The differences between the two protocols are outlined in [Cyphal vs. DroneCAN](https://forum.opencyphal.org/t/cyphal-vs-dronecan/1814). ::: -:::warning PX4 does not support other CAN software protocols for drones such as KDECAN (at time of writing). -::: ## 배선 The wiring for CAN networks is the same for both DroneCAN and Cyphal/CAN (in fact, for all CAN networks). -Devices are connected in a chain in any order. +Devices within a network are connected in a _daisy-chain_ in any order (this differs from UARTs peripherals, where you attach just one component per port). + +:::warning +Don't connect each CAN peripheral to a separate CAN port! +Unlike UARTs, CAN peripherals are designed to be daisy chained, with additional ports such as `CAN2` used for [redundancy](redundancy). +::: + At either end of the chain, a 120Ω termination resistor should be connected between the two data lines. Flight controllers and some GNSS modules have built in termination resistors for convenience, thus should be placed at opposite ends of the chain. Otherwise, you can use a termination resistor such as [this one from Zubax Robotics](https://shop.zubax.com/products/uavcan-micro-termination-plug?variant=6007985111069), or solder one yourself if you have access to a JST-GH crimper. The following diagram shows an example of a CAN bus connecting a flight controller to 4 CAN ESCs and a GNSS. +It includes a redundant bus connected to `CAN 2`. ![CAN Wiring](../../assets/can/uavcan_wiring.svg) The diagram does not show any power wiring. Refer to your manufacturer instructions to confirm whether components require separate power or can be powered from the CAN bus itself. +:::info For more information, see [Cyphal/CAN device interconnection](https://wiki.zubax.com/public/cyphal/CyphalCAN-device-interconnection?pageId=2195476) (kb.zubax.com). While the article is written with the Cyphal protocol in mind, it applies equally to DroneCAN hardware and any other CAN setup. For more advanced scenarios, consult with [On CAN bus topology and termination](https://forum.opencyphal.org/t/on-can-bus-topology-and-termination/1685). +::: ### 커넥터 @@ -54,7 +67,30 @@ However, as long as the device firmware supports DroneCAN or Cyphal, it can be u DroneCAN and Cyphal/CAN support using a second (redundant) CAN interface. This is completely optional but increases the robustness of the connection. -All Pixhawk flight controllers come with 2 CAN interfaces; if your peripherals support 2 CAN interfaces as well, it is recommended to wire both up for increased safety. + +Pixhawk flight controllers come with 2 CAN interfaces; if your peripherals support 2 CAN interfaces as well, it is recommended to wire both up for increased safety. + +### Flight Controllers with Multiple CAN Ports + +[Flight Controllers](../flight_controller/index.md) may have up to three independent CAN ports, such as `CAN1`, `CAN2`, `CAN3` (neither DroneCAN or Cyphal support more than three). +Note that you can't have both DroneCAN and Cyphal running on PX4 at the same time. + +:::tip +You only _need_ one CAN port to support an arbitrary number of CAN devices using a particular CAN protocol. +Don't connect each CAN peripheral to a separate CAN port! +::: + +Generally you'll daisy all CAN peripherals off a single port, and if there is more than one CAN port, use the second one for [redundancy](redundancy). +If three are three ports, you might use the remaining network for devices that support another CAN protocol. + +The documentation for your flight controller should indicate which ports are supported/enabled. +At runtime you can check what DroneCAN ports are enabled and their status using the following command on the [MAVLink Shell](../debug/mavlink_shell.md) (or some other console): + +```sh +uavcan status +``` + +Note that you can also check the number of supported CAN interfaces for a board by searching for `CONFIG_BOARD_UAVCAN_INTERFACES` in its [default.px4board](https://github.com/PX4/PX4-Autopilot/blob/main/boards/px4/fmu-v6xrt/default.px4board#) configuration file. ## 펌웨어 diff --git a/docs/ko/config_mc/filter_tuning.md b/docs/ko/config_mc/filter_tuning.md index 665ca9dc33..2a7e2cd406 100644 --- a/docs/ko/config_mc/filter_tuning.md +++ b/docs/ko/config_mc/filter_tuning.md @@ -70,7 +70,7 @@ Airframes with more than two frequency noise spikes typically clean the first tw Dynamic notch filters use ESC RPM feedback and/or the onboard FFT analysis. The ESC RPM feedback is used to track the rotor blade pass frequency and its harmonics, while the FFT analysis can be used to track a frequency of another vibration source, such as a fuel engine. -ESC RPM feedback requires ESCs capable of providing RPM feedback such as [DShot](../peripherals/esc_motors.md#dshot) with telemetry connected, a bidirectional DShot set up ([work in progress](https://github.com/PX4/PX4-Autopilot/pull/23863)), or [UAVCAN/DroneCAN ESCs](../dronecan/escs.md). +ESC RPM feedback requires ESCs capable of providing RPM feedback such as [DShot](../peripherals/dshot.md) with telemetry connected, a bidirectional DShot set up ([work in progress](https://github.com/PX4/PX4-Autopilot/pull/23863)), or [UAVCAN/DroneCAN ESCs](../dronecan/escs.md). Before enabling, make sure that the ESC RPM is correct. You might have to adjust the [pole count of the motors](../advanced_config/parameter_reference.md#MOT_POLE_COUNT). diff --git a/docs/ko/dronecan/ark_flow.md b/docs/ko/dronecan/ark_flow.md index 17766bbdb6..8ce7da3a11 100644 --- a/docs/ko/dronecan/ark_flow.md +++ b/docs/ko/dronecan/ark_flow.md @@ -94,6 +94,7 @@ Set the following parameters in _QGroundControl_: - To optionally disable GPS aiding, set [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to `0`. - Enable [UAVCAN_SUB_FLOW](../advanced_config/parameter_reference.md#UAVCAN_SUB_FLOW). - Enable [UAVCAN_SUB_RNG](../advanced_config/parameter_reference.md#UAVCAN_SUB_RNG). +- Set [EKF2_RNG_CTRL](../advanced_config/parameter_reference.md#EKF2_RNG_CTRL) to `1`. - Set [EKF2_RNG_A_HMAX](../advanced_config/parameter_reference.md#EKF2_RNG_A_HMAX) to `10`. - Set [EKF2_RNG_QLTY_T](../advanced_config/parameter_reference.md#EKF2_RNG_QLTY_T) to `0.2`. - Set [UAVCAN_RNG_MIN](../advanced_config/parameter_reference.md#UAVCAN_RNG_MIN) to `0.08`. diff --git a/docs/ko/dronecan/ark_flow_mr.md b/docs/ko/dronecan/ark_flow_mr.md index c9e0bbced3..e949b4cfa3 100644 --- a/docs/ko/dronecan/ark_flow_mr.md +++ b/docs/ko/dronecan/ark_flow_mr.md @@ -91,6 +91,7 @@ Set the following parameters in _QGroundControl_: - To optionally disable GPS aiding, set [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to `0`. - Enable [UAVCAN_SUB_FLOW](../advanced_config/parameter_reference.md#UAVCAN_SUB_FLOW). - Enable [UAVCAN_SUB_RNG](../advanced_config/parameter_reference.md#UAVCAN_SUB_RNG). +- Set [EKF2_RNG_CTRL](../advanced_config/parameter_reference.md#EKF2_RNG_CTRL) to `1`. - Set [EKF2_RNG_A_HMAX](../advanced_config/parameter_reference.md#EKF2_RNG_A_HMAX) to `10`. - Set [EKF2_RNG_QLTY_T](../advanced_config/parameter_reference.md#EKF2_RNG_QLTY_T) to `0.2`. - Set [UAVCAN_RNG_MIN](../advanced_config/parameter_reference.md#UAVCAN_RNG_MIN) to `0.08`. diff --git a/docs/ko/dronecan/escs.md b/docs/ko/dronecan/escs.md index 4d97c7cfd1..6afea8c2e2 100644 --- a/docs/ko/dronecan/escs.md +++ b/docs/ko/dronecan/escs.md @@ -1,7 +1,14 @@ # DroneCAN ESCs PX4 supports DroneCAN compliant ESCs. -For more information, see the following articles for specific hardware/firmware: + +## Supported ESC + +:::info +[Supported ESCs](../peripherals/esc_motors#supported-esc) in _ESCs & Motors_ may include additional devices that are not listed below. +::: + +The following articles have specific hardware/firmware information: - [PX4 Sapog ESC Firmware](sapog.md) - [Holybro Kotleta 20](holybro_kotleta.md) diff --git a/docs/ko/esc/ark_4in1_esc.md b/docs/ko/esc/ark_4in1_esc.md new file mode 100644 index 0000000000..e558bb5668 --- /dev/null +++ b/docs/ko/esc/ark_4in1_esc.md @@ -0,0 +1,65 @@ +# ARK 4IN1 ESC (with/without Connectors) + +4 in 1 Electronic Speed Controller (ESC) that is made in the USA, NDAA compliant, and DIU Blue Framework listed. + +The ESC comes in variants without connectors that you can solder in place, and a variant that has built-in motor and battery connectors (no soldering required). + +![ARK 4IN1 ESC without connectors ](../../assets/hardware/esc/ark/ark_4_in_1_esc.jpg)![ARK 4IN1 ESC with connectors](../../assets/hardware/esc/ark/ark_4_in_1_esc_with_connectors.jpg) + +## 구매처 + +Order this module from: + +- [4IN1 ESC (with connectors)](https://arkelectron.com/product/ark-4in1-esc/) (ARK Electronics - US) +- [ARK Electronics (without connectors)](https://arkelectron.com/product/ark-4in1-esc-cons/) (ARK Electronics US) + +## Hardware Specifications + +- Battery Voltage: 3-8s + - 6V Minimum + - 65V Absolute Maximum + +- Current Rating: 50A Continuous, 75A Burst Per Motor + +- [STM32F0](https://www.st.com/en/microcontrollers-microprocessors/stm32f0-series.html) + +- [AM32 Firmware](https://github.com/am32-firmware/AM32/pull/27) + +- Onboard Current Sensor, Serial Telemetry + - 100V/A + +- Input Protocols + - DShot (300, 600) + - Bi-directional DShot + - KISS Serial Telemetry + - PWM + +- 8 Pin JST-SH Input/Output + +- 10 Pin JST-SH Debug + +- Motor & Battery Connectors (with-connector version) + + - MR30 Connector Limit Per Motor: 30A Continuous, 40A Burst + - Four MR30 Motor Connectors + +- Dimensions (with connectors) + + - Size: 77.00mm x 42.00mm x 9.43mm + - Mounting Pattern: 30.5mm + - Weight: 24g + +- Dimensions (without connectors) + - Size: 43.00mm x 40.50mm x 7.60mm + - Mounting Pattern: 30.5mm + - Weight: 14.5g + +Other + +- Made in the USA +- Open source AM32 firmware +- [DIU Blue Framework Listed](https://www.diu.mil/blue-uas/framework) + +## See Also + +- [ARK 4IN1 ESC CONS](https://docs.arkelectron.com/electronic-speed-controller/ark-4in1-esc) (ARK Docs) diff --git a/docs/ko/esc/esc_protocols.md b/docs/ko/esc/esc_protocols.md new file mode 100644 index 0000000000..930d436de6 --- /dev/null +++ b/docs/ko/esc/esc_protocols.md @@ -0,0 +1,66 @@ +# ESC Protocols + +This topic lists the main [Electronic Speed Controller (ESC)](../peripherals/esc_motors.md) protocols supported by PX4. + +## DShot + +[DShot](../peripherals/dshot.md) is a digital ESC protocol that is highly recommended for vehicles that can benefit from reduced latency, in particular racing multicopters, VTOL vehicles, and so on. + +It has reduced latency and is more robust than both [PWM](#pwm) and [OneShot](#oneshot-125). +In addition it does not require ESC calibration, telemetry is available from some ESCs, and you can reverse motor spin directions. + +PX4 configuration is done in the [Actuator Configuration](../config/actuators.md). +Selecting a higher rate DShot ESC in the UI results in lower latency, but lower rates are more robust (and hence more suitable for large aircraft with longer leads); some ESCs only support lower rates (see datasheets for information). + +Setup: + +- [ESC Wiring](../peripherals/pwm_escs_and_servo.md) (same as for PWM ESCs) +- [DShot](../peripherals/dshot.md) also contains information about how to send commands etc. + +## DroneCAN + +[DroneCAN ESCs](../dronecan/escs.md) are recommended when DroneCAN is the primary bus used for your vehicle. +The PX4 implementation is currently limited to update rates of 200 Hz. + +DroneCAN shares many similar benefits to [DShot](#dshot) including high data rates, robust connection over long leads, telemetry feedback, no need for calibration of the ESC itself. + +[DroneCAN ESCs](../dronecan/escs.md) are connected via the DroneCAN bus (setup and configuration are covered at that link). + +## PWM + +[PWM ESCs](../peripherals/pwm_escs_and_servo.md) are commonly used for fixed-wing vehicles and ground vehicles (vehicles that require a lower latency like multicopters typically use oneshot or dshot ESCs). + +PWM ESCs communicate using a periodic pulse, where the _width_ of the pulse indicates the desired speed. +The pulse width typically ranges between 1000 μs for zero power and 2000 μs for full power. +The periodic frame rate of the signal depends on the capability of the ESC, and commonly ranges between 50 Hz and 490 Hz (the theoretical maximum being 500 Hz for a very small "off" cycle). +A higher rate is better for ESCs, in particular where a rapid response to setpoint changes is needed. +For PWM servos 50 Hz is usually sufficient, and many don't support higher rates. + +![duty cycle for PWM](../../assets/peripherals/esc_pwm_duty_cycle.png) + +In addition to being a relatively slow protocol PWM ESCs require [calibration](../advanced_config/esc_calibration.md) because the pulse widths representing low and high values can vary significantly. +Unlike [DShot](#dshot) and [DroneCAN ESC](#dronecan) they do not have the ability to provide telemetry and feedback on ESC (or servo) state. + +Setup: + +- [ESC Wiring](../peripherals/pwm_escs_and_servo.md) +- [PX4 Configuration](../peripherals/pwm_escs_and_servo.md#px4-configuration) +- [ESC Calibration](../advanced_config/esc_calibration.md) + +## OneShot 125 + +[OneShot 125 ESCs](../peripherals/oneshot.md) are usually much faster than PWM ESCs, and hence more responsive and easier to tune. +They are preferred over PWM for multicopters (but not as much as [DShot ESCs](#dshot), which do not require calibration, and may provide telemetry feedback). +There are a number of variants of the OneShot protocol, which support different rates. +PX4 only supports OneShot 125. + +OneShot 125 is the same as PWM but uses pulse widths that are 8 times shorter (from 125 μs to 250 μs for zero to full power). +This allows OneShot 125 ESCs to have a much shorter duty cycle/higher rate. +For PWM the theoretical maximum is close to 500 Hz while for OneShot it approaches 4 kHz. +The actual supported rate depends on the ESC used. + +Setup: + +- [ESC Wiring](../peripherals/pwm_escs_and_servo.md) (same as for PWM ESCs) +- [PX4 Configuration](../peripherals/oneshot.md#px4-configuration) +- [ESC Calibration](../advanced_config/esc_calibration.md) diff --git a/docs/ko/middleware/uorb.md b/docs/ko/middleware/uorb.md index 5dab1582fa..1928f9acf5 100644 --- a/docs/ko/middleware/uorb.md +++ b/docs/ko/middleware/uorb.md @@ -280,6 +280,8 @@ For more information see: [Plotting uORB Topic Data in Real Time using PlotJuggl ## See Also +- [uORB Documentation Standard](../uorb/uorb_documentation.md) + - _PX4 uORB Explained_ Blog series - [Part 1](https://px4.io/px4-uorb-explained-part-1/) - [Part 2](https://px4.io/px4-uorb-explained-part-2/) diff --git a/docs/ko/modules/modules_driver.md b/docs/ko/modules/modules_driver.md index 44b77996b8..cbcd7b8448 100644 --- a/docs/ko/modules/modules_driver.md +++ b/docs/ko/modules/modules_driver.md @@ -15,38 +15,6 @@ - [Rpm Sensor](modules_driver_rpm_sensor.md) - [Transponder](modules_driver_transponder.md) -## MCP23009 - -Source: [drivers/gpio/mcp23009](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/gpio/mcp23009) - -### Usage {#MCP23009_usage} - -``` -MCP23009 [arguments...] - Commands: - start - [-I] Internal I2C bus(es) - [-X] External I2C bus(es) - [-b ] board-specific bus (default=all) (external SPI: n-th bus - (default=1)) - [-f ] bus frequency in kHz - [-q] quiet startup (no message if no device found) - [-a ] I2C address - default: 37 - [-D ] Direction - default: 0 - [-O ] Output - default: 0 - [-P ] Pullups - default: 0 - [-U ] Update Interval [ms] - default: 0 - - stop - - status print status info -``` - ## atxxxx Source: [drivers/osd/atxxxx](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/osd/atxxxx) @@ -749,6 +717,40 @@ lsm303agr [arguments...] status print status info ``` +## mcp230xx + +Source: [lib/drivers/mcp_common](https://github.com/PX4/PX4-Autopilot/tree/main/src/lib/drivers/mcp_common) + +### Usage {#mcp230xx_usage} + +``` +mcp230xx [arguments...] + Commands: + start + [-I] Internal I2C bus(es) + [-X] External I2C bus(es) + [-b ] board-specific bus (default=all) (external SPI: n-th bus + (default=1)) + [-f ] bus frequency in kHz + [-q] quiet startup (no message if no device found) + [-a ] I2C address + default: 39 + [-D ] Direction (1=Input, 0=Output) + default: 0 + [-O ] Output + default: 0 + [-P ] Pullups + default: 0 + [-U ] Update Interval [ms] + default: 0 + [-M ] First minor number + default: 0 + + stop + + status print status info +``` + ## mcp9808 Source: [drivers/temperature_sensor/mcp9808](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/temperature_sensor/mcp9808) @@ -899,8 +901,6 @@ fetching the latest mixing result and write them to PCA9685 at its scheduling ti It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width that can be accepted by most ESCs and servos. -The I2C bus and address can be configured via parameters `PCA9685_EN_BUS` and `PCA9685_I2C_ADDR`, or via command line arguments. - ### 예 It is typically started with: diff --git a/docs/ko/modules/modules_system.md b/docs/ko/modules/modules_system.md index 66b946220f..8be479bba8 100644 --- a/docs/ko/modules/modules_system.md +++ b/docs/ko/modules/modules_system.md @@ -127,6 +127,10 @@ commander [arguments...] check Run preflight checks + safety Change prearm safety state + on|off [on] to activate safety, [off] to deactivate safety and allow + control surface movements + arm [-f] Force arming (do not run preflight checks) diff --git a/docs/ko/msg_docs/BatteryStatus.md b/docs/ko/msg_docs/BatteryStatus.md index addd1ae524..ec2c19c59e 100644 --- a/docs/ko/msg_docs/BatteryStatus.md +++ b/docs/ko/msg_docs/BatteryStatus.md @@ -2,7 +2,7 @@ Battery status -Battery status information for up to 4 battery instances. +Battery status information for up to 3 battery instances. These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. Battery instance information is also logged and streamed in MAVLink telemetry. @@ -11,7 +11,7 @@ Battery instance information is also logged and streamed in MAVLink telemetry. ```c # Battery status # -# Battery status information for up to 4 battery instances. +# Battery status information for up to 3 battery instances. # These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. # Battery instance information is also logged and streamed in MAVLink telemetry. @@ -33,9 +33,9 @@ uint8 cell_count # [-] [@invalid 0] Number of cells uint8 source # [@enum SOURCE] Battery source -uint8 SOURCE_POWER_MODULE = 0 # Power module -uint8 SOURCE_EXTERNAL = 1 # External -uint8 SOURCE_ESCS = 2 # ESCs +uint8 SOURCE_POWER_MODULE = 0 # Power module (analog ADC or I2C power monitor) +uint8 SOURCE_EXTERNAL = 1 # External (MAVLink, CAN, or external driver) +uint8 SOURCE_ESCS = 2 # ESCs (via ESC telemetry) uint8 priority # [-] Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # [mAh] Capacity of the battery when fully charged diff --git a/docs/ko/msg_docs/BatteryStatusV0.md b/docs/ko/msg_docs/BatteryStatusV0.md index 86500d17a6..cd900a1f17 100644 --- a/docs/ko/msg_docs/BatteryStatusV0.md +++ b/docs/ko/msg_docs/BatteryStatusV0.md @@ -32,9 +32,9 @@ uint8 cell_count # [@invalid 0] Number of cells uint8 source # [@enum SOURCE] Battery source -uint8 SOURCE_POWER_MODULE = 0 # Power module -uint8 SOURCE_EXTERNAL = 1 # External -uint8 SOURCE_ESCS = 2 # ESCs +uint8 SOURCE_POWER_MODULE = 0 # Power module (analog ADC or I2C power monitor) +uint8 SOURCE_EXTERNAL = 1 # External (MAVLink, CAN, or external driver) +uint8 SOURCE_ESCS = 2 # ESCs (via ESC telemetry) uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # [mAh] Capacity of the battery when fully charged diff --git a/docs/ko/msg_docs/DeviceInformation.md b/docs/ko/msg_docs/DeviceInformation.md new file mode 100644 index 0000000000..d415461f94 --- /dev/null +++ b/docs/ko/msg_docs/DeviceInformation.md @@ -0,0 +1,45 @@ +# DeviceInformation (UORB message) + +Device information + +Can be used to uniquely associate a device_id from a sensor topic with a physical device using serial number. +as well as tracking of the used firmware versions on the devices. + +[source file](https://github.com/PX4/PX4-Autopilot/blob/main/msg/DeviceInformation.msg) + +```c +# Device information +# +# Can be used to uniquely associate a device_id from a sensor topic with a physical device using serial number. +# as well as tracking of the used firmware versions on the devices. + +uint64 timestamp # time since system start (microseconds) + +uint8 device_type # [@enum DEVICE_TYPE] Type of the device. Matches MAVLink DEVICE_TYPE enum + +uint8 DEVICE_TYPE_GENERIC = 0 # Generic/unknown sensor +uint8 DEVICE_TYPE_AIRSPEED = 1 # Airspeed sensor +uint8 DEVICE_TYPE_ESC = 2 # ESC +uint8 DEVICE_TYPE_SERVO = 3 # Servo +uint8 DEVICE_TYPE_GPS = 4 # GPS +uint8 DEVICE_TYPE_MAGNETOMETER = 5 # Magnetometer +uint8 DEVICE_TYPE_PARACHUTE = 6 # Parachute +uint8 DEVICE_TYPE_RANGEFINDER = 7 # Rangefinder +uint8 DEVICE_TYPE_WINCH = 8 # Winch +uint8 DEVICE_TYPE_BAROMETER = 9 # Barometer +uint8 DEVICE_TYPE_OPTICAL_FLOW = 10 # Optical flow +uint8 DEVICE_TYPE_ACCELEROMETER = 11 # Accelerometer +uint8 DEVICE_TYPE_GYROSCOPE = 12 # Gyroscope +uint8 DEVICE_TYPE_DIFFERENTIAL_PRESSURE = 13 # Differential pressure +uint8 DEVICE_TYPE_BATTERY = 14 # Battery +uint8 DEVICE_TYPE_HYGROMETER = 15 # Hygrometer + +char[32] vendor_name # Name of the device vendor +char[32] model_name # Name of the device model + +uint32 device_id # [-] [@invalid 0 if not available] Unique device ID for the sensor. Does not change between power cycles. +char[24] firmware_version # [-] [@invalid empty if not available] Firmware version. +char[24] hardware_version # [-] [@invalid empty if not available] Hardware version. +char[33] serial_number # [-] [@invalid empty if not available] Device serial number or unique identifier. + +``` diff --git a/docs/ko/msg_docs/EstimatorStatus.md b/docs/ko/msg_docs/EstimatorStatus.md index 9c24221691..aca77484b9 100644 --- a/docs/ko/msg_docs/EstimatorStatus.md +++ b/docs/ko/msg_docs/EstimatorStatus.md @@ -21,6 +21,7 @@ uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail uint8 GPS_CHECK_FAIL_SPOOFED = 10 # 10 : GPS signal is spoofed +uint8 GPS_CHECK_FAIL_JAMMED = 11 # 11 : GPS signal is jammed uint64 control_mode_flags # Bitmask to indicate EKF logic state uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete diff --git a/docs/ko/msg_docs/GpioIn.md b/docs/ko/msg_docs/GpioIn.md index 039ed02851..589e7d7841 100644 --- a/docs/ko/msg_docs/GpioIn.md +++ b/docs/ko/msg_docs/GpioIn.md @@ -6,6 +6,7 @@ GPIO mask and state ```c # GPIO mask and state +uint8 MAX_INSTANCES = 8 uint64 timestamp # time since system start (microseconds) uint32 device_id # Device id diff --git a/docs/ko/msg_docs/GpsDump.md b/docs/ko/msg_docs/GpsDump.md index 1f96901671..03910da906 100644 --- a/docs/ko/msg_docs/GpsDump.md +++ b/docs/ko/msg_docs/GpsDump.md @@ -9,11 +9,15 @@ This message is used to dump the raw gps communication to the log. uint64 timestamp # time since system start (microseconds) +uint8 INSTANCE_MAIN = 0 +uint8 INSTANCE_SECONDARY = 1 + uint8 instance # Instance of GNSS receiver +uint32 device_id uint8 len # length of data, MSB bit set = message to the gps device, # clear = message from the device uint8[79] data # data to write to the log -uint8 ORB_QUEUE_LENGTH = 8 +uint8 ORB_QUEUE_LENGTH = 16 ``` diff --git a/docs/ko/msg_docs/VehicleCommand.md b/docs/ko/msg_docs/VehicleCommand.md index 1b1b3ed658..225f680857 100644 --- a/docs/ko/msg_docs/VehicleCommand.md +++ b/docs/ko/msg_docs/VehicleCommand.md @@ -108,6 +108,7 @@ uint16 VEHICLE_CMD_LOGGING_START = 2510 # Start streaming ULog data. uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # Stop streaming ULog data. uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # Control starting/stopping transmitting data over the high latency link. uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition. +uint16 VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE = 5300 # Command safety on/off. |1 to activate safety, 0 to deactivate safety and allow control surface movements|Unused|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization. uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan. uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment. @@ -187,6 +188,10 @@ int8 ARMING_ACTION_ARM = 1 uint8 GRIPPER_ACTION_RELEASE = 0 uint8 GRIPPER_ACTION_GRAB = 1 +# Used as param1 in DO_SET_SAFETY_SWITCH_STATE command. +uint8 SAFETY_OFF = 0 +uint8 SAFETY_ON = 1 + uint8 ORB_QUEUE_LENGTH = 8 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. diff --git a/docs/ko/msg_docs/index.md b/docs/ko/msg_docs/index.md index 6b377bf072..7458c79a24 100644 --- a/docs/ko/msg_docs/index.md +++ b/docs/ko/msg_docs/index.md @@ -105,6 +105,7 @@ Graphs showing how these are used [can be found here](../middleware/uorb_graph.m - [DebugKeyValue](DebugKeyValue.md) - [DebugValue](DebugValue.md) - [DebugVect](DebugVect.md) +- [DeviceInformation](DeviceInformation.md) — Device information - [DifferentialPressure](DifferentialPressure.md) — Differential-pressure (airspeed) sensor - [DistanceSensor](DistanceSensor.md) — DISTANCE_SENSOR message data - [DistanceSensorModeChangeRequest](DistanceSensorModeChangeRequest.md) diff --git a/docs/ko/peripherals/dshot.md b/docs/ko/peripherals/dshot.md index 873ebe3d8a..020bd3a994 100644 --- a/docs/ko/peripherals/dshot.md +++ b/docs/ko/peripherals/dshot.md @@ -11,6 +11,10 @@ DShot is an alternative ESC protocol that has several advantages over [PWM](../p 이 항목에서는 DShot ESC 연결과 설정 방법을 설명합니다. +## Supported ESC + +[ESCs & Motors > Supported ESCs](../peripherals/esc_motors#supported-esc) has a list of supported ESC (check "Protocols" column for DShot ESC). + ## Wiring/Connections {#wiring} DShot ESC are wired the same way as [PWM ESCs](pwm_escs_and_servo.md). diff --git a/docs/ko/peripherals/esc_motors.md b/docs/ko/peripherals/esc_motors.md index 7fdbe37245..3705936825 100644 --- a/docs/ko/peripherals/esc_motors.md +++ b/docs/ko/peripherals/esc_motors.md @@ -3,80 +3,44 @@ Many PX4 drones use brushless motors that are driven by the flight controller via an Electronic Speed Controller (ESC). The ESC takes a signal from the flight controller and uses it to set control the level of power delivered to the motor. -PX4 supports a number of common protocols for sending the signals to ESCs: [PWM ESCs](../peripherals/pwm_escs_and_servo.md), [OneShot ESCs](../peripherals/oneshot.md), [DShot ESCs](../peripherals/dshot.md), [DroneCAN ESCs](../dronecan/escs.md), PCA9685 ESC (via I2C), and some UART ESCs (from Yuneec). +PX4 supports a number of [common protocols](../esc/esc_protocols.md) for sending the signals to ESCs: [PWM ESCs](../peripherals/pwm_escs_and_servo.md), [OneShot ESCs](../peripherals/oneshot.md), [DShot ESCs](../peripherals/dshot.md), [DroneCAN ESCs](../dronecan/escs.md), PCA9685 ESC (via I2C), and some UART ESCs (from Yuneec). + +## Supported ESC + +The following list is non-exhaustive. + +| ESC Device | Protocols | Firmwares | 참고 | +| ------------------------------ | ------------------------------------ | ------------------------ | ----------------------------------------------------- | +| [ARK 4IN1 ESC] | [Dshot], [PWM] | [AM32] | Has versions with/without connnectors | +| [Holybro Kotleta 20] | [DroneCAN], [PWM] | [PX4 Sapog ESC Firmware] | | +| [Vertiq Motor & ESC modules] | [Dshot], [OneShot], Multishot, [PWM] | Vertiq firmware | Larger modules support DroneCAN, ESC and Motor in one | +| [RaccoonLab CAN PWM ESC nodes] | [DroneCAN], Cyphal | | Cyphal and DroneCAN notes for PWM ESC | +| [VESC ESCs] | [DroneCAN], [PWM] | VESC project firmware | | +| [Zubax Telega] | [DroneCAN], [PWM] | Telega-based | ESC and Motor in one | + + + +[ARK 4IN1 ESC]: ../esc/ark_4in1_esc.md +[AM32]: https://am32.ca/ +[PX4 Sapog ESC Firmware]: ../dronecan/sapog.md +[VESC ESCs]: ../peripherals/vesc.md +[DroneCAN]: ../dronecan/escs.md +[Dshot]: ../peripherals/dshot.md +[OneShot]: ../peripherals/oneshot.md +[PWM]: ../peripherals/pwm_escs_and_servo.md +[Holybro Kotleta 20]: ../dronecan/holybro_kotleta.md +[Vertiq Motor & ESC modules]: ../peripherals/vertiq.md +[RaccoonLab CAN PWM ESC nodes]: ../dronecan/raccoonlab_nodes.md +[Zubax Telega]: ../dronecan/zubax_telega.md + +## See Also 더 자세한 정보는 다음을 참고하십시오. +- [ESC Protocols](../esc/esc_protocols.md) — overview of main ESC/Servo protocols supported by PX4 - [PWM ESCs and Servos](../peripherals/pwm_escs_and_servo.md) - [OneShot ESCs and Servos](../peripherals/oneshot.md) - [DShot](../peripherals/dshot.md) - [DroneCAN ESCs](../dronecan/escs.md) - [ESC Calibration](../advanced_config/esc_calibration.md) - [ESC Firmware and Protocols Overview](https://oscarliang.com/esc-firmware-protocols/) (oscarliang.com) - -A high level overview of the main ESC/Servo protocols supported by PX4 is given below. - -## ESC Protocols - -### PWM - -[PWM ESCs](../peripherals/pwm_escs_and_servo.md) are commonly used for fixed-wing vehicles and ground vehicles (vehicles that require a lower latency like multicopters typically use oneshot or dshot ESCs). - -PWM ESCs communicate using a periodic pulse, where the _width_ of the pulse indicates the desired power level. -The pulse wdith typically ranges between 1000uS for zero power and 2000uS for full power. -The periodic frame rate of the signal depends on the capability of the ESC, and commonly ranges between 50Hz and 490 Hz (the theoretical maximum being 500Hz for a very small "off" cycle). -A higher rate is better for ESCs, in particular where a rapid response to setpoint changes is needed. -For PWM servos 50Hz is usually sufficient, and many don't support higher rates. - -![duty cycle for PWM](../../assets/peripherals/esc_pwm_duty_cycle.png) - -In addition to being a relatively slow protocol PWM ESCs require [calibration](../advanced_config/esc_calibration.md) because the range values representing low and high values can vary significantly. -Unlike [dshot](#dshot) and [DroneCAN ESC](#dronecan) they do not have the ability to provide telemetry and feedback on ESC (or servo) state. - -Setup: - -- [ESC Wiring](../peripherals/pwm_escs_and_servo.md) -- [PX4 Configuration](../peripherals/pwm_escs_and_servo.md#px4-configuration) -- [ESC Calibration](../advanced_config/esc_calibration.md) - -### Oneshot 125 - -[OneShot 125 ESCs](../peripherals/oneshot.md) are usually much faster than PWM ESCs, and hence more responsive and easier to tune. -They are preferred over PWM for multicopters (but not as much as [DShot ESCs](#dshot), which do not require calibration, and may provide telemetry feedback). -There are a number of variants of the OneShot protocol, which support different rates. -PX4 only supports OneShot 125. - -OneShot 125 is the same as PWM but uses pulse widths that are 8 times shorter (from 125us to 250us for zero to full power). -This allows OneShot 125 ESCs to have a much shorter duty cycle/higher rate. -For PWM the theoretical maximum is close to 500 Hz while for OneShot it approaches 4 kHz. -The actual supported rate depends on the ESC used. - -Setup: - -- [ESC Wiring](../peripherals/pwm_escs_and_servo.md) (same as for PWM ESCs) -- [PX4 Configuration](../peripherals/oneshot.md#px4-configuration) -- [ESC Calibration](../advanced_config/esc_calibration.md) - -### DShot - -[DShot](../peripherals/dshot.md) is a digital ESC protocol that is highly recommended for vehicles that can benefit from reduce latency, in particular racing multicopters, VTOL vehicles, and so on. - -It has reduced latency and is more robust than both [PWM](#pwm) and [OneShot](#oneshot-125). -In addition it does not require ESC calibration, telemetry is available from some ESCs, and you can revers motor spin directions - -PX4 configuration is done in the [Actuator Configuration](../config/actuators.md). -Selecting a higher rate DShot ESC in the UI result in lower latency, but lower rates are more robust (and hence more suitable for large aircraft with longer leads); some ESCs only support lower rates (see datasheets for information). - -Setup: - -- [ESC Wiring](../peripherals/pwm_escs_and_servo.md) (same as for PWM ESCs) -- [DShot](../peripherals/dshot.md) also contains information about how to send commands etc. - -### DroneCAN - -[DroneCAN ESCs](../dronecan/escs.md) are recommended when DroneCAN is the primary bus used for your vehicle. -The PX4 implementation is currently limited to update rates of 200Hz. - -DroneCAN shares many similar benefits to [Dshot](#dshot) including high data rates, robust connection over long leads, telemetry feedback, no need for calibration of the ESC itself. - -[DroneCAN ESCs](../dronecan/escs.md) are connected via the DroneCAN bus (setup and configuration are covered at that link). diff --git a/docs/ko/uorb/uorb_documentation.md b/docs/ko/uorb/uorb_documentation.md new file mode 100644 index 0000000000..d056d48860 --- /dev/null +++ b/docs/ko/uorb/uorb_documentation.md @@ -0,0 +1,170 @@ +# uORB Documentation Standard + +This topic demonstrates and explains how to document uORB messages. + +:::info +At time of writing many topics have not been updated. +::: + +## 개요 + +The [AirspeedValidated](../msg_docs/AirspeedValidated.md) message shown below is a good example of a uORB topic that has been documented to the current standard. + +```py +# Validated airspeed +# +# Provides information about airspeed (indicated, true, calibrated) and the source of the data. +# Used by controllers, estimators and for airspeed reporting to operator. + +uint32 MESSAGE_VERSION = 1 + +uint64 timestamp # [us] Time since system start + +float32 indicated_airspeed_m_s # [m/s] [@invalid NaN] Indicated airspeed (IAS) +float32 calibrated_airspeed_m_s # [m/s] [@invalid NaN] Calibrated airspeed (CAS) +float32 true_airspeed_m_s # [m/s] [@invalid NaN] True airspeed (TAS) + +int8 airspeed_source # [@enum SOURCE] Source of currently published airspeed values +int8 SOURCE_DISABLED = -1 # Disabled +int8 SOURCE_GROUND_MINUS_WIND = 0 # Ground speed minus wind +int8 SOURCE_SENSOR_1 = 1 # Sensor 1 +int8 SOURCE_SENSOR_2 = 2 # Sensor 2 +int8 SOURCE_SENSOR_3 = 3 # Sensor 3 +int8 SOURCE_SYNTHETIC = 4 # Synthetic airspeed + +float32 calibrated_ground_minus_wind_m_s # [m/s] [@invalid NaN] CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption +float32 calibraded_airspeed_synth_m_s # [m/s] [@invalid NaN] Synthetic airspeed +float32 airspeed_derivative_filtered # [m/s^2] Filtered indicated airspeed derivative +float32 throttle_filtered # [-] Filtered fixed-wing throttle +float32 pitch_filtered # [rad] Filtered pitch +``` + +The main things to note are: + +- Documentation is added using formatted uORB comments. + Any text on a line after the `#` character is a comment, except for lines that start with the text `# TOPIC` (which indicates a multi-topic message). +- The message starts with a comment block consisting of short description (mandatory), followed by a longer description and then a space. +- Field and constants almost all have comments. + The comments are added on the same line as the field/constant, separated by one space. +- Fields: + - Comments are all on the same line as the field (extra lines become internal comments). + - Comments start with metadata, such as the units (`[m/s]`, `[rad/s]`) or allowed values (`[@enum SOURCE]`), and can also list invalid values (`[@invalid NaN]`) and allowed ranges (`[@range min, max]`). + - Units are required except for boolean fields or for fields with an enum value. + `[-]` is used to indicate unitless fields. + - Comments follow the metadata after a space. + The line should not be terminated in a full stop. +- Constants: + - Don't have metadata: the description follows the comment marker after one space. + - Some constants, such as `MESSAGE_VERSION`, don't need documentation because they are standardized. + - Constants with the same name prefix are grouped together as enums after the associated field. + +The following sections expand on the allowed formats. + +## Message Description + +Every message should start with a comment block that describes the message: + +```py +# Short description (mandatory) +# +# Longer description for the message if needed. +# Can be multiline, and should have punctuation. +# Should be followed by an empty line. +``` + +This consists of a mandatory short description, optionally followed by an empty comment line, and then a longer description. + +Short description (mandatory): + +- A succinct explanation for the purpose of the message. +- Usually just one line without a terminating full stop. +- Minimally it may just mirror the message name. +- For example, [`AirspeedValidated`](../msg_docs/AirspeedValidated.md) above has the short description `Validated airspeed`. + +Long description (Optional): + +- Additional context required to understand how the message is used. +- In particular this should be anything that can't be inferred from the name, fields or constants, such as the publishers and expected consumers. + It might also cover whether the message is only used for a particular frame type or mode. +- The message is often multiline and contains punctuation. +- May include comment lines that are empty, in order to indicate paragraphs. + +Both short and long descriptions may be multi-line. +Single line descriptions should not include a terminating full stop, but multiline comments should do so. + +The message description block ends at the first non-comment line, which should be an empty line, but might be a field or constant. +Any subsequent comment lines are considered "internal comments". + +### Fields + +A typical field comment looks like this: + +```py +float32 indicated_airspeed_m_s # [m/s] [@invalid NaN] Indicated airspeed (IAS) +``` + +Field comments must all be on the same line as the field, and consist of optional metadata followed by a description: + +- `metadata` (Optional) + - Information about the field units and allowed values: + - `[]` + - The unit of measurement inside square brackets (note, no `@` delineator indicates a unit), such as `[m]` for metres. + - Allowed units include: `m`, `m/s`, `m/s^2`, `rad`, `rad/s`, `rpm`, `V`, `A`, `mA`, `mAh`, `W`, `dBm`, `s`, `ms`, `us`, `Ohm`, `MB`, `Kb/s`, `degC`, `Pa`. + - Units are required unless clearly invalid, such as when the field is a boolean, or is an enum value. + - Unitless values should be specified as `[-]`. + Note though that units are not required for boolean fields or enum fields. + - `[@enum ]` + - The `enum_name` gives the prefix of constant values in the message that can be assigned to the field. + Note that enums in uORB are just a naming convention: they are not explicitly declared. + Multiple enum names allowed for a field indicates a possible error in the field design. + - `[@range , ]` + - The allowed range of the field, specified as a `lower_value` and/or an `upper_value`. + Either value can be omitted to indicate an unbounded upper or lower value. + For example `[@range 0, 3]`, `[@range 5.3, ]`, `[@range , 3]`. + - `[@invalid ]` + - The `value` to set the field to indicate that the field doesn't contain valid data, such as `[@invalid NaN]`. + The `description` is optional, and might be used to indicate the conditions under which data is invalid. + - `[@frame ]` + - The `frame` in which the field is set, such as `[@frame NED]` or `[@frame Body]`. +- `description` + - A concise description of the purpose of the field, and including any important information that can't be inferred from the name! + Use a capital first letter, and omit the full stop if the description is a single sentence. + Multiple sentences may also omit the final full stop. + +### Constants + +Constants follow the documentation conventions as fields except they only have a description (no metadata). +Documentation for a constant might look like this: + +```py +int8 SOURCE_GROUND_MINUS_WIND = 0 # Ground speed minus wind +``` + +Constants are often grouped together following a field as enum values. +Note below how the prefix `SOURCE` for the values is specified as an enum against the _field_. + +```py +int8 airspeed_source # [@enum SOURCE] Source of currently published airspeed values +int8 SOURCE_DISABLED = -1 # Disabled +int8 SOURCE_GROUND_MINUS_WIND = 0 # Ground speed minus wind +... +``` + +A small number of constants have a standardised meaning and do not require documentation. +These are: + +- `ORB_QUEUE_LENGTH` +- `MESSAGE_VERSION` + +### `# TOPICS` + +The prefix `# TOPICS` is used to indicate topic names for multi-topic messages. +For example, the [VehicleGlobalPosition.msg](../msg_docs/VehicleGlobalPosition.md) message definition is used to define the topic ids as shown: + +```text +# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position +# TOPICS estimator_global_position +# TOPICS aux_global_position +``` + +At time of writing there is no format for documenting these. diff --git a/docs/uk/SUMMARY.md b/docs/uk/SUMMARY.md index 47841b7b8a..3e1e9c1f3c 100644 --- a/docs/uk/SUMMARY.md +++ b/docs/uk/SUMMARY.md @@ -584,6 +584,7 @@ - [DebugKeyValue](msg_docs/DebugKeyValue.md) - [DebugValue](msg_docs/DebugValue.md) - [DebugVect](msg_docs/DebugVect.md) + - [DeviceInformation](msg_docs/DeviceInformation.md) - [DifferentialPressure](msg_docs/DifferentialPressure.md) - [DistanceSensor](msg_docs/DistanceSensor.md) - [DistanceSensorModeChangeRequest](msg_docs/DistanceSensorModeChangeRequest.md) diff --git a/docs/uk/advanced_config/ethernet_setup.md b/docs/uk/advanced_config/ethernet_setup.md index 7a977ab488..5366b53e2f 100644 --- a/docs/uk/advanced_config/ethernet_setup.md +++ b/docs/uk/advanced_config/ethernet_setup.md @@ -25,6 +25,7 @@ PX4 supports Ethernet connectivity on [Pixhawk 5X-standard](https://github.com/p Підтримувані автопілоти включають: +- [ARK Electronics ARKV6X](../flight_controller/ark_v6x.md) - [CUAV Pixhawk V6X](../flight_controller/cuav_pixhawk_v6x.md) - [Holybro Pixhawk 5X](../flight_controller/pixhawk5x.md) - [Holybro Pixhawk 6X](../flight_controller/pixhawk6x.md) diff --git a/docs/uk/assembly/_assembly.md b/docs/uk/assembly/_assembly.md index 68998b05e1..163acaf4f7 100644 --- a/docs/uk/assembly/_assembly.md +++ b/docs/uk/assembly/_assembly.md @@ -285,7 +285,7 @@ A particular vehicle might have more/fewer motors and actuators, but the wiring The following sections explain each part in more detail. :::tip -If you're using [DroneCAN ESC](../peripherals/esc_motors.md#dronecan) the control signals will be connected to the CAN BUS instead of the PWM outputs as shown. +If you're using [DroneCAN ESC](../dronecan/escs.md) the control signals will be connected to the CAN BUS instead of the PWM outputs as shown. ::: ### Flight Controller Power @@ -426,7 +426,6 @@ They recommend sensors, power systems, and other components from the same manufa - [Drone Components & Parts](../getting_started/px4_basic_concepts.md#drone-components-parts) (Basic Concepts) - [Payloads](../getting_started/px4_basic_concepts.md#payloads) (Basic Concepts) - [Hardware Selection & Setup](../hardware/drone_parts.md) — information about connecting and configuring specific flight controllers, sensors and other peripherals (e.g. airspeed sensor for planes). - - [Mounting the Flight Controller](../assembly/mount_and_orient_controller.md) - [Vibration Isolation](../assembly/vibration_isolation.md) - [Mounting a Compass](../assembly/mount_gps_compass.md) diff --git a/docs/uk/config_mc/filter_tuning.md b/docs/uk/config_mc/filter_tuning.md index 8bfb92fd79..46d9e8bc99 100644 --- a/docs/uk/config_mc/filter_tuning.md +++ b/docs/uk/config_mc/filter_tuning.md @@ -70,7 +70,7 @@ Airframes with more than two frequency noise spikes typically clean the first tw Dynamic notch filters use ESC RPM feedback and/or the onboard FFT analysis. The ESC RPM feedback is used to track the rotor blade pass frequency and its harmonics, while the FFT analysis can be used to track a frequency of another vibration source, such as a fuel engine. -ESC RPM feedback requires ESCs capable of providing RPM feedback such as [DShot](../peripherals/esc_motors.md#dshot) with telemetry connected, a bidirectional DShot set up ([work in progress](https://github.com/PX4/PX4-Autopilot/pull/23863)), or [UAVCAN/DroneCAN ESCs](../dronecan/escs.md). +ESC RPM feedback requires ESCs capable of providing RPM feedback such as [DShot](../peripherals/dshot.md) with telemetry connected, a bidirectional DShot set up ([work in progress](https://github.com/PX4/PX4-Autopilot/pull/23863)), or [UAVCAN/DroneCAN ESCs](../dronecan/escs.md). Before enabling, make sure that the ESC RPM is correct. You might have to adjust the [pole count of the motors](../advanced_config/parameter_reference.md#MOT_POLE_COUNT). diff --git a/docs/uk/dronecan/escs.md b/docs/uk/dronecan/escs.md index 03cb7e87e8..9b1d1b748b 100644 --- a/docs/uk/dronecan/escs.md +++ b/docs/uk/dronecan/escs.md @@ -1,7 +1,14 @@ # DroneCAN ESCs -PX4 підтримує ESCs, які відповідають стандарту DroneCAN. -Для отримання додаткової інформації дивіться наступні статті для конкретного обладнання/прошивки: +PX4 supports DroneCAN compliant ESCs. + +## Supported ESC + +:::info +[Supported ESCs](../peripherals/esc_motors#supported-esc) in _ESCs & Motors_ may include additional devices that are not listed below. +::: + +The following articles have specific hardware/firmware information: - [PX4 Sapog ESC Firmware](sapog.md) - [Holybro Kotleta 20](holybro_kotleta.md) diff --git a/docs/uk/modules/modules_driver.md b/docs/uk/modules/modules_driver.md index 153914d198..b87be5aec8 100644 --- a/docs/uk/modules/modules_driver.md +++ b/docs/uk/modules/modules_driver.md @@ -15,38 +15,6 @@ - [Rpm Sensor](modules_driver_rpm_sensor.md) - [Transponder](modules_driver_transponder.md) -## MCP23009 - -Source: [drivers/gpio/mcp23009](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/gpio/mcp23009) - -### Usage {#MCP23009_usage} - -``` -MCP23009 [arguments...] - Commands: - start - [-I] Internal I2C bus(es) - [-X] External I2C bus(es) - [-b ] board-specific bus (default=all) (external SPI: n-th bus - (default=1)) - [-f ] bus frequency in kHz - [-q] quiet startup (no message if no device found) - [-a ] I2C address - default: 37 - [-D ] Direction - default: 0 - [-O ] Output - default: 0 - [-P ] Pullups - default: 0 - [-U ] Update Interval [ms] - default: 0 - - stop - - status print status info -``` - ## atxxxx Source: [drivers/osd/atxxxx](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/osd/atxxxx) @@ -749,6 +717,40 @@ lsm303agr [arguments...] status print status info ``` +## mcp230xx + +Source: [lib/drivers/mcp_common](https://github.com/PX4/PX4-Autopilot/tree/main/src/lib/drivers/mcp_common) + +### Usage {#mcp230xx_usage} + +``` +mcp230xx [arguments...] + Commands: + start + [-I] Internal I2C bus(es) + [-X] External I2C bus(es) + [-b ] board-specific bus (default=all) (external SPI: n-th bus + (default=1)) + [-f ] bus frequency in kHz + [-q] quiet startup (no message if no device found) + [-a ] I2C address + default: 39 + [-D ] Direction (1=Input, 0=Output) + default: 0 + [-O ] Output + default: 0 + [-P ] Pullups + default: 0 + [-U ] Update Interval [ms] + default: 0 + [-M ] First minor number + default: 0 + + stop + + status print status info +``` + ## mcp9808 Source: [drivers/temperature_sensor/mcp9808](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/temperature_sensor/mcp9808) @@ -899,8 +901,6 @@ fetching the latest mixing result and write them to PCA9685 at its scheduling ti It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width that can be accepted by most ESCs and servos. -The I2C bus and address can be configured via parameters `PCA9685_EN_BUS` and `PCA9685_I2C_ADDR`, or via command line arguments. - ### Приклади It is typically started with: diff --git a/docs/uk/modules/modules_system.md b/docs/uk/modules/modules_system.md index 66171c3846..ef5cd50c40 100644 --- a/docs/uk/modules/modules_system.md +++ b/docs/uk/modules/modules_system.md @@ -127,6 +127,10 @@ commander [arguments...] check Run preflight checks + safety Change prearm safety state + on|off [on] to activate safety, [off] to deactivate safety and allow + control surface movements + arm [-f] Force arming (do not run preflight checks) diff --git a/docs/uk/msg_docs/BatteryStatus.md b/docs/uk/msg_docs/BatteryStatus.md index 068b6a5b82..df35417e0f 100644 --- a/docs/uk/msg_docs/BatteryStatus.md +++ b/docs/uk/msg_docs/BatteryStatus.md @@ -2,7 +2,7 @@ Battery status -Battery status information for up to 4 battery instances. +Battery status information for up to 3 battery instances. These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. Battery instance information is also logged and streamed in MAVLink telemetry. @@ -11,7 +11,7 @@ Battery instance information is also logged and streamed in MAVLink telemetry. ```c # Battery status # -# Battery status information for up to 4 battery instances. +# Battery status information for up to 3 battery instances. # These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. # Battery instance information is also logged and streamed in MAVLink telemetry. @@ -33,9 +33,9 @@ uint8 cell_count # [-] [@invalid 0] Number of cells uint8 source # [@enum SOURCE] Battery source -uint8 SOURCE_POWER_MODULE = 0 # Power module -uint8 SOURCE_EXTERNAL = 1 # External -uint8 SOURCE_ESCS = 2 # ESCs +uint8 SOURCE_POWER_MODULE = 0 # Power module (analog ADC or I2C power monitor) +uint8 SOURCE_EXTERNAL = 1 # External (MAVLink, CAN, or external driver) +uint8 SOURCE_ESCS = 2 # ESCs (via ESC telemetry) uint8 priority # [-] Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # [mAh] Capacity of the battery when fully charged diff --git a/docs/uk/msg_docs/BatteryStatusV0.md b/docs/uk/msg_docs/BatteryStatusV0.md index 86500d17a6..cd900a1f17 100644 --- a/docs/uk/msg_docs/BatteryStatusV0.md +++ b/docs/uk/msg_docs/BatteryStatusV0.md @@ -32,9 +32,9 @@ uint8 cell_count # [@invalid 0] Number of cells uint8 source # [@enum SOURCE] Battery source -uint8 SOURCE_POWER_MODULE = 0 # Power module -uint8 SOURCE_EXTERNAL = 1 # External -uint8 SOURCE_ESCS = 2 # ESCs +uint8 SOURCE_POWER_MODULE = 0 # Power module (analog ADC or I2C power monitor) +uint8 SOURCE_EXTERNAL = 1 # External (MAVLink, CAN, or external driver) +uint8 SOURCE_ESCS = 2 # ESCs (via ESC telemetry) uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # [mAh] Capacity of the battery when fully charged diff --git a/docs/uk/msg_docs/DeviceInformation.md b/docs/uk/msg_docs/DeviceInformation.md new file mode 100644 index 0000000000..d415461f94 --- /dev/null +++ b/docs/uk/msg_docs/DeviceInformation.md @@ -0,0 +1,45 @@ +# DeviceInformation (UORB message) + +Device information + +Can be used to uniquely associate a device_id from a sensor topic with a physical device using serial number. +as well as tracking of the used firmware versions on the devices. + +[source file](https://github.com/PX4/PX4-Autopilot/blob/main/msg/DeviceInformation.msg) + +```c +# Device information +# +# Can be used to uniquely associate a device_id from a sensor topic with a physical device using serial number. +# as well as tracking of the used firmware versions on the devices. + +uint64 timestamp # time since system start (microseconds) + +uint8 device_type # [@enum DEVICE_TYPE] Type of the device. Matches MAVLink DEVICE_TYPE enum + +uint8 DEVICE_TYPE_GENERIC = 0 # Generic/unknown sensor +uint8 DEVICE_TYPE_AIRSPEED = 1 # Airspeed sensor +uint8 DEVICE_TYPE_ESC = 2 # ESC +uint8 DEVICE_TYPE_SERVO = 3 # Servo +uint8 DEVICE_TYPE_GPS = 4 # GPS +uint8 DEVICE_TYPE_MAGNETOMETER = 5 # Magnetometer +uint8 DEVICE_TYPE_PARACHUTE = 6 # Parachute +uint8 DEVICE_TYPE_RANGEFINDER = 7 # Rangefinder +uint8 DEVICE_TYPE_WINCH = 8 # Winch +uint8 DEVICE_TYPE_BAROMETER = 9 # Barometer +uint8 DEVICE_TYPE_OPTICAL_FLOW = 10 # Optical flow +uint8 DEVICE_TYPE_ACCELEROMETER = 11 # Accelerometer +uint8 DEVICE_TYPE_GYROSCOPE = 12 # Gyroscope +uint8 DEVICE_TYPE_DIFFERENTIAL_PRESSURE = 13 # Differential pressure +uint8 DEVICE_TYPE_BATTERY = 14 # Battery +uint8 DEVICE_TYPE_HYGROMETER = 15 # Hygrometer + +char[32] vendor_name # Name of the device vendor +char[32] model_name # Name of the device model + +uint32 device_id # [-] [@invalid 0 if not available] Unique device ID for the sensor. Does not change between power cycles. +char[24] firmware_version # [-] [@invalid empty if not available] Firmware version. +char[24] hardware_version # [-] [@invalid empty if not available] Hardware version. +char[33] serial_number # [-] [@invalid empty if not available] Device serial number or unique identifier. + +``` diff --git a/docs/uk/msg_docs/EstimatorStatus.md b/docs/uk/msg_docs/EstimatorStatus.md index 9054870693..95b8227483 100644 --- a/docs/uk/msg_docs/EstimatorStatus.md +++ b/docs/uk/msg_docs/EstimatorStatus.md @@ -21,6 +21,7 @@ uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail uint8 GPS_CHECK_FAIL_SPOOFED = 10 # 10 : GPS signal is spoofed +uint8 GPS_CHECK_FAIL_JAMMED = 11 # 11 : GPS signal is jammed uint64 control_mode_flags # Bitmask to indicate EKF logic state uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete diff --git a/docs/uk/msg_docs/GpioIn.md b/docs/uk/msg_docs/GpioIn.md index ae4d3c2029..668b6ba865 100644 --- a/docs/uk/msg_docs/GpioIn.md +++ b/docs/uk/msg_docs/GpioIn.md @@ -6,6 +6,7 @@ ```c # GPIO mask and state +uint8 MAX_INSTANCES = 8 uint64 timestamp # time since system start (microseconds) uint32 device_id # Device id diff --git a/docs/uk/msg_docs/GpsDump.md b/docs/uk/msg_docs/GpsDump.md index 7b7e3a776f..f42a2d9638 100644 --- a/docs/uk/msg_docs/GpsDump.md +++ b/docs/uk/msg_docs/GpsDump.md @@ -9,11 +9,15 @@ This message is used to dump the raw gps communication to the log. uint64 timestamp # time since system start (microseconds) +uint8 INSTANCE_MAIN = 0 +uint8 INSTANCE_SECONDARY = 1 + uint8 instance # Instance of GNSS receiver +uint32 device_id uint8 len # length of data, MSB bit set = message to the gps device, # clear = message from the device uint8[79] data # data to write to the log -uint8 ORB_QUEUE_LENGTH = 8 +uint8 ORB_QUEUE_LENGTH = 16 ``` diff --git a/docs/uk/msg_docs/VehicleCommand.md b/docs/uk/msg_docs/VehicleCommand.md index 6701853ea8..71314eb864 100644 --- a/docs/uk/msg_docs/VehicleCommand.md +++ b/docs/uk/msg_docs/VehicleCommand.md @@ -108,6 +108,7 @@ uint16 VEHICLE_CMD_LOGGING_START = 2510 # Start streaming ULog data. uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # Stop streaming ULog data. uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # Control starting/stopping transmitting data over the high latency link. uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition. +uint16 VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE = 5300 # Command safety on/off. |1 to activate safety, 0 to deactivate safety and allow control surface movements|Unused|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization. uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan. uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment. @@ -187,6 +188,10 @@ int8 ARMING_ACTION_ARM = 1 uint8 GRIPPER_ACTION_RELEASE = 0 uint8 GRIPPER_ACTION_GRAB = 1 +# Used as param1 in DO_SET_SAFETY_SWITCH_STATE command. +uint8 SAFETY_OFF = 0 +uint8 SAFETY_ON = 1 + uint8 ORB_QUEUE_LENGTH = 8 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. diff --git a/docs/uk/msg_docs/index.md b/docs/uk/msg_docs/index.md index 2275df088a..3bc682a23c 100644 --- a/docs/uk/msg_docs/index.md +++ b/docs/uk/msg_docs/index.md @@ -105,6 +105,7 @@ Graphs showing how these are used [can be found here](../middleware/uorb_graph.m - [DebugKeyValue](DebugKeyValue.md) - [DebugValue](DebugValue.md) - [DebugVect](DebugVect.md) +- [DeviceInformation](DeviceInformation.md) — Device information - [DifferentialPressure](DifferentialPressure.md) — Differential-pressure (airspeed) sensor - [DistanceSensor](DistanceSensor.md) — DISTANCE_SENSOR message data - [DistanceSensorModeChangeRequest](DistanceSensorModeChangeRequest.md) diff --git a/docs/uk/peripherals/dshot.md b/docs/uk/peripherals/dshot.md index e493119df4..711bfca89d 100644 --- a/docs/uk/peripherals/dshot.md +++ b/docs/uk/peripherals/dshot.md @@ -11,6 +11,10 @@ DShot is an alternative ESC protocol that has several advantages over [PWM](../p Ця тема показує, як підключити та налаштувати DShot ESC. +## Supported ESC + +[ESCs & Motors > Supported ESCs](../peripherals/esc_motors#supported-esc) has a list of supported ESC (check "Protocols" column for DShot ESC). + ## Wiring/Connections {#wiring} DShot ESC are wired the same way as [PWM ESCs](pwm_escs_and_servo.md). diff --git a/docs/uk/peripherals/esc_motors.md b/docs/uk/peripherals/esc_motors.md index 84be78bd2b..6fb60c6709 100644 --- a/docs/uk/peripherals/esc_motors.md +++ b/docs/uk/peripherals/esc_motors.md @@ -9,13 +9,14 @@ PX4 supports a number of [common protocols](../esc/esc_protocols.md) for sending The following list is non-exhaustive. -| ESC Device | Протоколи | Firmwares | Примітки | -| ---------------------------- | ------------------------------------ | ------------------------ | ----------------------------------------------------- | -| [ARK 4IN1 ESC] | [Dshot], [PWM] | [AM32] | Has versions with/without connnectors | -| [Holybro Kotleta 20] | [DroneCAN], [PWM] | [PX4 Sapog ESC Firmware] | | -| [Vertiq Motor & ESC modules] | [Dshot], [OneShot], Multishot, [PWM] | Vertiq firmware | Larger modules support DroneCAN, ESC and Motor in one | -| [VESC ESCs] | [DroneCAN], [PWM] | VESC project firmware | | -| [Zubax Telega] | [DroneCAN], [PWM] | Telega-based | ESC and Motor in one | +| ESC Device | Протоколи | Firmwares | Примітки | +| ------------------------------ | ------------------------------------ | ------------------------ | ----------------------------------------------------- | +| [ARK 4IN1 ESC] | [Dshot], [PWM] | [AM32] | Has versions with/without connnectors | +| [Holybro Kotleta 20] | [DroneCAN], [PWM] | [PX4 Sapog ESC Firmware] | | +| [Vertiq Motor & ESC modules] | [Dshot], [OneShot], Multishot, [PWM] | Vertiq firmware | Larger modules support DroneCAN, ESC and Motor in one | +| [RaccoonLab CAN PWM ESC nodes] | [DroneCAN], Cyphal | | Cyphal and DroneCAN notes for PWM ESC | +| [VESC ESCs] | [DroneCAN], [PWM] | VESC project firmware | | +| [Zubax Telega] | [DroneCAN], [PWM] | Telega-based | ESC and Motor in one | @@ -29,6 +30,7 @@ The following list is non-exhaustive. [PWM]: ../peripherals/pwm_escs_and_servo.md [Holybro Kotleta 20]: ../dronecan/holybro_kotleta.md [Vertiq Motor & ESC modules]: ../peripherals/vertiq.md +[RaccoonLab CAN PWM ESC nodes]: ../dronecan/raccoonlab_nodes.md [Zubax Telega]: ../dronecan/zubax_telega.md ## Дивіться також diff --git a/docs/zh/SUMMARY.md b/docs/zh/SUMMARY.md index 92a00d86c7..2eead55cb5 100644 --- a/docs/zh/SUMMARY.md +++ b/docs/zh/SUMMARY.md @@ -584,6 +584,7 @@ - [DebugKeyValue](msg_docs/DebugKeyValue.md) - [DebugValue](msg_docs/DebugValue.md) - [DebugVect](msg_docs/DebugVect.md) + - [DeviceInformation](msg_docs/DeviceInformation.md) - [DifferentialPressure](msg_docs/DifferentialPressure.md) - [DistanceSensor](msg_docs/DistanceSensor.md) - [DistanceSensorModeChangeRequest](msg_docs/DistanceSensorModeChangeRequest.md) diff --git a/docs/zh/advanced_config/ethernet_setup.md b/docs/zh/advanced_config/ethernet_setup.md index b41967a31e..db82bdd8be 100644 --- a/docs/zh/advanced_config/ethernet_setup.md +++ b/docs/zh/advanced_config/ethernet_setup.md @@ -25,6 +25,7 @@ PX4 supports Ethernet connectivity on [Pixhawk 5X-standard](https://github.com/p 支持的飞行控制器包括: +- [ARK Electronics ARKV6X](../flight_controller/ark_v6x.md) - [CUAV Pixhawk V6X](../flight_controller/cuav_pixhawk_v6x.md) - [Holybro Pixhawk 5X](../flight_controller/pixhawk5x.md) - [Holybro Pixhawk 6X](../flight_controller/pixhawk6x.md) diff --git a/docs/zh/advanced_config/tuning_the_ecl_ekf.md b/docs/zh/advanced_config/tuning_the_ecl_ekf.md index aaeb70e598..bd1dd5481d 100644 --- a/docs/zh/advanced_config/tuning_the_ecl_ekf.md +++ b/docs/zh/advanced_config/tuning_the_ecl_ekf.md @@ -99,7 +99,7 @@ EKF 实例的总数是 [EKF2_MULTI_IMU](../advanced_config/parameter_reference.m - [SENS_IMU_MODE](../advanced_config/parameter_reference.md#SENS_IMU_MODE): 如果是以 IMU 传感器多样性运行多个 EKF 实例，即 [EKF2_MULTI_IMU](../advanced_config/parameter_reference.md#EKF2_MULTI_IMU) > 1，则设置为 0。 - 当设置为 1（单个 EKF 操作的默认值）时，传感器模块选择 EKF 使用的 IMU 数据。 + 当设置为 1（单个 EKF 的默认值）时，传感器模块选择 EKF 使用的 IMU 数据。 这提供了针对传感器数据丢失的保护，但不提供针对错误传感器数据的保护。 当设置为 0 时，传感器模块不进行选择。 @@ -107,7 +107,7 @@ EKF 实例的总数是 [EKF2_MULTI_IMU](../advanced_config/parameter_reference.m 如果是以磁力计传感器多样性运行多个 EKF 实例，即 [EKF2_MULTI_MAG](../ advanced_config/parameter_reference.md#EKF2_MULTI_MAG) > 1，则设置为 0。 - 当设置为 1（单个 EKF 操作的默认值）时，传感器模块选择 EKF 使用的磁力计数据。 + 当设置为 1（单个 EKF 的默认值）时，传感器模块选择 EKF 使用的磁力计数据。 这提供了针对传感器数据丢失的保护，但不提供针对错误传感器数据的保护。 当设置为 0 时，传感器模块不进行选择。 @@ -115,13 +115,13 @@ EKF 实例的总数是 [EKF2_MULTI_IMU](../advanced_config/parameter_reference.m 此参数指定多个 EKF 使用的 IMU 传感器数量。 如果 `EKF2_MULTI_IMU` <= 1，则仅使用第一个 IMU 传感器。 当 [SENS_IMU_MODE](../advanced_config/parameter_reference.md#SENS_IMU_MODE) = 1 时，这将是传感器模块选择的传感器。 - 如果 `EKF2_MULTI_IMU` >= 2，则将针对指定数量的 IMU 传感器（最多 4 个或存在的 IMU 数量，取较小值）运行单独的 EKF 实例。 + 如果 `EKF2_MULTI_IMU` >= 2，那么将为指定数量的 IMU 传感器运行独立的 EKF 实例，最多支持 4 个或实际存在的 IMU 数量（取两者中的较小值）。 - [EKF2_MULTI_MAG](../advanced_config/parameter_reference.md#EKF2_MULTI_MAG): 此参数指定多个 EKF 使用的磁力计传感器数量。 如果 `EKF2_MULTI_MAG` <= 1，则仅使用第一个磁力计传感器。 当 [SENS_MAG_MODE](../advanced_config/parameter_reference.md#SENS_MAG_MODE) = 1 时，这将是传感器模块选择的传感器。 - 如果 `EKF2_MULTI_MAG` >= 2，则将针对指定数量的磁力计传感器（最多 4 个或存在的磁力计数量，取较小值）运行单独的 EKF 实例。 + 如果 `EKF2_MULTI_MAG` >= 2，那么将为指定数量的磁力计传感器运行独立的 EKF 实例，最多支持 4 个或实际存在的磁力计数量（取两者中的较小值）。 :::info 不支持多 EKF 实例飞行日志的记录和 [EKF2 回放](../debug/system_wide_replay.md#ekf2-replay)。 @@ -131,26 +131,26 @@ EKF 实例的总数是 [EKF2_MULTI_IMU](../advanced_config/parameter_reference.m ## 它使用哪些传感器测量？ EKF 具有不同的操作模式，允许不同的传感器测量组合。 -启动时，滤波器会检查最小的可行传感器组合，并在初始倾斜、偏航和高度对准完成后，进入提供旋转、垂直速度、垂直位置、IMU 角度增量零偏和 IMU 速度增量零偏估计的模式。 +启动时，滤波器会检查传感器的最小可用组合，并在初始倾斜、偏航和高度对准完成后，进入提供旋转、垂直速度、垂直位置、IMU 角度增量零偏和 IMU 速度增量零偏估计的模式。 此模式需要 IMU 数据、偏航源（磁力计或外部视觉）和高度数据源。 -所有 EKF 操作模式都需要此最小数据集。 -然后可以使用其他传感器数据来估计额外的状态。 +所有 EKF 工作模式都需要此最小数据集。 +其他传感器数据可用于估计额外状态。 ### IMU -- 三轴机体固定惯性测量单元 (IMU) 的角度增量和速度增量数据，最小速率为 100Hz。 - 注意：在 EKF 使用 IMU 角度增量数据之前，应先对其应用圆锥效应校正。 +- 固定在机体上的三轴 IMU，以至少100Hz的频率获取增量角度和角速度数据 。 + 注意：在 EKF 使用 IMU 角度增量数据之前，应该使用圆锥校正算法校正。 ### 磁力计 -估计器需要三轴机体固定磁力计数据，最小速率为 5Hz。 +固定在机体上的三轴磁力计数据，至少以 5Hz 提供数据才会被估计器用于估计。 ::: info - 磁力计 **零偏 (biases)** 仅在无人机旋转时可观测。 -- 当载具加速（线性加速度）且同时融合绝对位置或速度测量值（例如 GPS）时，真实航向是可观测的。 - 这意味着如果这些条件能够足够频繁地满足以约束航向漂移（由陀螺仪零偏引起），则初始化后的磁力计航向测量是可选的。 +- 当载具处于加速状态（线性加速度）时，可通过融合绝对位置或速度测量数据（例如GPS）来观测真实航向。 + 这意味着在初始化后，如果满足上述条件且频率足够高以约束（由陀螺仪偏置引起的）航向漂移，则磁力计航向测量是可选的。 ::: @@ -160,12 +160,12 @@ EKF 具有不同的操作模式，允许不同的传感器测量组合。 - 磁力计读数仅在解锁前影响航向估计，解锁后影响整个姿态。 - 使用此方法时会补偿航向和倾斜误差。 - 不正确的磁场测量会降低倾斜估计的质量。 - - 只要可观测，就会估计磁力计零偏。 + - 磁力计零偏会在可观测时被估计。 1. 磁航向 (Magnetic heading): - 仅修正航向。 倾斜估计永远不会受到不正确磁场测量的影响。 - 使用此方法时，不会修正因没有速度/位置辅助飞行而产生的倾斜误差。 - - 只要可观测，就会估计磁力计零偏。 + - 磁力计零偏会在可观测时被估计。 2. 已弃用 3. 已弃用 4. 已弃用 @@ -173,7 +173,7 @@ EKF 具有不同的操作模式，允许不同的传感器测量组合。 - 永不使用磁力计数据。 当数据完全不可信时（例如：传感器附近有大电流、外部异常），这很有用。 - 估计器将使用其他航向源：[GPS 航向](#yaw-measurements) 或外部视觉。 - - 当使用 GPS 测量而没有其他航向源时，航向只能在充分的水平加速后才能初始化。 + - 当使用 GPS 测量而没有其他航向源时，航向只能在获得足够的水平加速度后才能初始化。 参见下文的 [从载具运动估计偏航](#yaw-from-gps-velocity)。 6. 仅初始化 (Init only): - 磁力计数据仅用于初始化航向估计。 @@ -317,9 +317,9 @@ GSF 应用于各个 3 状态 EKF 输出的权重位于 `weight` 字段中。 - 检查来自每个接收机的 `s_variance_m_s`、`eph` 和 `epv` 数据，并决定可以使用哪些精度指标。 如果两个接收机都输出合理的 `s_variance_m_s` 和 `eph` 数据，并且 GPS 垂直位置未直接用于导航，则建议将 [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) 设置为 3。 如果只有 `eph` 数据可用，且两个接收机都不输出 `s_variance_m_s` 数据，则将 [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) 设置为 2。 - 只有当 GPS 已通过 [EKF2_HGT_REF](../advanced_config/parameter_reference.md#EKF2_HGT_REF) 参数被选为参考高度源，且两个接收机都输出合理的 `epv` 数据时，才会设置第 2 位。 + 只有当 GPS 已通过 [EKF2_HGT_REF](../advanced_config/parameter_reference.md#EKF2_HGT_REF) 参数被选为参考高度源，且两个接收机都输出合理的 `epv` 数据时，第 2 位才会被置位。 - 混合接收机数据的输出记录为 `ekf_gps_position`，可以在连接 nsh 终端时使用命令 `listener ekf_gps_position` 进行检查。 -- 如果接收机以不同的速率输出，则混合输出将采用较慢接收机的速率。 +- 若各接收机输出速率不同，融合后的输出速率将与速率较慢的接收机保持一致。 在可能的情况下，接收机应配置为以相同的速率输出。 #### GNSS 性能要求 @@ -546,11 +546,11 @@ EKF 会考虑视觉位姿估计中的不确定性。 此不确定性信息可以通过 MAVLink [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) 消息中的协方差字段发送，也可以通过参数 [EKF2_EVP_NOISE](../advanced_config/parameter_reference.md#EKF2_EVP_NOISE)、[EKF2_EVV_NOISE](../advanced_config/parameter_reference.md#EKF2_EVV_NOISE) 和 [EKF2_EVA_NOISE](../advanced_config/parameter_reference.md#EKF2_EVA_NOISE) 进行设置。 您可以使用 [EKF2_EV_NOISE_MD](../advanced_config/parameter_reference.md#EKF2_EV_NOISE_MD) 选择不确定性的来源。 -## 如何使用 'ecl' 库 EKF？ +## 如何使用 'ecl' 库中的EKF？ EKF2 默认启用（有关更多信息，请参阅 [切换状态估计器](../advanced/switching_state_estimators.md) 和 [EKF2_EN](../advanced_config/parameter_reference.md#EKF2_EN)）。 -## 如何使用 'ecl' 库 EKF？ +## ecl EKF相较于其他估计器的优缺点是什么？ 像所有估计器一样，大部分性能来自于与传感器特性相匹配的调参。 调参是精度和鲁棒性之间的折衷，虽然我们试图提供满足大多数用户需求的参数，但仍会有需要更改参数的应用。 @@ -624,7 +624,7 @@ covariances\[24\] 的索引映射如下： - \[19 ... 21\] 机体磁场 XYZ \(gauss^2\) - \[22 ... 23\] 风速 NE \(m/s\)^2 -### 观测创新量与创新方差 +### 观测新息与新息方差 观测 `estimator_innovations`、`estimator_innovation_variances` 与 `estimator_innovation_test_ratios` 消息字段定义在 [EstimatorInnovations.msg](https://github.com/PX4/PX4-Autopilot/blob/main/msg/EstimatorInnovations.msg) 中。 这些消息字段名称/类型相同（但单位不同）。 @@ -654,30 +654,30 @@ covariances\[24\] 的索引映射如下： 这些字段基本自说明，下面给出原始定义： ``` -float32[2] gps_hvel # 水平 GPS 速度创新量 (m/sec) 与创新方差 ((m/sec)**2) -float32 gps_vvel # 垂直 GPS 速度创新量 (m/sec) 与创新方差 ((m/sec)**2) -float32[2] gps_hpos # 水平 GPS 位置创新量 (m) 与创新方差 (m**2) -float32 gps_vpos # 垂直 GPS 位置创新量 (m) 与创新方差 (m**2) +float32[2] gps_hvel # 水平 GPS 速度新息 (m/sec) 与新息方差 ((m/sec)**2) +float32 gps_vvel # 垂直 GPS 速度新息 (m/sec) 与新息方差 ((m/sec)**2) +float32[2] gps_hpos # 水平 GPS 位置新息 (m) 与新息方差 (m**2) +float32 gps_vpos # 垂直 GPS 位置新息 (m) 与新息方差 (m**2) # External Vision -float32[2] ev_hvel # 水平外部视觉速度创新量 (m/sec) 与创新方差 ((m/sec)**2) -float32 ev_vvel # 垂直外部视觉速度创新量 (m/sec) 与创新方差 ((m/sec)**2) -float32[2] ev_hpos # 水平外部视觉位置创新量 (m) 与创新方差 (m**2) -float32 ev_vpos # 垂直外部视觉位置创新量 (m) 与创新方差 (m**2) +float32[2] ev_hvel # 水平外部视觉速度新息 (m/sec) 与新息方差 ((m/sec)**2) +float32 ev_vvel # 垂直外部视觉速度新息 (m/sec) 与新息方差 ((m/sec)**2) +float32[2] ev_hpos # 水平外部视觉位置新息 (m) 与新息方差 (m**2) +float32 ev_vpos # 垂直外部视觉位置新息 (m) 与新息方差 (m**2) # Fake Position and Velocity -float32[2] fake_hvel # 虚拟水平速度创新量 (m/s) 与创新方差 ((m/s)**2) -float32 fake_vvel # 虚拟垂直速度创新量 (m/s) 与创新方差 ((m/s)**2) -float32[2] fake_hpos # 虚拟水平位置创新量 (m) 与创新方差 (m**2) -float32 fake_vpos # 虚拟垂直位置创新量 (m) 与创新方差 (m**2) +float32[2] fake_hvel # 虚拟水平速度新息 (m/s) 与新息方差 ((m/s)**2) +float32 fake_vvel # 虚拟垂直速度新息 (m/s) 与新息方差 ((m/s)**2) +float32[2] fake_hpos # 虚拟水平位置新息 (m) 与新息方差 (m**2) +float32 fake_vpos # 虚拟垂直位置新息 (m) 与新息方差 (m**2) # Height sensors -float32 rng_vpos # 测距高度创新量 (m) 与创新方差 (m**2) -float32 baro_vpos # 气压计高度创新量 (m) 与创新方差 (m**2) +float32 rng_vpos # 测距高度新息 (m) 与新息方差 (m**2) +float32 baro_vpos # 气压计高度新息 (m) 与新息方差 (m**2) # Auxiliary velocity -float32[2] aux_hvel # 来自着陆目标测量的水平辅助速度创新量 (m/sec) 与创新方差 ((m/sec)**2) -float32 aux_vvel # 来自着陆目标测量的垂直辅助速度创新量 (m/sec) 与创新方差 ((m/sec)**2) +float32[2] aux_hvel # 来自着陆目标测量的水平辅助速度新息 (m/sec) 与新息方差 ((m/sec)**2) +float32 aux_vvel # 来自着陆目标测量的垂直辅助速度新息 (m/sec) 与新息方差 ((m/sec)**2) ``` ### 输出互补滤波器 @@ -710,17 +710,17 @@ EKF 包含针对严重条件状态和协方差更新的内部错误检查。 这种情况的一个例子是过度振动导致大的垂直位置误差，导致气压计高度测量被拒绝。 这两者都可能导致观测数据被拒绝，如果时间足够长，使得 EKF 尝试重置状态以使用传感器观测数据。 -所有观测都会对创新量进行统计置信度检查。 +所有观测结果均对新息进行了统计置信度检查。 各观测类型的检查标准差数由对应的 `EKF2_*_GATE` 参数控制。 测试指标可在 [EstimatorStatus](https://github.com/PX4/PX4-Autopilot/blob/main/msg/EstimatorStatus.msg) 中查看： -- `mag_test_ratio`：磁力计创新量最大分量与测试限值的比值 -- `vel_test_ratio`：速度创新量最大分量与测试限值的比值 -- `pos_test_ratio`：水平位置创新量最大分量与测试限值的比值 -- `hgt_test_ratio`：垂直位置创新量与测试限值的比值 -- `tas_test_ratio`：真空速创新量与测试限值的比值 -- `hagl_test_ratio`：离地高度创新量与测试限值的比值 +- `mag_test_ratio`：磁力计新息最大分量与测试限值的比值 +- `vel_test_ratio`：速度新息最大分量与测试限值的比值 +- `pos_test_ratio`：水平位置新息最大分量与测试限值的比值 +- `hgt_test_ratio`：垂直位置新息与测试限值的比值 +- `tas_test_ratio`：真空速新息与测试限值的比值 +- `hagl_test_ratio`：离地高度新息与测试限值的比值 若需查看每个传感器的二值通过/失败汇总，请参考 [EstimatorStatus](https://github.com/PX4/PX4-Autopilot/blob/main/msg/EstimatorStatus.msg) 中的 `innovation_check_flags`。 @@ -749,7 +749,7 @@ EKF 对其所有计算使用单精度浮点运算，并使用一阶近似来推 重新调参后，尤其是降低噪声变量的调参，应检查 `estimator_status.gps_check_fail_flags` 是否保持为零。 -## 如果高度估计值发散了怎么办? +## 如何应对高度估计的发散？ 在飞行期间 EKF 高度偏离 GPS 和高度计测量的最常见原因是由振动引起的 IMU 测量的削波和/或混叠。 出现该问题时，通常会在数据中看到以下迹象： @@ -772,7 +772,7 @@ EKF 对其所有计算使用单精度浮点运算，并使用一阶近似来推 注意 这些变化的影响将使 EKF 对 GPS 垂直速度和气压的误差更敏感。 -## 如果位置估计发散了应该怎么办? +## 如何应对位置估计的发散？ 位置发散的最常见原因是： @@ -821,7 +821,7 @@ EKF 对其所有计算使用单精度浮点运算，并使用一阶近似来推 ### 确定过度振动 -高振动通常会影响垂直位置与速度创新量以及水平分量。 +高振动通常会影响垂直位置与速度新息以及水平分量。 磁力计测试级别仅受到很小程度的影响。 \(在此插入示例绘图显示不好振动\) @@ -865,11 +865,11 @@ GPS 数据精度差通常伴随着接收器报告的速度误差的增加以及 GPS 数据丢失会表现为速度与位置创新测试比值“贴平(flat-lining)”。 出现该情况时，请检查 `vehicle_gps_position` 中的其他 GPS 状态数据。 -下图显示了使用 SITL Gazebo 模拟 VTOL 飞行生成的 NED GPS 速度创新量 `ekf2_innovations_0.vel_pos_innov[0 ... 2]`、GPS NE 位置创新量 `ekf2_innovations_0.vel_pos_innov[3 ... 4]` 以及气压垂直位置创新量 `ekf2_innovations_0.vel_pos_innov[5]`。 +下图显示了使用 SITL Gazebo 模拟 VTOL 飞行生成的 NED GPS 速度新息 `ekf2_innovations_0.vel_pos_innov[0 ... 2]`、GPS NE 位置新息 `ekf2_innovations_0.vel_pos_innov[3 ... 4]` 以及气压垂直位置新息 `ekf2_innovations_0.vel_pos_innov[5]`。 模拟的 GPS 在 73 秒时失锁。 -注意 GPS 丢失后 NED 速度创新量与 NE 位置创新量“贴平(flat-line)”。 -注意 GPS 丢失 10 秒后，EKF 会回退到使用最后已知位置的静态位置模式，NE 位置创新量开始再次变化。 +注意 GPS 丢失后 NED 速度新息与 NE 位置新息“贴平(flat-line)”。 +注意 GPS 丢失 10 秒后，EKF 会回退到使用最后已知位置的静态位置模式，NE 位置新息开始再次变化。 ![GPS Data Loss - in SITL](../../assets/ecl/gps_data_loss_-_velocity_innovations.png) diff --git a/docs/zh/assembly/_assembly.md b/docs/zh/assembly/_assembly.md index 42804d30f4..fed4f8f7ed 100644 --- a/docs/zh/assembly/_assembly.md +++ b/docs/zh/assembly/_assembly.md @@ -285,7 +285,7 @@ A particular vehicle might have more/fewer motors and actuators, but the wiring The following sections explain each part in more detail. :::tip -If you're using [DroneCAN ESC](../peripherals/esc_motors.md#dronecan) the control signals will be connected to the CAN BUS instead of the PWM outputs as shown. +If you're using [DroneCAN ESC](../dronecan/escs.md) the control signals will be connected to the CAN BUS instead of the PWM outputs as shown. ::: ### Flight Controller Power @@ -426,7 +426,6 @@ They recommend sensors, power systems, and other components from the same manufa - [Drone Components & Parts](../getting_started/px4_basic_concepts.md#drone-components-parts) (Basic Concepts) - [Payloads](../getting_started/px4_basic_concepts.md#payloads) (Basic Concepts) - [Hardware Selection & Setup](../hardware/drone_parts.md) — information about connecting and configuring specific flight controllers, sensors and other peripherals (e.g. airspeed sensor for planes). - - [Mounting the Flight Controller](../assembly/mount_and_orient_controller.md) - [Vibration Isolation](../assembly/vibration_isolation.md) - [Mounting a Compass](../assembly/mount_gps_compass.md) diff --git a/docs/zh/config_mc/filter_tuning.md b/docs/zh/config_mc/filter_tuning.md index 034fe29163..0b9796bb23 100644 --- a/docs/zh/config_mc/filter_tuning.md +++ b/docs/zh/config_mc/filter_tuning.md @@ -70,7 +70,7 @@ Airframes with more than two frequency noise spikes typically clean the first tw Dynamic notch filters use ESC RPM feedback and/or the onboard FFT analysis. The ESC RPM feedback is used to track the rotor blade pass frequency and its harmonics, while the FFT analysis can be used to track a frequency of another vibration source, such as a fuel engine. -ESC RPM feedback requires ESCs capable of providing RPM feedback such as [DShot](../peripherals/esc_motors.md#dshot) with telemetry connected, a bidirectional DShot set up ([work in progress](https://github.com/PX4/PX4-Autopilot/pull/23863)), or [UAVCAN/DroneCAN ESCs](../dronecan/escs.md). +ESC RPM feedback requires ESCs capable of providing RPM feedback such as [DShot](../peripherals/dshot.md) with telemetry connected, a bidirectional DShot set up ([work in progress](https://github.com/PX4/PX4-Autopilot/pull/23863)), or [UAVCAN/DroneCAN ESCs](../dronecan/escs.md). Before enabling, make sure that the ESC RPM is correct. You might have to adjust the [pole count of the motors](../advanced_config/parameter_reference.md#MOT_POLE_COUNT). diff --git a/docs/zh/dev_setup/building_px4.md b/docs/zh/dev_setup/building_px4.md index 8f53e3c356..ba27d4940a 100644 --- a/docs/zh/dev_setup/building_px4.md +++ b/docs/zh/dev_setup/building_px4.md @@ -145,7 +145,7 @@ make px4_fmu-v5_default - [Pixhawk 1 (FMUv2)](../flight_controller/pixhawk.md): `make px4_fmu-v2_default` :::warning - 您**必须**使用受支持的GCC版本来构建此开发板（例如与[CI/docker](../test_and_ci/docker.md)中使用的相同版本），否则需从构建中移除相关模块。 Building with an unsupported GCC may fail, as PX4 is close to the board's 1MB flash limit. + 您**必须**使用受支持的GCC版本来构建此开发板（例如与[CI/docker](../test_and_ci/docker.md)中使用的相同版本），否则需从构建中移除相关模块。 使用不受支持的GCC进行构建可能会失败，因为PX4接近板载1MB闪存的容量限制。 ::: @@ -174,8 +174,8 @@ Rebooting. ``` :::tip -在 WSL 2 上开发时不支持此操作。（其实也有办法，见 [WSL 2 连接 USB 设备](https://learn.microsoft.com/zh-cn/windows/wsl/connect-usb)）。 -参见[ Windows 开发环境 (WSL2-基于) > Flash控制板](../dev_setup/dev_env_windows_wsl.md#flash-a-flight-control-board)。 +在 WSL2 上开发时不支持此操作。 +参见[ Windows 开发环境 (WSL2-基于) > 烧录主板](../dev_setup/dev_env_windows_wsl.md#flash-a-flight-control-board)。 ::: ## 其他飞控板 diff --git a/docs/zh/dronecan/escs.md b/docs/zh/dronecan/escs.md index 547208bf30..5d40bc879c 100644 --- a/docs/zh/dronecan/escs.md +++ b/docs/zh/dronecan/escs.md @@ -1,7 +1,14 @@ # DroneCAN ESCs PX4 supports DroneCAN compliant ESCs. -For more information, see the following articles for specific hardware/firmware: + +## Supported ESC + +:::info +[Supported ESCs](../peripherals/esc_motors#supported-esc) in _ESCs & Motors_ may include additional devices that are not listed below. +::: + +The following articles have specific hardware/firmware information: - [PX4 Sapog ESC Firmware](sapog.md) - [Holybro Kotleta 20](holybro_kotleta.md) diff --git a/docs/zh/modules/modules_driver.md b/docs/zh/modules/modules_driver.md index 85d89bd634..9d2534d052 100644 --- a/docs/zh/modules/modules_driver.md +++ b/docs/zh/modules/modules_driver.md @@ -15,38 +15,6 @@ - [Rpm Sensor](modules_driver_rpm_sensor.md) - [Transponder](modules_driver_transponder.md) -## MCP23009 - -Source: [drivers/gpio/mcp23009](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/gpio/mcp23009) - -### Usage {#MCP23009_usage} - -``` -MCP23009 [arguments...] - Commands: - start - [-I] Internal I2C bus(es) - [-X] External I2C bus(es) - [-b ] board-specific bus (default=all) (external SPI: n-th bus - (default=1)) - [-f ] bus frequency in kHz - [-q] quiet startup (no message if no device found) - [-a ] I2C address - default: 37 - [-D ] Direction - default: 0 - [-O ] Output - default: 0 - [-P ] Pullups - default: 0 - [-U ] Update Interval [ms] - default: 0 - - stop - - status print status info -``` - ## atxxxx Source: [drivers/osd/atxxxx](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/osd/atxxxx) @@ -749,6 +717,40 @@ lsm303agr [arguments...] status print status info ``` +## mcp230xx + +Source: [lib/drivers/mcp_common](https://github.com/PX4/PX4-Autopilot/tree/main/src/lib/drivers/mcp_common) + +### Usage {#mcp230xx_usage} + +``` +mcp230xx [arguments...] + Commands: + start + [-I] Internal I2C bus(es) + [-X] External I2C bus(es) + [-b ] board-specific bus (default=all) (external SPI: n-th bus + (default=1)) + [-f ] bus frequency in kHz + [-q] quiet startup (no message if no device found) + [-a ] I2C address + default: 39 + [-D ] Direction (1=Input, 0=Output) + default: 0 + [-O ] Output + default: 0 + [-P ] Pullups + default: 0 + [-U ] Update Interval [ms] + default: 0 + [-M ] First minor number + default: 0 + + stop + + status print status info +``` + ## mcp9808 Source: [drivers/temperature_sensor/mcp9808](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/temperature_sensor/mcp9808) @@ -899,8 +901,6 @@ fetching the latest mixing result and write them to PCA9685 at its scheduling ti It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width that can be accepted by most ESCs and servos. -The I2C bus and address can be configured via parameters `PCA9685_EN_BUS` and `PCA9685_I2C_ADDR`, or via command line arguments. - ### 示例 It is typically started with: diff --git a/docs/zh/modules/modules_system.md b/docs/zh/modules/modules_system.md index a1b139322d..b23833af58 100644 --- a/docs/zh/modules/modules_system.md +++ b/docs/zh/modules/modules_system.md @@ -127,6 +127,10 @@ commander [arguments...] check Run preflight checks + safety Change prearm safety state + on|off [on] to activate safety, [off] to deactivate safety and allow + control surface movements + arm [-f] Force arming (do not run preflight checks) diff --git a/docs/zh/msg_docs/BatteryStatus.md b/docs/zh/msg_docs/BatteryStatus.md index addd1ae524..ec2c19c59e 100644 --- a/docs/zh/msg_docs/BatteryStatus.md +++ b/docs/zh/msg_docs/BatteryStatus.md @@ -2,7 +2,7 @@ Battery status -Battery status information for up to 4 battery instances. +Battery status information for up to 3 battery instances. These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. Battery instance information is also logged and streamed in MAVLink telemetry. @@ -11,7 +11,7 @@ Battery instance information is also logged and streamed in MAVLink telemetry. ```c # Battery status # -# Battery status information for up to 4 battery instances. +# Battery status information for up to 3 battery instances. # These are populated from power module and smart battery device drivers, and one battery updated from MAVLink. # Battery instance information is also logged and streamed in MAVLink telemetry. @@ -33,9 +33,9 @@ uint8 cell_count # [-] [@invalid 0] Number of cells uint8 source # [@enum SOURCE] Battery source -uint8 SOURCE_POWER_MODULE = 0 # Power module -uint8 SOURCE_EXTERNAL = 1 # External -uint8 SOURCE_ESCS = 2 # ESCs +uint8 SOURCE_POWER_MODULE = 0 # Power module (analog ADC or I2C power monitor) +uint8 SOURCE_EXTERNAL = 1 # External (MAVLink, CAN, or external driver) +uint8 SOURCE_ESCS = 2 # ESCs (via ESC telemetry) uint8 priority # [-] Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # [mAh] Capacity of the battery when fully charged diff --git a/docs/zh/msg_docs/BatteryStatusV0.md b/docs/zh/msg_docs/BatteryStatusV0.md index 86500d17a6..cd900a1f17 100644 --- a/docs/zh/msg_docs/BatteryStatusV0.md +++ b/docs/zh/msg_docs/BatteryStatusV0.md @@ -32,9 +32,9 @@ uint8 cell_count # [@invalid 0] Number of cells uint8 source # [@enum SOURCE] Battery source -uint8 SOURCE_POWER_MODULE = 0 # Power module -uint8 SOURCE_EXTERNAL = 1 # External -uint8 SOURCE_ESCS = 2 # ESCs +uint8 SOURCE_POWER_MODULE = 0 # Power module (analog ADC or I2C power monitor) +uint8 SOURCE_EXTERNAL = 1 # External (MAVLink, CAN, or external driver) +uint8 SOURCE_ESCS = 2 # ESCs (via ESC telemetry) uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # [mAh] Capacity of the battery when fully charged diff --git a/docs/zh/msg_docs/DeviceInformation.md b/docs/zh/msg_docs/DeviceInformation.md new file mode 100644 index 0000000000..d415461f94 --- /dev/null +++ b/docs/zh/msg_docs/DeviceInformation.md @@ -0,0 +1,45 @@ +# DeviceInformation (UORB message) + +Device information + +Can be used to uniquely associate a device_id from a sensor topic with a physical device using serial number. +as well as tracking of the used firmware versions on the devices. + +[source file](https://github.com/PX4/PX4-Autopilot/blob/main/msg/DeviceInformation.msg) + +```c +# Device information +# +# Can be used to uniquely associate a device_id from a sensor topic with a physical device using serial number. +# as well as tracking of the used firmware versions on the devices. + +uint64 timestamp # time since system start (microseconds) + +uint8 device_type # [@enum DEVICE_TYPE] Type of the device. Matches MAVLink DEVICE_TYPE enum + +uint8 DEVICE_TYPE_GENERIC = 0 # Generic/unknown sensor +uint8 DEVICE_TYPE_AIRSPEED = 1 # Airspeed sensor +uint8 DEVICE_TYPE_ESC = 2 # ESC +uint8 DEVICE_TYPE_SERVO = 3 # Servo +uint8 DEVICE_TYPE_GPS = 4 # GPS +uint8 DEVICE_TYPE_MAGNETOMETER = 5 # Magnetometer +uint8 DEVICE_TYPE_PARACHUTE = 6 # Parachute +uint8 DEVICE_TYPE_RANGEFINDER = 7 # Rangefinder +uint8 DEVICE_TYPE_WINCH = 8 # Winch +uint8 DEVICE_TYPE_BAROMETER = 9 # Barometer +uint8 DEVICE_TYPE_OPTICAL_FLOW = 10 # Optical flow +uint8 DEVICE_TYPE_ACCELEROMETER = 11 # Accelerometer +uint8 DEVICE_TYPE_GYROSCOPE = 12 # Gyroscope +uint8 DEVICE_TYPE_DIFFERENTIAL_PRESSURE = 13 # Differential pressure +uint8 DEVICE_TYPE_BATTERY = 14 # Battery +uint8 DEVICE_TYPE_HYGROMETER = 15 # Hygrometer + +char[32] vendor_name # Name of the device vendor +char[32] model_name # Name of the device model + +uint32 device_id # [-] [@invalid 0 if not available] Unique device ID for the sensor. Does not change between power cycles. +char[24] firmware_version # [-] [@invalid empty if not available] Firmware version. +char[24] hardware_version # [-] [@invalid empty if not available] Hardware version. +char[33] serial_number # [-] [@invalid empty if not available] Device serial number or unique identifier. + +``` diff --git a/docs/zh/msg_docs/EstimatorStatus.md b/docs/zh/msg_docs/EstimatorStatus.md index 9c24221691..aca77484b9 100644 --- a/docs/zh/msg_docs/EstimatorStatus.md +++ b/docs/zh/msg_docs/EstimatorStatus.md @@ -21,6 +21,7 @@ uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail uint8 GPS_CHECK_FAIL_SPOOFED = 10 # 10 : GPS signal is spoofed +uint8 GPS_CHECK_FAIL_JAMMED = 11 # 11 : GPS signal is jammed uint64 control_mode_flags # Bitmask to indicate EKF logic state uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete diff --git a/docs/zh/msg_docs/GpioIn.md b/docs/zh/msg_docs/GpioIn.md index 039ed02851..589e7d7841 100644 --- a/docs/zh/msg_docs/GpioIn.md +++ b/docs/zh/msg_docs/GpioIn.md @@ -6,6 +6,7 @@ GPIO mask and state ```c # GPIO mask and state +uint8 MAX_INSTANCES = 8 uint64 timestamp # time since system start (microseconds) uint32 device_id # Device id diff --git a/docs/zh/msg_docs/GpsDump.md b/docs/zh/msg_docs/GpsDump.md index 1f96901671..03910da906 100644 --- a/docs/zh/msg_docs/GpsDump.md +++ b/docs/zh/msg_docs/GpsDump.md @@ -9,11 +9,15 @@ This message is used to dump the raw gps communication to the log. uint64 timestamp # time since system start (microseconds) +uint8 INSTANCE_MAIN = 0 +uint8 INSTANCE_SECONDARY = 1 + uint8 instance # Instance of GNSS receiver +uint32 device_id uint8 len # length of data, MSB bit set = message to the gps device, # clear = message from the device uint8[79] data # data to write to the log -uint8 ORB_QUEUE_LENGTH = 8 +uint8 ORB_QUEUE_LENGTH = 16 ``` diff --git a/docs/zh/msg_docs/VehicleCommand.md b/docs/zh/msg_docs/VehicleCommand.md index 1b1b3ed658..225f680857 100644 --- a/docs/zh/msg_docs/VehicleCommand.md +++ b/docs/zh/msg_docs/VehicleCommand.md @@ -108,6 +108,7 @@ uint16 VEHICLE_CMD_LOGGING_START = 2510 # Start streaming ULog data. uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # Stop streaming ULog data. uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # Control starting/stopping transmitting data over the high latency link. uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition. +uint16 VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE = 5300 # Command safety on/off. |1 to activate safety, 0 to deactivate safety and allow control surface movements|Unused|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization. uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan. uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment. @@ -187,6 +188,10 @@ int8 ARMING_ACTION_ARM = 1 uint8 GRIPPER_ACTION_RELEASE = 0 uint8 GRIPPER_ACTION_GRAB = 1 +# Used as param1 in DO_SET_SAFETY_SWITCH_STATE command. +uint8 SAFETY_OFF = 0 +uint8 SAFETY_ON = 1 + uint8 ORB_QUEUE_LENGTH = 8 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. diff --git a/docs/zh/msg_docs/index.md b/docs/zh/msg_docs/index.md index 3a4b937196..10c21f35f0 100644 --- a/docs/zh/msg_docs/index.md +++ b/docs/zh/msg_docs/index.md @@ -105,6 +105,7 @@ Graphs showing how these are used [can be found here](../middleware/uorb_graph.m - [DebugKeyValue](DebugKeyValue.md) - [DebugValue](DebugValue.md) - [DebugVect](DebugVect.md) +- [DeviceInformation](DeviceInformation.md) — Device information - [DifferentialPressure](DifferentialPressure.md) — Differential-pressure (airspeed) sensor - [DistanceSensor](DistanceSensor.md) — DISTANCE_SENSOR message data - [DistanceSensorModeChangeRequest](DistanceSensorModeChangeRequest.md) diff --git a/docs/zh/peripherals/dshot.md b/docs/zh/peripherals/dshot.md index a957c031ed..8ea59236a7 100644 --- a/docs/zh/peripherals/dshot.md +++ b/docs/zh/peripherals/dshot.md @@ -11,6 +11,10 @@ DShot is an alternative ESC protocol that has several advantages over [PWM](../p 本章介绍了如何连接和配置 DShot 电调。 +## Supported ESC + +[ESCs & Motors > Supported ESCs](../peripherals/esc_motors#supported-esc) has a list of supported ESC (check "Protocols" column for DShot ESC). + ## Wiring/Connections {#wiring} DShot ESC are wired the same way as [PWM ESCs](pwm_escs_and_servo.md). diff --git a/docs/zh/peripherals/esc_motors.md b/docs/zh/peripherals/esc_motors.md index d91c16a9b1..c372d04192 100644 --- a/docs/zh/peripherals/esc_motors.md +++ b/docs/zh/peripherals/esc_motors.md @@ -9,13 +9,14 @@ PX4 supports a number of [common protocols](../esc/esc_protocols.md) for sending The following list is non-exhaustive. -| ESC Device | Protocols | Firmwares | 备注 | -| ---------------------------- | ------------------------------------ | ------------------------ | ----------------------------------------------------- | -| [ARK 4IN1 ESC] | [Dshot], [PWM] | [AM32] | Has versions with/without connnectors | -| [Holybro Kotleta 20] | [DroneCAN], [PWM] | [PX4 Sapog ESC Firmware] | | -| [Vertiq Motor & ESC modules] | [Dshot], [OneShot], Multishot, [PWM] | Vertiq firmware | Larger modules support DroneCAN, ESC and Motor in one | -| [VESC ESCs] | [DroneCAN], [PWM] | VESC project firmware | | -| [Zubax Telega] | [DroneCAN], [PWM] | Telega-based | ESC and Motor in one | +| ESC Device | Protocols | Firmwares | 备注 | +| ------------------------------ | ------------------------------------ | ------------------------ | ----------------------------------------------------- | +| [ARK 4IN1 ESC] | [Dshot], [PWM] | [AM32] | Has versions with/without connnectors | +| [Holybro Kotleta 20] | [DroneCAN], [PWM] | [PX4 Sapog ESC Firmware] | | +| [Vertiq Motor & ESC modules] | [Dshot], [OneShot], Multishot, [PWM] | Vertiq firmware | Larger modules support DroneCAN, ESC and Motor in one | +| [RaccoonLab CAN PWM ESC nodes] | [DroneCAN], Cyphal | | Cyphal and DroneCAN notes for PWM ESC | +| [VESC ESCs] | [DroneCAN], [PWM] | VESC project firmware | | +| [Zubax Telega] | [DroneCAN], [PWM] | Telega-based | ESC and Motor in one | @@ -29,6 +30,7 @@ The following list is non-exhaustive. [PWM]: ../peripherals/pwm_escs_and_servo.md [Holybro Kotleta 20]: ../dronecan/holybro_kotleta.md [Vertiq Motor & ESC modules]: ../peripherals/vertiq.md +[RaccoonLab CAN PWM ESC nodes]: ../dronecan/raccoonlab_nodes.md [Zubax Telega]: ../dronecan/zubax_telega.md ## 另见 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 29b538401a..5d1fb157b6 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -257,6 +257,8 @@ set(msg_files versioned/LongitudinalControlConfiguration.msg versioned/ManualControlSetpoint.msg versioned/ModeCompleted.msg + versioned/RaptorInput.msg + versioned/RaptorStatus.msg versioned/RegisterExtComponentReply.msg versioned/RegisterExtComponentRequest.msg versioned/TrajectorySetpoint.msg diff --git a/msg/FixedWingRunwayControl.msg b/msg/FixedWingRunwayControl.msg index 62d09a5ebf..f83e109547 100644 --- a/msg/FixedWingRunwayControl.msg +++ b/msg/FixedWingRunwayControl.msg @@ -1,8 +1,15 @@ # Auxiliary control fields for fixed-wing runway takeoff/landing -# Passes information from the FixedWingModeManager to the FixedWingAttitudeController +# Passes information from the FixedWingModeManager to the FixedWingAttitudeController (wheel control) and FixedWingLandDetector (takeoff state) uint64 timestamp # [us] time since system start +uint8 STATE_THROTTLE_RAMP = 0 # ramping up throttle +uint8 STATE_CLAMPED_TO_RUNWAY = 1 # clamped to runway, controlling yaw directly (wheel or rudder) +uint8 STATE_CLIMBOUT = 2 # climbout to safe height before navigation +uint8 STATE_FLYING = 3 # navigate freely + +uint8 runway_takeoff_state # Current state of runway takeoff state machine + bool wheel_steering_enabled # Flag that enables the wheel steering. float32 wheel_steering_nudging_rate # [norm] [@range -1, 1] [FRD] Manual wheel nudging, added to controller output. NAN is interpreted as 0. diff --git a/msg/versioned/RaptorInput.msg b/msg/versioned/RaptorInput.msg new file mode 100644 index 0000000000..4193397137 --- /dev/null +++ b/msg/versioned/RaptorInput.msg @@ -0,0 +1,18 @@ +# Raptor Input + +# The exact inputs to the Raptor foundation policy. +# Having access to the exact inputs helps with debugging and post-hoc analysis. + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # [us] Time since system start +uint64 timestamp_sample # [us] Sampling timestamp of the data this control response is based on +bool active # Signals if the policy is active (aka publishing actuator_motors) +float32[3] position # [m] [@frame FLU] Position of the vehicle_local_position frame +float32[4] orientation # [-] Orientation in the vehicle_attitude frame but using the FLU convention as a unit quaternion (w, x, y, z) +float32[3] linear_velocity # [m/s] [@frame FLU] Linear velocity in the vehicle_local_position frame +float32[3] angular_velocity # [rad/s] [@frame FLU] Angular velocity in the body frame +uint8 ACTION_DIM = 4 # Policy output dimensionality (for quadrotors) +float32[4] previous_action # [@range -1, 1] Previous action. Motor commands normalized to [-1, 1] + +# TOPICS raptor_input diff --git a/msg/versioned/RaptorStatus.msg b/msg/versioned/RaptorStatus.msg new file mode 100644 index 0000000000..25801a0020 --- /dev/null +++ b/msg/versioned/RaptorStatus.msg @@ -0,0 +1,46 @@ +# Raptor Status + +# Diagnostic messages for the Raptor foundation policy. +# This diagnostic data is useful for debugging (e.g. identifying missing input information). + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # [us] Time since system start +uint64 timestamp_sample # [us] Sampling timestamp of the data this control response is based on +bool subscription_update_angular_velocity # [bool] Flag signalling if the vehicle_angular_velocity was updated +bool subscription_update_local_position # [bool] Flag signalling if the vehicle_local_position was updated +bool subscription_update_attitude # [bool] Flag signalling if the vehicle_attitude was updated +bool subscription_update_trajectory_setpoint # [bool] Flag signalling if the trajectory_setpoint was updated +bool subscription_update_vehicle_status # [bool] Flag signalling if the vehicle_status was updated + +uint8 exit_reason # [enum] Exit reason identifier. Representing conditions that lead to the Raptor policy not being executed +uint8 EXIT_REASON_NONE = 0 # No exit reason => Raptor control step was executed (actuator_motors should have been published) +uint8 EXIT_REASON_NO_ANGULAR_VELOCITY_UPDATE = 1 # We synchronize the control onto the input observation with the highest update frequency, which is vehicle_angular_velocity. If there was no update, we do not need to execute the policy again +uint8 EXIT_REASON_NOT_ALL_OBSERVATIONS_SET = 2 # We can not execute the policy if not all observations are available +uint8 EXIT_REASON_ANGULAR_VELOCITY_STALE = 3 # If OBSERVATION_TIMEOUT_ANGULAR_VELOCITY is exceeded, we treat the vehicle_angular_velocity as stale and can not run the policy +uint8 EXIT_REASON_LOCAL_POSITION_STALE = 4 # If OBSERVATION_TIMEOUT_LOCAL_POSITION is exceeded, we treat the vehicle_local_position as stale and can not run the policy +uint8 EXIT_REASON_ATTITUDE_STALE = 5 # If OBSERVATION_TIMEOUT_ATTITUDE is exceeded, we treat the vehicle_attitude as stale and can not run the policy +uint8 EXIT_REASON_EXECUTOR_STATUS_SOURCE_NOT_CONTROL = 6 # The executor that runs the policy can run in oversampling mode, where it decides if the policy should be ran based on the timestamp and not based on fixed synchronization onto the vehicle_angular_velocity. In this case the executor can decide to skip running the policy if the interval is too small, in which case this flag is set. + +uint32 timestamp_last_vehicle_angular_velocity # [us] Timestamp of the last received vehicle_angular_velocity message +uint32 timestamp_last_vehicle_local_position # [us] Timestamp of the last received vehicle_local_position message +uint32 timestamp_last_vehicle_attitude # [us] Timestamp of the last received vehicle_attitude message +uint32 timestamp_last_trajectory_setpoint # [us] Timestamp of the last received trajectory_setpoint message + +bool vehicle_angular_velocity_stale # [bool] True if vehicle_angular_velocity data is considered stale (exceeded timeout) +bool vehicle_local_position_stale # [bool] True if vehicle_local_position data is considered stale (exceeded timeout) +bool vehicle_attitude_stale # [bool] True if vehicle_attitude data is considered stale (exceeded timeout) +bool trajectory_setpoint_stale # [bool] True if trajectory_setpoint data is considered stale (exceeded timeout) + +bool active # [bool] True if the Raptor policy is currently active (publishing actuator_motors) +uint8 substep # [-] The policy is trained at a fixed frequency (e.g. 100 Hz) but we might want to use it for control at higher frequencies (e.g. 400 Hz), which leads to a number of intermediate steps before the actual policy state is advanced (in this case 4 = 400 Hz / 100 Hz). This field provides the current substep (e.g. 0-3). +float32 control_interval # [s] Time interval between control updates + +float32 trajectory_setpoint_dt_mean # [us] The average trajectory setpoint arrival time interval (since Raptor mode activation within NUM_TRAJECTORY_SETPOINT_DTS received trajectory_setpoint messages) +float32 trajectory_setpoint_dt_max # [us] The max trajectory setpoint arrival time interval (since Raptor mode activation and within NUM_TRAJECTORY_SETPOINT_DTS received trajectory_setpoint messages) +float32 trajectory_setpoint_dt_max_since_activation # [us] The max trajectory setpoint arrival time interval (since Raptor mode activation) + +float32[3] internal_reference_position # [m] [@frame FLU] Internal reference position +float32[3] internal_reference_linear_velocity # [m/s] [@frame FLU] Internal reference linear velocity + +# TOPICS raptor_status diff --git a/msg/versioned/VehicleCommandAck.msg b/msg/versioned/VehicleCommandAck.msg index c11a5f8c5d..c3f679bfbc 100644 --- a/msg/versioned/VehicleCommandAck.msg +++ b/msg/versioned/VehicleCommandAck.msg @@ -23,7 +23,7 @@ uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 -uint8 ORB_QUEUE_LENGTH = 4 +uint8 ORB_QUEUE_LENGTH = 8 uint32 command # Command that is being acknowledged uint8 result # Command result diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 61dbb2f35c..bcd663dd05 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -184,6 +184,7 @@ #define DRV_IMU_DEVTYPE_UAVCAN 0x87 #define DRV_MAG_DEVTYPE_UAVCAN 0x88 #define DRV_DIST_DEVTYPE_UAVCAN 0x89 +#define DRV_HYGRO_DEVTYPE_UAVCAN 0x8A #define DRV_ADC_DEVTYPE_ADS1115 0x90 @@ -266,6 +267,7 @@ #define DRV_TEMP_DEVTYPE_MCP9808 0xEE +#define DRV_TEMP_DEVTYPE_TMP102 0xF0 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 38a1d9d4e6..754ab2051e 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 38a1d9d4e6629ca2411b9b0e8cbe6d5fde85fc54 +Subproject commit 754ab2051e135046be6265f9a3c8150040c3af95 diff --git a/src/drivers/temperature_sensor/tmp102/CMakeLists.txt b/src/drivers/temperature_sensor/tmp102/CMakeLists.txt new file mode 100644 index 0000000000..39e23322ba --- /dev/null +++ b/src/drivers/temperature_sensor/tmp102/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2026 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_module( + MODULE drivers__temperature_sensor__tmp102 + MAIN tmp102 + COMPILE_FLAGS + SRCS + tmp102_main.cpp + tmp102.cpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/temperature_sensor/tmp102/Kconfig b/src/drivers/temperature_sensor/tmp102/Kconfig new file mode 100644 index 0000000000..977a82929b --- /dev/null +++ b/src/drivers/temperature_sensor/tmp102/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_TEMPERATURE_SENSOR_TMP102 + bool "TMP102 temperature sensor" + default n + ---help--- + Enable support for the TMP102 temperature sensor diff --git a/src/drivers/temperature_sensor/tmp102/module.yaml b/src/drivers/temperature_sensor/tmp102/module.yaml new file mode 100644 index 0000000000..1d159deaa7 --- /dev/null +++ b/src/drivers/temperature_sensor/tmp102/module.yaml @@ -0,0 +1,15 @@ +__max_num_config_instances: &max_num_config_instances 1 + +module_name: TMP102 + +parameters: + - group: Sensors + definitions: + SENS_EN_TMP102: + description: + short: Enable TMP102 + long: | + Enable the driver for the TMP102 temperature sensor + type: boolean + reboot_required: true + default: 0 diff --git a/src/drivers/temperature_sensor/tmp102/tmp102.cpp b/src/drivers/temperature_sensor/tmp102/tmp102.cpp new file mode 100644 index 0000000000..5dc9df27e3 --- /dev/null +++ b/src/drivers/temperature_sensor/tmp102/tmp102.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#include "tmp102.h" + +TMP102::TMP102(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) +{ +} + +TMP102::~TMP102() +{ + ScheduleClear(); + perf_free(_cycle_perf); + perf_free(_comms_errors); +} + +void TMP102::RunImpl() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + float temperature = read_temperature(); + + if (std::isnan(temperature)) { + perf_count(_comms_errors); + + } else { + sensor_temp_s _sensor_temp{}; + _sensor_temp.timestamp = hrt_absolute_time(); + _sensor_temp.temperature = temperature; + _sensor_temp.device_id = get_device_id(); + _sensor_temp_pub.publish(_sensor_temp); + } + + perf_end(_cycle_perf); +} + +int TMP102::probe() +{ + uint16_t conf_reg; + + for (int i = 0; i < 3; i++) { + if (read_reg(TMP102_CONFIG_REG, conf_reg) == PX4_OK && (conf_reg | 0x0020) == DEFAULT_CONFIG_REG) { // Mask the AL bit + return PX4_OK; + } + + px4_sleep(1); + } + + return PX4_ERROR; +} + +int TMP102::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + PX4_ERR("TMP102, I2C init failed"); + return ret; + } + + _sensor_temp_pub.advertise(); + ScheduleOnInterval(250_ms); // DEFAULT SAMPLE RATE IS 4HZ => 250ms INTERVAL + return PX4_OK; +} + +float TMP102::read_temperature() +{ + uint16_t tmp_data; + + if (read_reg(TMP102_TEMP_REG, tmp_data) != PX4_OK) { + return NAN; + } + + float temperature = ((int16_t)(tmp_data) >> 4) * 0.0625f; + return temperature; +} + +int TMP102::read_reg(uint8_t reg, uint16_t &data) +{ + uint8_t tmp_data[2]; + tmp_data[0] = reg; + + if (transfer(tmp_data, 1, nullptr, 0) != PX4_OK) { // Set the PR to the desired register + return PX4_ERROR; + } + + int ret = transfer(nullptr, 0, tmp_data, 2); // Read the data from the desired register + data = (tmp_data[0] << 8) | tmp_data[1]; + return ret; +} diff --git a/src/drivers/temperature_sensor/tmp102/tmp102.h b/src/drivers/temperature_sensor/tmp102/tmp102.h new file mode 100644 index 0000000000..e0265867a6 --- /dev/null +++ b/src/drivers/temperature_sensor/tmp102/tmp102.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +#define TMP102_TEMP_REG 0x00 +#define TMP102_CONFIG_REG 0x01 +#define TMP102_TLOW_REG 0x02 +#define TMP102_THIGH_REG 0x03 + +constexpr uint16_t DEFAULT_CONFIG_REG = 0x60A0; // 12-bit resolution, comparator mode, active low, 4Hz + +class TMP102 : public device::I2C, public I2CSPIDriver +{ +public: + TMP102(const I2CSPIDriverConfig &config); + ~TMP102() override; + + int init() override; + int probe() override; + void RunImpl(); + static void print_usage(); + +protected: + void print_status(); + +private: + uORB::PublicationMulti _sensor_temp_pub{ORB_ID(sensor_temp)}; + perf_counter_t _cycle_perf; + perf_counter_t _comms_errors; + + float read_temperature(); + int read_reg(uint8_t address, uint16_t &data); +}; diff --git a/src/drivers/temperature_sensor/tmp102/tmp102_main.cpp b/src/drivers/temperature_sensor/tmp102/tmp102_main.cpp new file mode 100644 index 0000000000..22ea03bcbf --- /dev/null +++ b/src/drivers/temperature_sensor/tmp102/tmp102_main.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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 tmp102_main.cpp + * @author Philipp Engljaehringer + * + * Driver for the TMP102 Temperature Sensor connected via I2C. + */ + +#include "tmp102.h" +#include + +void TMP102::print_usage() +{ + PRINT_MODULE_USAGE_NAME("tmp102", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x48); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +void TMP102::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_cycle_perf); + perf_print_counter(_comms_errors); +} + +extern "C" int tmp102_main(int argc, char *argv[]) +{ + using ThisDriver = TMP102; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 400000; + cli.i2c_address = 0x48; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_TEMP_DEVTYPE_TMP102); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index f39d589bf5..33a955be8e 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -43,8 +43,6 @@ #include #include -#define MOTOR_BIT(x) (1<<(x)) - using namespace time_literals; UavcanEscController::UavcanEscController(uavcan::INode &node) : diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 90e8f5f5cc..1170e67574 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -47,13 +47,9 @@ #include #include #include -#include #include -#include #include -#include -#include -#include +#include "../node_info.hpp" class UavcanEscController { diff --git a/src/drivers/uavcan/arming_status.cpp b/src/drivers/uavcan/arming_status.cpp index 21b1c7f3f4..80ce14065d 100644 --- a/src/drivers/uavcan/arming_status.cpp +++ b/src/drivers/uavcan/arming_status.cpp @@ -66,10 +66,9 @@ void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &) if (_actuator_armed_sub.update(&actuator_armed)) { uavcan::equipment::safety::ArmingStatus cmd; - if (actuator_armed.lockdown || actuator_armed.kill) { - cmd.status = cmd.STATUS_DISARMED; + bool lockdown_active = actuator_armed.lockdown || actuator_armed.termination || actuator_armed.kill; - } else if (actuator_armed.armed) { + if (!lockdown_active && (actuator_armed.armed || _is_actuator_test_running)) { cmd.status = cmd.STATUS_FULLY_ARMED; } else { diff --git a/src/drivers/uavcan/arming_status.hpp b/src/drivers/uavcan/arming_status.hpp index 3c7bdd041b..4cfaac4b56 100644 --- a/src/drivers/uavcan/arming_status.hpp +++ b/src/drivers/uavcan/arming_status.hpp @@ -57,6 +57,8 @@ public: */ int init(); + void setActuatorTestRunning(bool running) {_is_actuator_test_running = running;} + private: /* * Max update rate to avoid exessive bus traffic @@ -80,4 +82,5 @@ private: uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + bool _is_actuator_test_running = false; }; diff --git a/src/drivers/uavcan/sensors/accel.cpp b/src/drivers/uavcan/sensors/accel.cpp index b725b978bd..0efa5c5e6e 100644 --- a/src/drivers/uavcan/sensors/accel.cpp +++ b/src/drivers/uavcan/sensors/accel.cpp @@ -43,7 +43,9 @@ const char *const UavcanAccelBridge::NAME = "accel"; UavcanAccelBridge::UavcanAccelBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) : UavcanSensorBridgeBase("uavcan_accel", ORB_ID(sensor_accel), node_info_publisher), _sub_imu_data(node) -{ } +{ + set_device_type(DRV_ACC_DEVTYPE_UAVCAN); +} int UavcanAccelBridge::init() { @@ -59,7 +61,7 @@ int UavcanAccelBridge::init() void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); const hrt_abstime timestamp_sample = hrt_absolute_time(); @@ -87,13 +89,10 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure(channel->node_id), channel->iface_index); - device_id.devid_s.devtype = DRV_ACC_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); - - channel->h_driver = new PX4Accelerometer(device_id.devid); + channel->h_driver = new PX4Accelerometer(device_id); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index 2ef69eb6b4..51c8a4b899 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -49,7 +49,9 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublis UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro), node_info_publisher), _sub_air_pressure_data(node), _sub_air_temperature_data(node) -{ } +{ + set_device_type(DRV_BARO_DEVTYPE_UAVCAN); +} int UavcanBarometerBridge::init() { @@ -91,7 +93,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const { const hrt_abstime timestamp_sample = hrt_absolute_time(); - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr) { // Something went wrong - no channel to publish on; return @@ -105,23 +107,18 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const return; } - DeviceId device_id{}; - device_id.devid_s.bus = 0; - device_id.devid_s.bus_type = DeviceBusType_UAVCAN; - - device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); + uint32_t device_id = make_uavcan_device_id(msg); // Register barometer capability with NodeInfoPublisher after first successful message if (_node_info_publisher != nullptr) { - _node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id.devid, + _node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id, NodeInfoPublisher::DeviceCapability::BAROMETER); } // publish sensor_baro_s sensor_baro{}; sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = device_id.devid; + sensor_baro.device_id = device_id; sensor_baro.pressure = msg.static_pressure; if (PX4_ISFINITE(_last_temperature_kelvin) && (_last_temperature_kelvin >= 0.f)) { diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp index 7099a55dc4..107547a121 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.cpp +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -49,6 +49,7 @@ UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure), node_info_publisher), _sub_air(node) { + set_device_type(DRV_DIFF_PRESS_DEVTYPE_UAVCAN); } int UavcanDifferentialPressureBridge::init() @@ -68,8 +69,6 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const { const hrt_abstime timestamp_sample = hrt_absolute_time(); - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN; - _device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; float diff_press_pa = msg.differential_pressure; int32_t differential_press_rev = 0; param_get(param_find("SENS_DPRES_REV"), &differential_press_rev); @@ -83,7 +82,7 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const differential_pressure_s report{}; report.timestamp_sample = timestamp_sample; - report.device_id = _device_id.devid; + report.device_id = make_uavcan_device_id(msg); report.differential_pressure_pa = diff_press_pa; report.temperature = temperature_c; report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index cda437970e..001b78566d 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -41,6 +41,7 @@ UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node, NodeInfoPublisher *node_ UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow), node_info_publisher), _sub_flow(node) { + set_device_type(DRV_FLOW_DEVTYPE_UAVCAN); } int @@ -61,13 +62,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure const uint8_t spoofing_state) { sensor_gps_s sensor_gps{}; - sensor_gps.device_id = get_device_id(); + + sensor_gps.device_id = make_uavcan_device_id(msg); // Register GPS capability with NodeInfoPublisher after first successful message if (_node_info_publisher != nullptr) { diff --git a/src/drivers/uavcan/sensors/gnss_relative.cpp b/src/drivers/uavcan/sensors/gnss_relative.cpp index b2d3960404..8cb5bf344e 100644 --- a/src/drivers/uavcan/sensors/gnss_relative.cpp +++ b/src/drivers/uavcan/sensors/gnss_relative.cpp @@ -42,6 +42,7 @@ UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node, NodeInfo UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative), node_info_publisher), _sub_rel_pos_heading(node) { + set_device_type(DRV_GPS_DEVTYPE_UAVCAN); } int @@ -70,7 +71,7 @@ void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const sensor_gnss_relative.position_length = msg.relative_distance_m; sensor_gnss_relative.position[2] = msg.relative_down_pos_m; - sensor_gnss_relative.device_id = get_device_id(); + sensor_gnss_relative.device_id = make_uavcan_device_id(msg); // Register GPS capability with NodeInfoPublisher after first successful message if (_node_info_publisher != nullptr) { diff --git a/src/drivers/uavcan/sensors/gyro.cpp b/src/drivers/uavcan/sensors/gyro.cpp index 2f9ab4a9f5..ab72693866 100644 --- a/src/drivers/uavcan/sensors/gyro.cpp +++ b/src/drivers/uavcan/sensors/gyro.cpp @@ -43,7 +43,9 @@ const char *const UavcanGyroBridge::NAME = "gyro"; UavcanGyroBridge::UavcanGyroBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) : UavcanSensorBridgeBase("uavcan_gyro", ORB_ID(sensor_gyro), node_info_publisher), _sub_imu_data(node) -{ } +{ + set_device_type(DRV_GYR_DEVTYPE_UAVCAN); +} int UavcanGyroBridge::init() { @@ -59,7 +61,7 @@ int UavcanGyroBridge::init() void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr) { // Something went wrong - no channel to publish on; return @@ -87,13 +89,10 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure(channel->node_id), channel->iface_index); - device_id.devid_s.devtype = DRV_GYR_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); - - channel->h_driver = new PX4Gyroscope(device_id.devid); + channel->h_driver = new PX4Gyroscope(device_id); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/hygrometer.cpp b/src/drivers/uavcan/sensors/hygrometer.cpp index d19b286e94..d9975b226e 100755 --- a/src/drivers/uavcan/sensors/hygrometer.cpp +++ b/src/drivers/uavcan/sensors/hygrometer.cpp @@ -42,6 +42,7 @@ UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node, NodeInfoPubl UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer), node_info_publisher), _sub_hygro(node) { + set_device_type(DRV_HYGRO_DEVTYPE_UAVCAN); } int UavcanHygrometerBridge::init() @@ -64,7 +65,7 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr) { // Something went wrong - no channel to publish on; return @@ -104,7 +105,7 @@ void UavcanMagnetometerBridge::mag2_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr || channel->instance < 0) { // Something went wrong - no channel to publish on; return @@ -134,13 +135,10 @@ UavcanMagnetometerBridge::mag2_sub_cb(const int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) { - // update device id as we now know our device node_id - DeviceId device_id{_device_id}; + // Build device ID using node_id and interface index + uint32_t device_id = make_uavcan_device_id(static_cast(channel->node_id), channel->iface_index); - device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); - - channel->h_driver = new PX4Magnetometer(device_id.devid, ROTATION_NONE); + channel->h_driver = new PX4Magnetometer(device_id, ROTATION_NONE); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp index 1222aad1ea..b476c66947 100644 --- a/src/drivers/uavcan/sensors/rangefinder.cpp +++ b/src/drivers/uavcan/sensors/rangefinder.cpp @@ -45,7 +45,9 @@ const char *const UavcanRangefinderBridge::NAME = "rangefinder"; UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) : UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor), node_info_publisher), _sub_range_data(node) -{ } +{ + set_device_type(DRV_DIST_DEVTYPE_UAVCAN); +} int UavcanRangefinderBridge::init() { @@ -66,7 +68,7 @@ int UavcanRangefinderBridge::init() void UavcanRangefinderBridge::range_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr || channel->instance < 0) { // Something went wrong - no channel to publish on; return @@ -125,13 +127,10 @@ void UavcanRangefinderBridge::range_sub_cb(const int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel) { - // update device id as we now know our device node_id - DeviceId device_id{_device_id}; + // Build device ID using node_id and interface index + uint32_t device_id = make_uavcan_device_id(static_cast(channel->node_id), channel->iface_index); - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); - - channel->h_driver = new PX4Rangefinder(device_id.devid, distance_sensor_s::ROTATION_DOWNWARD_FACING); + channel->h_driver = new PX4Rangefinder(device_id, distance_sensor_s::ROTATION_DOWNWARD_FACING); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index ec9f7c9131..350dd9a625 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -316,7 +316,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) (void)orb_publish(_orb_topic, channel->orb_advert, report); } -uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id) +uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id, uint8_t iface_index) { uavcan_bridge::Channel *channel = nullptr; @@ -354,6 +354,7 @@ uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id // initialize the driver, which registers the class device name and uORB publisher channel->node_id = node_id; + channel->iface_index = iface_index; int ret = init_driver(channel); if (ret != PX4_OK) { diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index 9c59b816dc..e8fb1a124b 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -92,6 +92,7 @@ struct Channel { orb_advert_t orb_advert{nullptr}; int instance{-1}; void *h_driver{nullptr}; + uint8_t iface_index{0}; }; } // namespace uavcan_bridge @@ -138,7 +139,34 @@ protected: */ virtual int init_driver(uavcan_bridge::Channel *channel) { return PX4_OK; }; - uavcan_bridge::Channel *get_channel_for_node(int node_id); + uavcan_bridge::Channel *get_channel_for_node(int node_id, uint8_t iface_index); + + /** + * Builds a unique device ID from a UAVCAN message + * @param msg UAVCAN message (must have getSrcNodeID() and getIfaceIndex() methods) + * @return Complete device ID with node address and interface encoded + */ + template + uint32_t make_uavcan_device_id(const uavcan::ReceivedDataStructure &msg) const + { + return make_uavcan_device_id(msg.getSrcNodeID().get(), msg.getIfaceIndex()); + } + + /** + * Builds a unique device ID from node ID and interface index + * @param node_id UAVCAN node ID + * @param iface_index CAN interface index (0 = CAN1, 1 = CAN2, etc.) + * @return Complete device ID with node address and interface encoded + */ + uint32_t make_uavcan_device_id(uint8_t node_id, uint8_t iface_index) const + { + device::Device::DeviceId device_id{}; + device_id.devid_s.devtype = get_device_type(); + device_id.devid_s.address = node_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType_UAVCAN; + device_id.devid_s.bus = iface_index; + return device_id.devid; + } public: virtual ~UavcanSensorBridgeBase(); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 5f9b55438b..742682cf4d 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -969,6 +969,10 @@ UavcanNode::Run() } } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) + _arming_status_controller.setActuatorTestRunning(_mixing_interface_esc.isActuatorTestRunning()); +#endif + perf_end(_cycle_perf); pthread_mutex_unlock(&_node_mutex); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index ab49dfe50f..56f7c445fe 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -136,6 +136,8 @@ public: MixingOutput &mixingOutput() { return _mixing_output; } + bool isActuatorTestRunning() const { return _mixing_output.isActuatorTestRunning(); } + protected: void Run() override; private: diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f732ddbef1..33d58ab640 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -68,6 +68,7 @@ add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) +add_subdirectory(rl_tools EXCLUDE_FROM_ALL) add_subdirectory(rover_control EXCLUDE_FROM_ALL) add_subdirectory(rtl EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index 2ea654fba2..572b7b5fd5 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -71,5 +71,4 @@ public: private: uORB::PublicationMultiData _distance_sensor_pub{ORB_ID(distance_sensor)}; - hrt_abstime _q_update_now {}; }; diff --git a/src/lib/matrix/test/test_data.py b/src/lib/matrix/test/test_data.py index 43bc03e324..95d18a3fb8 100644 --- a/src/lib/matrix/test/test_data.py +++ b/src/lib/matrix/test/test_data.py @@ -1,7 +1,9 @@ -from __future__ import print_function +#!/usr/bin/env python3 + from pylab import * from pprint import pprint import scipy.linalg +import sys # test cases, derived from doc/nasa_rotation_def.pdf @@ -47,7 +49,7 @@ def quat_prod(q, r): def dcm_to_euler(dcm): return array([ arctan(dcm[2,1]/ dcm[2,2]), - arctan(-dcm[2,0]/ std::sqrt(1 - dcm[2,0]**2)), + arctan(-dcm[2,0]/ (1 - dcm[2,0]**2)**0.5), arctan(dcm[1,0]/ dcm[0,0]), ]) @@ -75,6 +77,7 @@ psi = 0.3 print('euler', phi, theta, psi) q = euler_to_quat(phi, theta, psi) +FLT_EPSILON = sys.float_info.epsilon assert(abs(norm(q) - 1) < FLT_EPSILON) assert(abs(norm(q) - 1) < FLT_EPSILON) assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index fe1b818f86..eb67188018 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -545,8 +545,10 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const float output = _disarmed_value[i]; - if (_function_assignment[i] >= OutputFunction::Servo1 - && _function_assignment[i] <= OutputFunction::ServoMax + if (((_function_assignment[i] >= OutputFunction::Servo1 + && _function_assignment[i] <= OutputFunction::ServoMax) || _function_assignment[i] == OutputFunction::Landing_Gear_Wheel + || (_function_assignment[i] >= OutputFunction::Gimbal_Roll + && _function_assignment[i] <= OutputFunction::Gimbal_Yaw)) && _param_handles[i].center != PARAM_INVALID && _center_value[i] >= 800 && _center_value[i] <= 2200) { diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 4fdc0f2904..3e4cec0d51 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -163,6 +163,7 @@ public: void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us); const actuator_armed_s &armed() const { return _armed; } + bool isActuatorTestRunning() const { return _actuator_test.inTestMode(); } void setAllFailsafeValues(uint16_t value); void setAllDisarmedValues(uint16_t value); diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 560d6eec82..772a5c2a9e 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -438,7 +438,7 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } if (user_config.store(param, new_value)) { - params_unsaved.set(param, !mark_saved); + params_unsaved.set(param, !mark_saved && param_changed); result = PX4_OK; } else { diff --git a/src/lib/rl_tools/CMakeLists.txt b/src/lib/rl_tools/CMakeLists.txt new file mode 100644 index 0000000000..83402f2c45 --- /dev/null +++ b/src/lib/rl_tools/CMakeLists.txt @@ -0,0 +1,15 @@ +if(CONFIG_LIB_RL_TOOLS) + px4_add_git_submodule(TARGET git_rl_tools PATH "rl_tools") + add_library(rl_tools INTERFACE) + target_include_directories(rl_tools INTERFACE + ${CMAKE_CURRENT_SOURCE_DIR}/rl_tools/include + ) + + target_compile_features(rl_tools INTERFACE cxx_std_17) + + target_compile_options(rl_tools INTERFACE + -Wno-unused-parameter + -Wno-unused-variable + -Wno-unused-local-typedefs + ) +endif() diff --git a/src/lib/rl_tools/Kconfig b/src/lib/rl_tools/Kconfig new file mode 100644 index 0000000000..8014b02bcf --- /dev/null +++ b/src/lib/rl_tools/Kconfig @@ -0,0 +1,7 @@ +config LIB_RL_TOOLS + bool "RLtools" + default n + ---help--- + RLtools is a header-only library for reinforcement learning and neural networks. + It enables running trained RL policies on embedded devices. + This library is used for running RL-based controllers on the PX4 autopilot. diff --git a/src/lib/rl_tools/rl_tools b/src/lib/rl_tools/rl_tools new file mode 160000 index 0000000000..15940da2a8 --- /dev/null +++ b/src/lib/rl_tools/rl_tools @@ -0,0 +1 @@ +Subproject commit 15940da2a8334d7532c4a5650ee4e09526206414 diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0f3a97de4b..eb3c2109e4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -607,13 +607,14 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2); /** * Manual control loss exceptions * - * Specify modes where manual control loss is ignored and no failsafe is triggered. + * Specify modes in which stick input is ignored and no failsafe action is triggered. * External modes requiring stick input will still failsafe. + * Auto modes are: Hold, Takeoff, Land, RTL, Descend, Follow Target, Precland, Orbit. * * @min 0 * @max 31 * @bit 0 Mission - * @bit 1 Hold + * @bit 1 Auto modes * @bit 2 Offboard * @bit 3 External Mode * @bit 4 Altitude Cruise @@ -624,13 +625,16 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); /** * Datalink loss exceptions * - * Specify modes in which datalink loss is ignored and the failsafe action not triggered. + * Specify modes in which ground control station connection loss is ignored and no failsafe action is triggered. + * See also COM_RCL_EXCEPT. * * @min 0 - * @max 7 + * @max 31 * @bit 0 Mission - * @bit 1 Hold + * @bit 1 Auto modes * @bit 2 Offboard + * @bit 3 External Mode + * @bit 4 Altitude Cruise * @group Commander */ PARAM_DEFINE_INT32(COM_DLL_EXCEPT, 0); diff --git a/src/modules/commander/failsafe/emscripten_template.html b/src/modules/commander/failsafe/emscripten_template.html index 1b21012de6..0e90830ee2 100644 --- a/src/modules/commander/failsafe/emscripten_template.html +++ b/src/modules/commander/failsafe/emscripten_template.html @@ -182,21 +182,23 @@ diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index e366e12947..5090b22a9c 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -442,18 +442,53 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par return options; } +bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter) +{ + switch (user_intended_mode) { + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + return exception_mask_parameter & (int)LinkLossExceptionBits::Mission; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_ORBIT: + return exception_mask_parameter & (int)LinkLossExceptionBits::AutoModes; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + return exception_mask_parameter & (int)LinkLossExceptionBits::Offboard; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: + return exception_mask_parameter & (int)LinkLossExceptionBits::ExternalMode; + + case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE: + return exception_mask_parameter & (int)LinkLossExceptionBits::AltitudeCruise; + + default: + return false; + } +} + void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const failsafe_flags_s &status_flags) { updateArmingState(time_us, state.armed, status_flags); - const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - || state.vtol_in_transition_mode; - // Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter // altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established - const bool ignore_any_link_loss_vtol_takeoff_fixedwing = state.user_intended_mode == - vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF + const bool in_forward_flight = (state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || state.vtol_in_transition_mode; + const bool ignore_any_link_loss_vtol_takeoff_fixedwing = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) && in_forward_flight && !state.mission_finished; // Manual control (RC or joystick) loss @@ -462,59 +497,23 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, _manual_control_lost_at_arming = false; } - const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission); - const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold); - const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard); - const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold); - const bool rc_loss_ignored_altitude_cruise = (state.user_intended_mode == - vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AltitudeCruise)); - - const bool rc_loss_ignored_external_mode = - (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL2 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL3 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL4 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL5 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL6 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL7 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL8) - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::ExternalMode); - - const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || - rc_loss_ignored_takeoff || rc_loss_ignored_external_mode || ignore_any_link_loss_vtol_takeoff_fixedwing - || _manual_control_lost_at_arming || rc_loss_ignored_altitude_cruise; + const bool rc_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get()) + || ignore_any_link_loss_vtol_takeoff_fixedwing || _manual_control_lost_at_arming; if (_param_com_rc_in_mode.get() != int32_t(RcInMode::DisableManualControl) && !rc_loss_ignored) { CHECK_FAILSAFE(status_flags, manual_control_signal_lost, fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss)); } - // GCS connection loss + // Ground control station connection loss const bool dll_loss_ignored_land = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; - const bool dll_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION - && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Mission); - const bool dll_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER - && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold); - const bool dll_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD - && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Offboard); - const bool dll_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) - && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold); - - const bool dll_loss_ignored = dll_loss_ignored_mission || dll_loss_ignored_loiter || dll_loss_ignored_offboard || - dll_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land; + const bool dll_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_dll_except.get()) + || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land; if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !dll_loss_ignored) { - CHECK_FAILSAFE(status_flags, gcs_connection_lost, - fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss)); + CHECK_FAILSAFE(status_flags, gcs_connection_lost, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss)); } // VTOL transition failure (quadchute) @@ -531,7 +530,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, // If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished, // trigger RTL to avoid losing the vehicle - if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl) || rc_loss_ignored_mission) + if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl) + || isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get())) && _param_nav_dll_act.get() == int32_t(gcs_connection_loss_failsafe_mode::Disabled) && state.mission_finished) { _last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost, diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 9a9384f621..d5b3f0a79e 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -53,20 +53,14 @@ protected: private: void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags); - enum class ManualControlLossExceptionBits : int32_t { + enum class LinkLossExceptionBits : int32_t { Mission = (1 << 0), - Hold = (1 << 1), + AutoModes = (1 << 1), Offboard = (1 << 2), ExternalMode = (1 << 3), AltitudeCruise = (1 << 4) }; - enum class DatalinkLossExceptionBits : int32_t { - Mission = (1 << 0), - Hold = (1 << 1), - Offboard = (1 << 2) - }; - // COM_LOW_BAT_ACT parameter values enum class LowBatteryAction : int32_t { Warning = 0, // Warning @@ -175,6 +169,8 @@ private: static ActionOptions fromPosLowActParam(int param_value); static ActionOptions fromRemainingFlightTimeLowActParam(int param_value); + static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter); + const int _caller_id_mode_fallback{genCallerId()}; bool _last_state_mode_fallback{false}; const int _caller_id_mission_control_lost{genCallerId()}; diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index 18b927409f..2c190c660c 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -35,6 +35,7 @@ #include "framework.h" #include +#include "../ModeUtil/mode_requirements.hpp" // to run: make tests TESTFILTER=failsafe_test @@ -50,16 +51,16 @@ protected: void checkStateAndMode(const hrt_abstime &time_us, const State &state, const failsafe_flags_s &status_flags) override { - CHECK_FAILSAFE(status_flags, manual_control_signal_lost, - ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm)); + CHECK_FAILSAFE(status_flags, manual_control_signal_lost, ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm)); CHECK_FAILSAFE(status_flags, gcs_connection_lost, Action::Descend); if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { CHECK_FAILSAFE(status_flags, mission_failure, Action::Descend); } - CHECK_FAILSAFE(status_flags, wind_limit_exceeded, - ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never)); + CHECK_FAILSAFE(status_flags, wind_limit_exceeded, ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never)); + CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::RemainingFlightTimeLow)); + CHECK_FAILSAFE(status_flags, offboard_control_signal_lost, ActionOptions(Action::Hold)); _last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure && status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred()); @@ -258,6 +259,77 @@ TEST_F(FailsafeTest, takeover_denied) ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate); } +TEST_F(FailsafeTest, can_takeover_degraded_failsafe) +{ + FailsafeTester failsafe(nullptr); + + FailsafeBase::State state{}; + state.armed = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_MANUAL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 3847124342; + failsafe_flags_s failsafe_flags{}; + mode_util::getModeRequirements(state.vehicle_type, failsafe_flags); // Load mode requirements to degrade without valid position estimate + bool user_intended_mode_updated = false; + + uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + + // Battery time low -> Hold for the delay + time += 10_ms; + failsafe_flags.battery_low_remaining_time = true; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold); + + // Delay over -> RTL + time += 5_s; + failsafe_flags.battery_low_remaining_time = true; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL); + + // Global position gets invalid -> Land + time += 10_ms; + failsafe_flags.global_position_invalid = true; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Land); + + // User wants takeover -> Altitude mode + Warning + time += 10_ms; + user_intended_mode_updated = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn); + ASSERT_TRUE(failsafe.userTakeoverActive()); +} + +TEST_F(FailsafeTest, no_immediate_takeover_when_failsafe_on_mode_switch) +{ + FailsafeTester failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + FailsafeBase::State state{}; + state.armed = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 3847124342; + bool user_intended_mode_updated = false; + + uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + + // Switch to offboard but no offboard signal -> No immediate user takeover flagged but rather Hold + time += 10_ms; + user_intended_mode_updated = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + failsafe_flags.offboard_control_signal_lost = true; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold); + ASSERT_FALSE(failsafe.userTakeoverActive()); +} + TEST_F(FailsafeTest, defer) { FailsafeTester failsafe(nullptr); diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index ab3e30c847..19528b59e9 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -499,9 +499,9 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; } - // User takeover is activated on user intented mode update (w/o action change, so takeover is not immediately - // requested when entering failsafe) or rc stick movements - bool want_user_takeover_mode_switch = user_intended_mode_updated && _selected_action == selected_action; + // User takeover interrupting a failsafe is triggered by a change of the user-intended mode + // (only if a failsafe action is already active otherwise there can be immediate takeover when entering a failsafe) or by stick movement + bool want_user_takeover_mode_switch = user_intended_mode_updated && (_selected_action > Action::Warn); bool want_user_takeover = want_user_takeover_mode_switch || rc_sticks_takeover_request; bool takeover_allowed = (allow_user_takeover == UserTakeoverAllowed::Always && (_user_takeover_active || want_user_takeover)) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index f6e216b855..093185e240 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -59,7 +59,7 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul _param_handles[i].scale_spoiler = param_find(buffer); } - _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate); + _flaps_setpoint_with_slewrate.setSlewRate(_param_ca_flap_slew.get()); _spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate); _count_handle = param_find("CA_SV_CS_COUNT"); @@ -75,6 +75,9 @@ void ActuatorEffectivenessControlSurfaces::updateParams() return; } + // Update flap slewrates + _flaps_setpoint_with_slewrate.setSlewRate(_param_ca_flap_slew.get()); + // Helper to check if a PWM center parameter is enabled, and clamp it to valid range auto check_pwm_center = [](const char *prefix, int channel) -> bool { char param_name[20]; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 5e64e5a738..8a77a08080 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -38,7 +38,6 @@ #include #include -static constexpr float kFlapSlewRate = 0.5f; // slew rate for normalized flaps setpoint [1/s] static constexpr float kSpoilersSlewRate = 0.5f; // slew rate for normalized spoilers setpoint [1/s] class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness @@ -111,4 +110,7 @@ private: SlewRate _flaps_setpoint_with_slewrate; SlewRate _spoilers_setpoint_with_slewrate; + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_flap_slew + ) }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index dd36a2d070..23405466a6 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -336,6 +336,15 @@ parameters: instance_start: 0 default: 0 + CA_SV_FLAP_SLEW: + description: + short: Control Surface slew rate for normalized flaps setpoint + type: float + decimal: 1 + min: 0.0 + max: 5.0 + default: 0.5 + CA_SV_CS${i}_SPOIL: description: short: Control Surface ${i} configuration as spoiler diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 53005d8ba5..00283b9848 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -141,11 +141,12 @@ void FwLateralLongitudinalControl::Run() if (_local_pos_sub.update(&_local_pos)) { - const float control_interval = math::constrain((_local_pos.timestamp - _last_time_loop_ran) * 1e-6f, - 0.001f, 0.1f); - _last_time_loop_ran = _local_pos.timestamp; + const hrt_abstime now = _local_pos.timestamp_sample; - updateControllerConfiguration(); + const float control_interval = math::constrain((now - _last_time_loop_ran) * 1e-6f, 0.001f, 0.1f); + _last_time_loop_ran = now; + + updateControllerConfiguration(now); _tecs.set_speed_weight(_long_configuration.speed_weight); updateTECSAltitudeTimeConstant(checkLowHeightConditions() @@ -165,7 +166,7 @@ void FwLateralLongitudinalControl::Run() _landed = landed.landed; } - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; + uint8_t current_flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; _vehicle_status_sub.update(); _control_mode_sub.update(); @@ -176,7 +177,7 @@ void FwLateralLongitudinalControl::Run() _flaps_setpoint = flaps_setpoint.normalized_setpoint; } - update_control_state(); + update_control_state(now); if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) { @@ -210,17 +211,18 @@ void FwLateralLongitudinalControl::Run() // If the both altitude and height rate are set, set altitude setpoint to NAN const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude; - tecs_update_pitch_throttle(control_interval, altitude_sp, - airspeed_sp_eas, - _long_configuration.pitch_min, - _long_configuration.pitch_max, - _long_configuration.throttle_min, - _long_configuration.throttle_max, - _long_configuration.sink_rate_target, - _long_configuration.climb_rate_target, - _long_configuration.disable_underspeed_protection, - _long_control_sp.height_rate - ); + current_flight_phase = tecs_update_pitch_throttle(control_interval, altitude_sp, + airspeed_sp_eas, + _long_configuration.pitch_min, + _long_configuration.pitch_max, + _long_configuration.throttle_min, + _long_configuration.throttle_max, + _long_configuration.sink_rate_target, + _long_configuration.climb_rate_target, + _long_configuration.disable_underspeed_protection, + _long_control_sp.height_rate, + now + ); pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint(); throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct : @@ -279,13 +281,13 @@ void FwLateralLongitudinalControl::Run() lateral_accel_sp = 0.f; // mitigation if no valid setpoint is received: 0 lateral acceleration } - lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp); + lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp, now); lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_configuration.lateral_accel_max, _lateral_configuration.lateral_accel_max); roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp); fixed_wing_lateral_status_s fixed_wing_lateral_status{}; - fixed_wing_lateral_status.timestamp = hrt_absolute_time(); + fixed_wing_lateral_status.timestamp = now; fixed_wing_lateral_status.lateral_acceleration_setpoint = lateral_accel_sp; fixed_wing_lateral_status.can_run_factor = _can_run_factor; @@ -307,7 +309,7 @@ void FwLateralLongitudinalControl::Run() // roll slew rate roll_body = _roll_slew_rate.update(roll_body, control_interval); - _att_sp.timestamp = hrt_absolute_time(); + _att_sp.timestamp = now; const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); @@ -317,22 +319,32 @@ void FwLateralLongitudinalControl::Run() } + // Publish flight phase with low rate, but immediately if updated + const bool flight_phase_updated = current_flight_phase != _flight_phase_estimation_pub.get().flight_phase; + const hrt_abstime time_since_last_flightphase_pub = now - _flight_phase_estimation_pub.get().timestamp; + + if (flight_phase_updated || time_since_last_flightphase_pub >= 1_s) { + _flight_phase_estimation_pub.get().timestamp = now; + _flight_phase_estimation_pub.get().flight_phase = current_flight_phase; + _flight_phase_estimation_pub.update(); + } + _z_reset_counter = _local_pos.z_reset_counter; } perf_end(_loop_perf); } -void FwLateralLongitudinalControl::updateControllerConfiguration() +void FwLateralLongitudinalControl::updateControllerConfiguration(hrt_abstime timestamp) { if (_lateral_configuration.timestamp == 0) { - _lateral_configuration.timestamp = _local_pos.timestamp; + _lateral_configuration.timestamp = timestamp; _lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G; } if (_long_configuration.timestamp == 0) { - setDefaultLongitudinalControlConfiguration(); + setDefaultLongitudinalControlConfiguration(timestamp); } if (_long_control_configuration_sub.updated() || _parameter_update_sub.updated()) { @@ -356,12 +368,12 @@ void FwLateralLongitudinalControl::updateControllerConfiguration() } } -void +uint8_t FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, const float desired_max_sinkrate, const float desired_max_climbrate, - bool disable_underspeed_detection, float hgt_rate_sp) + bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now) { bool tecs_is_running = true; @@ -370,8 +382,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status_sub.get().in_transition_mode)) { tecs_is_running = false; - return; - + return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; } const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min, @@ -400,7 +411,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int _long_control_state.height_rate, hgt_rate_sp); - tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated); + tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated, now); if (tecs_is_running && !_vehicle_status_sub.get().in_transition_mode && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { @@ -409,25 +420,28 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int // Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) && fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) { - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL; + return flight_phase_estimation_s::FLIGHT_PHASE_LEVEL; } else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) || (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB; + return flight_phase_estimation_s::FLIGHT_PHASE_CLIMB; } else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) || (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) { - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND; + return flight_phase_estimation_s::FLIGHT_PHASE_DESCEND; } else { - //We can't infer the flight phase , do nothing, estimation is reset at each step + // We can't infer the flight phase + return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; } } + + return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; } void FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, - float true_airspeed_derivative_raw, float throttle_trim) + float true_airspeed_derivative_raw, float throttle_trim, hrt_abstime timestamp) { tecs_status_s tecs_status{}; @@ -458,7 +472,7 @@ FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio(); tecs_status.fast_descend_ratio = debug_output.fast_descend; - tecs_status.timestamp = hrt_absolute_time(); + tecs_status.timestamp = timestamp; _tecs_status_pub.publish(tecs_status); } @@ -531,16 +545,16 @@ fw_lat_lon_control computes attitude and throttle setpoints from lateral and lon return 0; } -void FwLateralLongitudinalControl::update_control_state() { +void FwLateralLongitudinalControl::update_control_state(hrt_abstime now) { updateAltitudeAndHeightRate(); updateAirspeed(); updateAttitude(); - updateWind(); + updateWind(now); _lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy); } -void FwLateralLongitudinalControl::updateWind() { +void FwLateralLongitudinalControl::updateWind(hrt_abstime now) { if (_wind_sub.updated()) { wind_s wind{}; _wind_sub.update(&wind); @@ -549,14 +563,14 @@ void FwLateralLongitudinalControl::updateWind() { _wind_valid = PX4_ISFINITE(wind.windspeed_north) && PX4_ISFINITE(wind.windspeed_east); - _time_wind_last_received = hrt_absolute_time(); + _time_wind_last_received = now; _lateral_control_state.wind_speed(0) = wind.windspeed_north; _lateral_control_state.wind_speed(1) = wind.windspeed_east; } else { // invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout - _wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT; + _wind_valid = _wind_valid && (now - _time_wind_last_received) < WIND_EST_TIMEOUT; } if (!_wind_valid) { @@ -735,32 +749,30 @@ float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local return flying_forward_factor * low_ground_speed_factor; } -float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp) +float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp, hrt_abstime now) { // Scale the npfg output to zero if npfg is not certain for correct output _can_run_factor = math::constrain(getGuidanceQualityFactor(_local_pos, _wind_valid), 0.f, 1.f); - hrt_abstime now{hrt_absolute_time()}; - // Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition) // If the npfg was not running before, reset the user warning variables. - if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) { + if ((now - _time_of_last_npfg_call) > ROLL_WARNING_TIMEOUT) { _need_report_npfg_uncertain_condition = true; - _time_since_first_reduced_roll = 0U; + _time_of_first_reduced_roll = 0U; } if (_vehicle_status_sub.get().in_transition_mode || _can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) { // NPFG reports a good condition or we are in transition, reset the user warning variables. _need_report_npfg_uncertain_condition = true; - _time_since_first_reduced_roll = 0U; + _time_of_first_reduced_roll = 0U; } else if (_need_report_npfg_uncertain_condition) { - if (_time_since_first_reduced_roll == 0U) { - _time_since_first_reduced_roll = now; + if (_time_of_first_reduced_roll == 0U) { + _time_of_first_reduced_roll = now; } - if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) { + if ((now - _time_of_first_reduced_roll) > ROLL_WARNING_TIMEOUT) { _need_report_npfg_uncertain_condition = false; events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, "Roll command reduced due to uncertain velocity/wind estimates!"); @@ -770,7 +782,7 @@ float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float later // Nothing to do, already reported. } - _time_since_last_npfg_call = now; + _time_of_last_npfg_call = now; return _can_run_factor * (lateral_accel_sp); } @@ -778,8 +790,8 @@ float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float late return atanf(lateral_acceleration_sp / CONSTANTS_ONE_G); } -void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration() { - _long_configuration.timestamp = hrt_absolute_time(); +void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration(hrt_abstime timestamp) { + _long_configuration.timestamp = timestamp; _long_configuration.pitch_min = radians(_param_fw_p_lim_min.get()); _long_configuration.pitch_max = radians(_param_fw_p_lim_max.get()); _long_configuration.throttle_min = _param_fw_thr_min.get(); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index 87b94b1bf3..13b21e4529 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -195,8 +195,8 @@ private: matrix::Vector2f wind_speed; } _lateral_control_state{}; bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed - hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time - hrt_abstime _time_since_last_npfg_call{0U}; ///< absolute time since start when the npfg reduced roll angle calculations was last performed + hrt_abstime _time_of_first_reduced_roll{0U}; ///< absolute time when entering reduced roll angle for the first time + hrt_abstime _time_of_last_npfg_call{0U}; ///< absolute time when the npfg reduced roll angle calculations was last performed vehicle_attitude_setpoint_s _att_sp{}; bool _landed{false}; float _can_run_factor{0.f}; @@ -212,15 +212,16 @@ private: float _min_airspeed_from_guidance{0.f}; // need to store it bc we only update after running longitudinal controller void parameters_update(); - void update_control_state(); - void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, - float pitch_min_rad, float pitch_max_rad, float throttle_min, - float throttle_max, const float desired_max_sinkrate, - const float desired_max_climbrate, - bool disable_underspeed_detection, float hgt_rate_sp); + void update_control_state(hrt_abstime now); + + uint8_t tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, + float pitch_min_rad, float pitch_max_rad, float throttle_min, + float throttle_max, const float desired_max_sinkrate, + const float desired_max_climbrate, + bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now); void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw, - float throttle_trim); + float throttle_trim, hrt_abstime now); void updateAirspeed(); @@ -230,7 +231,7 @@ private: float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const; - void updateWind(); + void updateWind(hrt_abstime now); void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt); @@ -238,13 +239,13 @@ private: float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const; - float getCorrectedLateralAccelSetpoint(float lateral_accel_sp); + float getCorrectedLateralAccelSetpoint(float lateral_accel_sp, hrt_abstime now); - void setDefaultLongitudinalControlConfiguration(); + void setDefaultLongitudinalControlConfiguration(hrt_abstime now); void updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in); - void updateControllerConfiguration(); + void updateControllerConfiguration(hrt_abstime now); float getLoadFactor() const; diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 538961c324..ce11ef7f9c 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -388,11 +388,9 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) } } else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) - && (_position_setpoint_current_valid - || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + && _position_setpoint_current_valid) { - // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE. - // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. + // Enter this mode only if the current waypoint has valid 3D position setpoints. if (doing_backtransition) { _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW; @@ -433,6 +431,28 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) _control_mode_current = FW_POSCTRL_MODE_AUTO; } + } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled + && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + + // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. + if (doing_backtransition) { + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD; + + } else { + _control_mode_current = FW_POSCTRL_MODE_AUTO; + } + + } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled + && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) { + + if (doing_backtransition) { + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD; + + } else { + // Only circular landing are supported when LAND is sent without valid position + _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; + } + } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes @@ -1150,6 +1170,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = _runway_takeoff.getState(); fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f; @@ -1323,6 +1344,7 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = _runway_takeoff.getState(); fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f; @@ -1571,6 +1593,7 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = fixed_wing_runway_control_s::STATE_FLYING; // not in takeoff, use FLYING as default fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? _sticks.getYaw() : 0.f; @@ -1594,6 +1617,12 @@ void FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { + if (_time_started_landing == 0) { + // save time at which we started landing and reset landing abort status + reset_landing_state(); + _time_started_landing = now; + } + const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _param_fw_airspd_min.get(); @@ -1601,12 +1630,11 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons const Vector2f local_position{_local_pos.x, _local_pos.y}; - Vector2f local_landing_orbit_center = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - if (_time_started_landing == 0) { - // save time at which we started landing and reset landing abort status - reset_landing_state(); - _time_started_landing = now; + if (!_local_landing_orbit_center.isAllFinite()) { + _local_landing_orbit_center = _position_setpoint_current_valid + ? _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon) + : local_position; } const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(), @@ -1642,7 +1670,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons 1.0f); /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ - const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + const DirectionalGuidanceOutput sp = navigateLoiter(_local_landing_orbit_center, local_position, loiter_radius, loiter_direction_ccw, ground_speed, _wind_vel); fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; @@ -1700,7 +1728,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons } else { // follow the glide slope - const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + const DirectionalGuidanceOutput sp = navigateLoiter(_local_landing_orbit_center, local_position, loiter_radius, loiter_direction_ccw, ground_speed, _wind_vel); fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; @@ -1736,6 +1764,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons fixed_wing_runway_control_s fw_runway_control{}; fw_runway_control.timestamp = now; + fw_runway_control.runway_takeoff_state = fixed_wing_runway_control_s::STATE_FLYING; // not in takeoff, use FLYING as default fw_runway_control.wheel_steering_enabled = true; fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? _sticks.getYaw() : 0.f; @@ -2285,6 +2314,7 @@ FixedWingModeManager::reset_landing_state() _flare_states = FlareStates{}; + _local_landing_orbit_center.setNaN(); _lateral_touchdown_position_offset = 0.0f; _last_time_terrain_alt_was_valid = 0; @@ -2299,6 +2329,10 @@ FixedWingModeManager::reset_landing_state() float FixedWingModeManager::getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const { + if (!PX4_ISFINITE(altitude) || !PX4_ISFINITE(terrain_altitude)) { + return math::radians(_param_fw_r_lim.get()); + } + // we want the wings level when at the wing height above ground const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f); @@ -2424,7 +2458,7 @@ FixedWingModeManager::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, if (_local_pos.dist_bottom_valid) { - const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom; + const float terrain_estimate = _reference_altitude + -_local_pos.z - _local_pos.dist_bottom; _last_valid_terrain_alt_estimate = terrain_estimate; _last_time_terrain_alt_was_valid = now; diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index fcc6270a22..bf207b6d68 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -319,6 +319,7 @@ private: // orbit to altitude only when the aircraft has entered the final *straight approach. hrt_abstime _time_started_landing{0}; + Vector2f _local_landing_orbit_center{NAN, NAN}; // [m] lateral touchdown position offset manually commanded during landing float _lateral_touchdown_position_offset{0.0f}; diff --git a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp index 498a5332b1..96cffebb52 100644 --- a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp @@ -86,7 +86,7 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs case RunwayTakeoffState::CLIMBOUT: if (vehicle_altitude > clearance_altitude) { - takeoff_state_ = RunwayTakeoffState::FLY; + takeoff_state_ = RunwayTakeoffState::FLYING; events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude"); } @@ -134,7 +134,7 @@ float RunwayTakeoff::getThrottle(const float idle_throttle) const break; - case RunwayTakeoffState::FLY: + case RunwayTakeoffState::FLYING: throttle = NAN; } @@ -147,7 +147,7 @@ float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) c // constrain to the taxi pitch setpoint return math::radians(param_rwto_psp_.get() - 0.01f); - } else if (takeoff_state_ < RunwayTakeoffState::FLY) { + } else if (takeoff_state_ < RunwayTakeoffState::FLYING) { // ramp in the climbout pitch constraint over the rotation transition time const float taxi_pitch_min = math::radians(param_rwto_psp_.get() - 0.01f); return interpolateValuesOverAbsoluteTime(taxi_pitch_min, min_pitch_in_climbout, takeoff_time_, @@ -164,7 +164,7 @@ float RunwayTakeoff::getMaxPitch(const float max_pitch) const // constrain to the taxi pitch setpoint return math::radians(param_rwto_psp_.get() + 0.01f); - } else if (takeoff_state_ < RunwayTakeoffState::FLY) { + } else if (takeoff_state_ < RunwayTakeoffState::FLYING) { // ramp in the climbout pitch constraint over the rotation transition time const float taxi_pitch_max = math::radians(param_rwto_psp_.get() + 0.01f); return interpolateValuesOverAbsoluteTime(taxi_pitch_max, max_pitch, takeoff_time_, param_rwto_rot_time_.get()); diff --git a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h index 22fa8a51ec..5b77426bca 100644 --- a/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h @@ -57,7 +57,7 @@ enum RunwayTakeoffState { THROTTLE_RAMP = 0, // ramping up throttle CLAMPED_TO_RUNWAY, // clamped to runway, controlling yaw directly (wheel or rudder) CLIMBOUT, // climbout to safe height before navigation - FLY // navigate freely + FLYING // navigate freely }; class __EXPORT RunwayTakeoff : public ModuleParams @@ -135,7 +135,7 @@ public: float getMaxPitch(const float max_pitch) const; // NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air - void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; } + void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLYING; } /** * @brief Reset the state machine. diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5de7f5cb0a..df7016f097 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,18 +57,31 @@ bool FixedwingLandDetector::_get_landed_state() return true; } + // Force the landed state to stay landed if we're currently in an early state of the takeoff state machines. + // This prevents premature transitions to in-air during the early takeoff phase. + if (_landed_hysteresis.get_state()) { + launch_detection_status_s launch_detection_status{}; + _launch_detection_status_sub.copy(&launch_detection_status); + + fixed_wing_runway_control_s fixed_wing_runway_control{}; + _fixed_wing_runway_control_sub.copy(&fixed_wing_runway_control); + + // Check if we're in catapult/hand-launch waiting state + const bool waiting_for_catapult_launch = hrt_elapsed_time(&launch_detection_status.timestamp) < 500_ms + && launch_detection_status.launch_detection_state == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH; + + // Check if we're in runway takeoff early phase (throttle ramp or clamped to runway) + const bool waiting_for_auto_runway_climbout = hrt_elapsed_time(&fixed_wing_runway_control.timestamp) < 500_ms + && fixed_wing_runway_control.runway_takeoff_state < fixed_wing_runway_control_s::STATE_CLIMBOUT; + + if (waiting_for_catapult_launch || waiting_for_auto_runway_climbout) { + return true; + } + } + bool landDetected = false; - launch_detection_status_s launch_detection_status{}; - _launch_detection_status_sub.copy(&launch_detection_status); - - // force the landed state to stay landed if we're currently in the catapult/hand-launch launch process. Detect that we are in this state - // by checking if the last publication of launch_detection_status is less than 0.5s old, and we're still in the wait for launch state. - if (_landed_hysteresis.get_state() && hrt_elapsed_time(&launch_detection_status.timestamp) < 500_ms - && launch_detection_status.launch_detection_state == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { - landDetected = true; - - } else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { + if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { float val = 0.0f; diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 435ecc1fae..2454b83f43 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,6 +44,7 @@ #include #include +#include #include #include "LandDetector.h" @@ -67,6 +68,7 @@ protected: private: uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; + uORB::Subscription _fixed_wing_runway_control_sub{ORB_ID(fixed_wing_runway_control)}; float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 44f9973a39..9f585500eb 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -91,6 +91,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("landing_gear_wheel", 100); add_optional_topic("landing_target_pose", 1000); add_optional_topic("launch_detection_status", 200); + add_topic("logger_status", 200); add_optional_topic("magnetometer_bias_estimate", 200); add_topic("manual_control_setpoint", 200); add_topic("manual_control_switches"); @@ -124,7 +125,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("sensor_gyro_fft", 50); add_topic("sensor_selection"); add_topic("sensors_status_imu", 200); - add_optional_topic("sensor_temp", 100); add_optional_topic("spoilers_setpoint", 1000); add_topic("system_power", 500); add_optional_topic("takeoff_status", 1000); @@ -166,6 +166,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("control_allocator_status", 200, 2); add_optional_topic_multi("rate_ctrl_status", 200, 2); add_optional_topic_multi("sensor_hygrometer", 500, 4); + add_optional_topic_multi("sensor_temp", 100, 4); add_optional_topic_multi("rpm", 200); add_topic_multi("timesync_status", 1000, 3); add_optional_topic_multi("telemetry_status", 1000, 4); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8ac70225a0..4e01ca3683 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1115,6 +1115,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t ocm.position = !matrix::Vector3f(setpoint.position).isAllNan(); ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan(); ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan(); + ocm.attitude = PX4_ISFINITE(setpoint.yaw); + ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed); if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) { mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported\t"); @@ -1237,6 +1239,8 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t ocm.position = !matrix::Vector3f(setpoint.position).isAllNan(); ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan(); ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan(); + ocm.attitude = PX4_ISFINITE(setpoint.yaw); + ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed); if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) { mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT force not supported\t"); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 0501dadcc8..0d6b57c68b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -408,6 +408,7 @@ void MulticopterPositionControl::Run() } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) { // clear existing setpoint when controller is no longer active _setpoint = PositionControl::empty_trajectory_setpoint; + _control.setInputSetpoint(_setpoint); } } } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 72ae5a4a31..30e35f3235 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -84,8 +84,11 @@ void PositionControl::updateHoverThrust(const float hover_thrust_new) const float previous_hover_thrust = _hover_thrust; setHoverThrust(hover_thrust_new); - _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust - + CONSTANTS_ONE_G - _acc_sp(2); + if (PX4_ISFINITE(_acc_sp(2))) { + _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust + + CONSTANTS_ONE_G - _acc_sp(2); + } + } void PositionControl::setState(const PositionControlStates &states) diff --git a/src/modules/mc_raptor/CHECKLIST.md b/src/modules/mc_raptor/CHECKLIST.md new file mode 100644 index 0000000000..ce2ca6be47 --- /dev/null +++ b/src/modules/mc_raptor/CHECKLIST.md @@ -0,0 +1,3 @@ +# Checklist +1. Check the `REMAP_CRAZYFLIE` flag (this remaps the Crazyflie outputs to the PX4 SIH inputs) +2. Check the `CONTROL_MULTIPLE` setting (this controls how much faster the control loop is than the simulation/step frequency during training. This is needed to aggregate e.g. 4 steps of action history into one step of the policy input) diff --git a/src/modules/mc_raptor/CMakeLists.txt b/src/modules/mc_raptor/CMakeLists.txt new file mode 100644 index 0000000000..5cde791827 --- /dev/null +++ b/src/modules/mc_raptor/CMakeLists.txt @@ -0,0 +1,21 @@ +include(px4_add_library) + +px4_add_git_submodule(TARGET git_raptor_blob PATH "blob") + +px4_add_module( + MODULE modules__mc_raptor + MAIN mc_raptor + STACK_MAIN 4000 + SRCS + mc_raptor.cpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + rl_tools + git_raptor_blob + EXTERNAL + ) + +target_compile_features(modules__mc_raptor PRIVATE cxx_std_17) +target_compile_options(modules__mc_raptor PRIVATE -Wno-unused-result) diff --git a/src/modules/mc_raptor/Kconfig b/src/modules/mc_raptor/Kconfig new file mode 100644 index 0000000000..a99c05db70 --- /dev/null +++ b/src/modules/mc_raptor/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_MC_RAPTOR + bool "mc_raptor" + default n + ---help--- + Enable support for mc_raptor + +menuconfig USER_MC_RAPTOR + bool "mc_raptor running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_MC_RAPTOR + ---help--- + Put mc_raptor in userspace memory diff --git a/src/modules/mc_raptor/README.md b/src/modules/mc_raptor/README.md new file mode 100644 index 0000000000..753187b43e --- /dev/null +++ b/src/modules/mc_raptor/README.md @@ -0,0 +1,116 @@ +# RAPTOR + + +## SITL +#### Standalone Usage (Without External Trajectory Setpoint) +Build PX4 SITL with Raptor, disable QGC requirement, and adjust the `IMU_GYRO_RATEMAX` to match the simulation IMU rate +```bash +make px4_sitl_raptor gz_x500 +param set NAV_DLL_ACT 0 +param set COM_DISARM_LAND -1 # When taking off in offboard the landing detector can cause mid-air disarms +param set IMU_GYRO_RATEMAX 250 # Just for SITL. Tested with IMU_GYRO_RATEMAX=400 on real FCUs +param set MC_RAPTOR_ENABLE 1 +param set MC_RAPTOR_OFFB 0 +param save +``` +Upload the RAPTOR checkpoint to the "SD card": Separate terminal +```bash +mavproxy.py --master udp:127.0.0.1:14540 +ftp mkdir /raptor # for the real FMU use: /fs/microsd/raptor +ftp put src/modules/mc_raptor/blob/policy.tar /raptor/policy.tar +``` +restart (ctrl+c) +```bash +make px4_sitl_raptor gz_x500 +commander takeoff +commander status +``` + +Note the external mode ID of `RAPTOR` in the status report + +```bash +commander mode ext{RAPTOR_MODE_ID} +``` + + +#### Usage with External Trajectory Setpoint + + +Send Lissajous setpoints via Mavlink: +```bash +pip install px4 +px4 udp:localhost:14540 track lissajous --A 2.0 --B 1.0 --duration 10 --ramp-duration 5 --takeoff 10.0 --iterations 2 +``` + + +## SIH +```bash +make px4_fmu-v6c_raptor upload +``` +In QGroundControl: +- Airframes => SIH Quadrotor X +- Settings => Comm Links => Disable Pixhawk (disable automatic USB serial connection) +```bash +mavproxy.py --master /dev/serial/by-id/usb-Auterion_PX4_FMU_v6C.x_0-if00 --out udp:localhost:14550 --out udp:localhost:13337 --out udp:localhost:13338 +``` +New terminal (optional): +```bash +./Tools/mavlink_shell.py udp:localhost:13338 +``` + +```bash +param set SIH_IXX 0.005 +param set SIH_IYY 0.005 +param set SIH_IZZ 0.010 +param set IMU_GYRO_RATEMAX 400 +param save +reboot +``` + +New terminal: +```bash +./Tools/simulation/jmavsim/jmavsim_run.sh -u -p 13337 -o +``` + + +## Real World +Using a DroneBridge WiFi telemetry @ 1000000 baud (also set `SER_TEL1_BAUD=1000000`) and maximum packet size = 16. It seems like larger maximum packet sizes can lead to delays in forwarding the `SET_POSITION_TARGET_LOCAL_NED` messages to `trajectory_setpoint`. +```bash +./Tools/mavlink_shell.py tcp:192.168.2.1:5760 +``` +```bash +px4 tcp:192.168.2.1:5760 track lissajous --A 0.5 --B 0.5 --duration 10 --ramp-duration 5 --takeoff 1.0 --iterations 2 +``` + + +## Troubleshooting + + +```bash +cat > logger_topics.txt << EOF +raptor_status 0 +raptor_input 0 +trajectory_setpoint 0 +vehicle_local_position 0 +vehicle_angular_velocity 0 +vehicle_attitude 0 +vehicle_status 0 +actuator_motors 0 +EOF +``` +```bash +mavproxy.py +``` +```bash +ftp mkdir /fs/microsd/etc +ftp mkdir /fs/microsd/etc/logging +ftp put logger_topics.txt /fs/microsd/etc/logging/logger_topics.txt +``` + +For SITL: + +```bash +ftp mkdir etc +ftp mkdir logging +ftp put logger_topics.txt etc/logging/logger_topics.txt +``` diff --git a/src/modules/mc_raptor/blob b/src/modules/mc_raptor/blob new file mode 160000 index 0000000000..4f31fc9736 --- /dev/null +++ b/src/modules/mc_raptor/blob @@ -0,0 +1 @@ +Subproject commit 4f31fc97366487e460e44a43054756a085ab6cb7 diff --git a/src/modules/mc_raptor/mc_raptor.cpp b/src/modules/mc_raptor/mc_raptor.cpp new file mode 100644 index 0000000000..4d3f0d486c --- /dev/null +++ b/src/modules/mc_raptor/mc_raptor.cpp @@ -0,0 +1,1077 @@ +#include "mc_raptor.hpp" +#undef OK + +#include +#include + +#include + +Raptor::Raptor(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + // node state + timestamp_last_angular_velocity_set = false; + timestamp_last_local_position_set = false; + timestamp_last_attitude_set = false; + timestamp_last_trajectory_setpoint_set = false; + timestamp_last_vehicle_status_set = false; + previous_trajectory_setpoint_stale = false; + previous_active = false; + timeout_message_sent = false; + timestamp_last_policy_frequency_check_set = false; + last_intermediate_status_set = false; + last_native_status_set = false; + policy_frequency_check_counter = 0; + flightmode_state = FlightModeState::UNREGISTERED; + can_arm = false; + trajectory_setpoint_dt_index = 0; + trajectory_setpoint_dts_full = false; + trajectory_setpoint_invalid_count = 0; + trajectory_setpoint_dt_max_since_reset = 0; + internal_reference_activation_position[0] = 0.0f; + internal_reference_activation_position[1] = 0.0f; + internal_reference_activation_position[2] = 0.0f; + internal_reference_params_changed = false; + + _actuator_motors_pub.advertise(); + _tune_control_pub.advertise(); +} +void Raptor::reset() +{ + + trajectory_setpoint_dt_index = 0; + trajectory_setpoint_dts_full = false; + trajectory_setpoint_invalid_count = 0; + trajectory_setpoint_dt_max_since_reset = 0; + timestamp_last_trajectory_setpoint_set = false; + + + for (TI action_i = 0; action_i < EXECUTOR_SPEC::OUTPUT_DIM; action_i++) { + this->previous_action[action_i] = RESET_PREVIOUS_ACTION_VALUE; + } + + rlt::reset(device, executor, policy, rng); +} + +Raptor::~Raptor() +{ + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +#ifdef MC_RAPTOR_EMBED_POLICY +bool Raptor::test_policy() +{ +#else +bool Raptor::test_policy(FILE *f, TI input_offset, TI output_offset) +{ +#endif + using namespace rl_tools::inference::applications::l2f; +#ifndef RL_TOOLS_DISABLE_TEST + // This tests the policy using a known input output pair that has been saved into the policy checkpoint to verify that it has been loaded correctly + using POLICY = EXECUTOR_CONFIG::POLICY_TEST; + POLICY::template Buffer buffers_test; + POLICY::State policy_state_test; + rl_tools::Tensor, false>> + test_output; + rl_tools::Mode> mode; + using EXAMPLE_INPUT_SPEC = MC_RAPTOR_EXAMPLE_NAMESPACE::input::SPEC; + using EXAMPLE_OUTPUT_SPEC = MC_RAPTOR_EXAMPLE_NAMESPACE::output::SPEC; + float acc = 0; + uint64_t num_values = 0; + rl_tools::inference::applications::l2f::Action action; + + for (TI batch_i = 0; batch_i < EXECUTOR_CONFIG::TEST_BATCH_SIZE_ACTUAL; batch_i++) { + rl_tools::reset(device, policy, policy_state_test, rng); + + for (TI step_i = 0; step_i < EXECUTOR_CONFIG::TEST_SEQUENCE_LENGTH_ACTUAL; step_i++) { +#ifdef MC_RAPTOR_EMBED_POLICY + const auto step_input = rl_tools::view(device, MC_RAPTOR_EXAMPLE_NAMESPACE::input::container, step_i); + const auto batch_input = rl_tools::view_range(device, step_input, batch_i, rlt::tensor::ViewSpec<0, 1> {}); + const auto step_output_target = rl_tools::view(device, MC_RAPTOR_EXAMPLE_NAMESPACE::output::container, step_i); + const auto batch_output_target = rl_tools::view_range(device, step_output_target, batch_i, rlt::tensor::ViewSpec<0, 1> {}); +#else + rl_tools::Tensor, false>> + batch_input; + rl_tools::Tensor, false>> + batch_output_target; + fseek(f, input_offset + (step_i * EXAMPLE_INPUT_SPEC::STRIDE::FIRST + batch_i * EXAMPLE_INPUT_SPEC::STRIDE::template GET<1>)*sizeof( + EXAMPLE_INPUT_SPEC::T), SEEK_SET); + fread(batch_input._data, sizeof(EXAMPLE_INPUT_SPEC::T), EXAMPLE_INPUT_SPEC::SHAPE::LAST, f); + fseek(f, output_offset + (step_i * EXAMPLE_OUTPUT_SPEC::STRIDE::FIRST + batch_i * EXAMPLE_OUTPUT_SPEC::STRIDE::template GET<1>)*sizeof( + EXAMPLE_OUTPUT_SPEC::T), SEEK_SET); + fread(batch_output_target._data, sizeof(EXAMPLE_OUTPUT_SPEC::T), EXAMPLE_OUTPUT_SPEC::SHAPE::LAST, f); +#endif + rl_tools::utils::assert_exit(device, !rl_tools::is_nan(device, batch_input), "input is nan"); + rl_tools::evaluate_step(device, policy, batch_input, policy_state_test, test_output, buffers_test, rng, mode); + rl_tools::utils::assert_exit(device, !rl_tools::is_nan(device, test_output), "output is nan"); + + for (TI action_i = 0; action_i < EXECUTOR_CONFIG::OUTPUT_DIM; action_i++) { + acc += rl_tools::math::abs(device.math, rl_tools::get(device, test_output, 0, action_i) - rl_tools::get(device, batch_output_target, 0, + action_i)); + num_values += 1; + rl_tools::utils::assert_exit(device, !rl_tools::math::is_nan(device.math, acc), "output is nan"); + + if (batch_i == 0 && step_i == EXECUTOR_CONFIG::TEST_SEQUENCE_LENGTH_ACTUAL - 1) { + action.action[action_i] = rl_tools::get(device, test_output, 0, action_i); + } + } + } + } + + float abs_diff = acc / num_values; + PX4_INFO("Checkpoint test diff: %f", (double)abs_diff); + + for (TI output_i = 0; output_i < EXECUTOR_CONFIG::OUTPUT_DIM; output_i++) { + PX4_INFO("output[%d]: %f", (int)output_i, (double)action.action[output_i]); + } + + constexpr float EPSILON = 1e-5; + + bool healthy = abs_diff < EPSILON; + + if (!healthy) { + PX4_ERR("Checkpoint test failed with diff %.10f", (double)abs_diff); + return false; + + } else { + PX4_INFO("Checkpoint test passed with diff %.10f", (double)abs_diff); + return true; + } + +#else + return 0; +#endif +} + +bool Raptor::init() +{ + this->init_time = hrt_absolute_time(); + + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity_sub callback registration failed"); + return false; + } + +#ifndef MC_RAPTOR_EMBED_POLICY + const char *path = PX4_STORAGEDIR "/raptor/policy.tar"; + + struct stat st; + bool file_exists = (stat(path, &st) == 0); + + if (file_exists) { + PX4_INFO("Policy checkpoint %s exists", path); + FILE *f = fopen(path, "rb"); + + if (!f) { + PX4_ERR("Failed to open %s: %s", path, strerror(errno)); + return false; + } + + if (fseek(f, 0, SEEK_END) != 0) { + PX4_ERR("fseek failed: %s", strerror(errno)); + fclose(f); + return false; + } + + long size = ftell(f); + + if (size < 0) { + PX4_ERR("ftell failed: %s", strerror(errno)); + fclose(f); + return false; + + } else { + rewind(f); + bool successfully_loaded = false; + using SPEC = rlt::persist::backends::tar::ReaderGroupSpecification>; + rlt::persist::backends::tar::ReaderGroup reader_group; + reader_group.data.f = f; + reader_group.data.size = size; + auto actor_group = rlt::get_group(device, reader_group, "actor"); + successfully_loaded = rlt::load(device, policy, actor_group); + constexpr TI METADATA_BUFFER_SIZE = 256; + char metadata_buffer[METADATA_BUFFER_SIZE]; + TI read_size = 0; + rlt::persist::backends::tar::get(device, reader_group.data, "actor/meta", metadata_buffer, METADATA_BUFFER_SIZE, read_size); + TI checkpoint_name_position = 0; + TI checkpoint_name_len = 0; + + if (rlt::persist::backends::tar::seek_in_metadata(device, metadata_buffer, METADATA_BUFFER_SIZE, "checkpoint_name", + checkpoint_name_position, checkpoint_name_len)) { + strncpy(checkpoint_name, metadata_buffer + checkpoint_name_position, CHECKPOINT_NAME_LENGTH); + checkpoint_name[checkpoint_name_len < CHECKPOINT_NAME_LENGTH ? checkpoint_name_len : CHECKPOINT_NAME_LENGTH - 1] = '\0'; + + } else { + PX4_ERR("Failed to get checkpoint name from metadata"); + return false; + } + + if (successfully_loaded) { + PX4_INFO("Policy loaded from file %s", path); + TI input_offset = 0; + TI input_size = 0; + rlt::persist::backends::tar::seek(device, reader_group.data, "example/input/data", input_offset, input_size); + PX4_INFO("Input offset: %d", (int)input_offset); + TI output_offset = 0; + TI output_size = 0; + rlt::persist::backends::tar::seek(device, reader_group.data, "example/output/data", output_offset, output_size); + PX4_INFO("Output offset: %d", (int)output_offset); + + if (!test_policy(f, input_offset, output_offset)) { + PX4_ERR("Checkpoint test failed"); + return false; + } + + } else { + PX4_ERR("Failed to load policy from file %s", path); + return false; + } + + fclose(f); + } + + } else { + PX4_INFO("File %s does not exist", path); + return false; + } + +#else + + strncpy(checkpoint_name, MC_RAPTOR_META_NAMESPACE::name, CHECKPOINT_NAME_LENGTH); + + if (!test_policy()) { + PX4_ERR("Checkpoint test failed"); + return false; + } + +#endif + PX4_INFO("Checkpoint name: %s", checkpoint_name); + + + register_ext_component_request_s register_ext_component_request{}; + register_ext_component_request.timestamp = hrt_absolute_time(); + strncpy(register_ext_component_request.name, "RAPTOR", sizeof(register_ext_component_request.name) - 1); + register_ext_component_request.request_id = Raptor::EXT_COMPONENT_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.enable_replace_internal_mode = _param_mc_raptor_offboard.get(); + register_ext_component_request.replace_internal_mode = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + _register_ext_component_request_pub.publish(register_ext_component_request); + + int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get(); + + if (imu_gyro_ratemax % POLICY_CONTROL_FREQUENCY_TRAINING != 0) { + PX4_WARN("IMU_GYRO_RATEMAX=%d Hz is not a multiple of the training frequency (%d Hz)", (int)imu_gyro_ratemax, + (int)POLICY_CONTROL_FREQUENCY_TRAINING); + } + + int32_t force_sync_native = imu_gyro_ratemax / POLICY_CONTROL_FREQUENCY_TRAINING; + executor.executor.force_sync_native = force_sync_native; + executor.executor.force_sync_native_initialized = true; + PX4_INFO("IMU_GYRO_RATEMAX=%d Hz", (int)imu_gyro_ratemax); + PX4_INFO("POLICY_CONTROL_FREQUENCY_TRAINING=%d Hz", (int)POLICY_CONTROL_FREQUENCY_TRAINING); + PX4_INFO("Setting force_sync_native = %d Hz / %d Hz = %d", (int)imu_gyro_ratemax, (int)POLICY_CONTROL_FREQUENCY_TRAINING, + (int)force_sync_native); + + this->use_internal_reference = _param_mc_raptor_intref.get(); + + this->reset(); + + return true; +} +template +T clip(T x, T max, T min) +{ + if (x > max) { + return max; + } + + if (x < min) { + return min; + } + + return x; +} +template +void quaternion_multiplication(T q1[4], T q2[4], T q_res[4]) +{ + q_res[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + q_res[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + q_res[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + q_res[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; +} +template +void quaternion_conjugate(T q[4], T q_res[4]) +{ + q_res[0] = +q[0]; + q_res[1] = -q[1]; + q_res[2] = -q[2]; + q_res[3] = -q[3]; +} +template +void quaternion_to_rotation_matrix(T q[4], T R[9]) +{ + // row-major + T qw = q[0]; + T qx = q[1]; + T qy = q[2]; + T qz = q[3]; + + R[0] = 1 - 2 * qy * qy - 2 * qz * qz; + R[1] = 2 * qx * qy - 2 * qw * qz; + R[2] = 2 * qx * qz + 2 * qw * qy; + R[3] = 2 * qx * qy + 2 * qw * qz; + R[4] = 1 - 2 * qx * qx - 2 * qz * qz; + R[5] = 2 * qy * qz - 2 * qw * qx; + R[6] = 2 * qx * qz - 2 * qw * qy; + R[7] = 2 * qy * qz + 2 * qw * qx; + R[8] = 1 - 2 * qx * qx - 2 * qy * qy; +} + +template +void rotate_vector(T R[9], T v[3], T v_rotated[3]) +{ + v_rotated[0] = R[0] * v[0] + R[1] * v[1] + R[2] * v[2]; + v_rotated[1] = R[3] * v[0] + R[4] * v[1] + R[5] * v[2]; + v_rotated[2] = R[6] * v[0] + R[7] * v[1] + R[8] * v[2]; +} + +void Raptor::observe(rl_tools::inference::applications::l2f::Observation &observation) +{ + // converting from FRD to FLU + T Rt_inv[9]; + + { + // Orientation + // FRD to FLU + T q_target[4]; + q_target[0] = cosf(0.5f * _trajectory_setpoint.yaw); // minus because the setpoint yaw is in NED + q_target[1] = 0; + q_target[2] = 0; + q_target[3] = sinf(0.5f * _trajectory_setpoint.yaw); + + T qt[4], qtc[4], qr[4]; + qt[0] = +q_target[0]; // conjugate to build the difference between setpoint and current + qt[1] = +q_target[1]; + qt[2] = -q_target[2]; + qt[3] = -q_target[3]; + quaternion_conjugate(qt, qtc); + quaternion_to_rotation_matrix(qtc, Rt_inv); + + qr[0] = +_vehicle_attitude.q[0]; + qr[1] = +_vehicle_attitude.q[1]; + qr[2] = -_vehicle_attitude.q[2]; + qr[3] = -_vehicle_attitude.q[3]; + // qr = qt * qd + // qd = qt' * qr + T qd[4]; + quaternion_multiplication(qtc, qr, qd); + + observation.orientation[0] = qd[0]; + observation.orientation[1] = qd[1]; + observation.orientation[2] = qd[2]; + observation.orientation[3] = qd[3]; + } + + { + // Position + T p[3], pt[3]; // FLU + p[0] = +(position[0] - _trajectory_setpoint.position[0]); + p[1] = -(position[1] - _trajectory_setpoint.position[1]); + p[2] = -(position[2] - _trajectory_setpoint.position[2]); + rotate_vector(Rt_inv, p, pt); // The position and velocity error are in the target frame + observation.position[0] = clip(pt[0], max_position_error, -max_position_error); + observation.position[1] = clip(pt[1], max_position_error, -max_position_error); + observation.position[2] = clip(pt[2], max_position_error, -max_position_error); + } + { + // Linear Velocity + T v[3], vt[3]; + v[0] = +(linear_velocity[0] - _trajectory_setpoint.velocity[0]); + v[1] = -(linear_velocity[1] - _trajectory_setpoint.velocity[1]); + v[2] = -(linear_velocity[2] - _trajectory_setpoint.velocity[2]); + rotate_vector(Rt_inv, v, vt); + observation.linear_velocity[0] = clip(vt[0], max_velocity_error, -max_velocity_error); + observation.linear_velocity[1] = clip(vt[1], max_velocity_error, -max_velocity_error); + observation.linear_velocity[2] = clip(vt[2], max_velocity_error, -max_velocity_error); + } + { + // Angular Velocity + observation.angular_velocity[0] = +_vehicle_angular_velocity.xyz[0]; + observation.angular_velocity[1] = -_vehicle_angular_velocity.xyz[1]; + observation.angular_velocity[2] = -_vehicle_angular_velocity.xyz[2]; + } + + for (TI action_i = 0; action_i < EXECUTOR_CONFIG::OUTPUT_DIM; action_i++) { + observation.previous_action[action_i] = this->previous_action[action_i]; + } +} + + +void Raptor::updateArmingCheckReply() +{ + if (flightmode_state == FlightModeState::CONFIGURED) { + if (_arming_check_request_sub.updated()) { + arming_check_request_s arming_check_request; + _arming_check_request_sub.copy(&arming_check_request); + arming_check_reply_s arming_check_reply; + arming_check_reply.timestamp = hrt_absolute_time(); + arming_check_reply.request_id = arming_check_request.request_id; + arming_check_reply.registration_id = ext_component_arming_check_id; + arming_check_reply.health_component_index = arming_check_reply.HEALTH_COMPONENT_INDEX_NONE; + arming_check_reply.num_events = 0; + arming_check_reply.can_arm_and_run = can_arm; + 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_local_alt = true; + arming_check_reply.mode_req_home_position = false; + 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 Raptor::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + + if (flightmode_state >= FlightModeState::REGISTERED) { + unregister_ext_component_s unregister_ext_component{}; + unregister_ext_component.timestamp = hrt_absolute_time(); + strncpy(unregister_ext_component.name, "RAPTOR", sizeof(unregister_ext_component.name) - 1); + unregister_ext_component.arming_check_id = ext_component_arming_check_id; + unregister_ext_component.mode_id = ext_component_mode_id; + unregister_ext_component.mode_executor_id = -1; + _unregister_ext_component_pub.publish(unregister_ext_component); + } + + ScheduleClear(); + exit_and_cleanup(); + return; + } + + register_ext_component_reply_s register_ext_component_reply; + + if (_register_ext_component_reply_sub.update(®ister_ext_component_reply)) { + if (register_ext_component_reply.request_id == Raptor::EXT_COMPONENT_REQUEST_ID && register_ext_component_reply.success) { + ext_component_arming_check_id = register_ext_component_reply.arming_check_id; + ext_component_mode_id = register_ext_component_reply.mode_id; + flightmode_state = FlightModeState::REGISTERED; + PX4_INFO("Raptor mode registration successful, arming_check_id: %d, mode_id: %d", ext_component_arming_check_id, ext_component_mode_id); + } + } + + if (flightmode_state == FlightModeState::REGISTERED) { + vehicle_control_mode_s config_control_setpoints{}; + config_control_setpoints.timestamp = hrt_absolute_time(); + config_control_setpoints.source_id = ext_component_mode_id; + config_control_setpoints.flag_multicopter_position_control_enabled = false; + config_control_setpoints.flag_control_manual_enabled = false; + config_control_setpoints.flag_control_offboard_enabled = false; + config_control_setpoints.flag_control_position_enabled = false; + config_control_setpoints.flag_control_climb_rate_enabled = false; + config_control_setpoints.flag_control_allocation_enabled = false; + config_control_setpoints.flag_control_termination_enabled = true; + _config_control_setpoints_pub.publish(config_control_setpoints); + flightmode_state = FlightModeState::CONFIGURED; + PX4_INFO("Raptor mode configuration sent"); + } + + + perf_count(_loop_interval_perf); + + perf_begin(_loop_perf); + hrt_abstime current_time = hrt_absolute_time(); + + raptor_status_s status{}; + status.timestamp = current_time; + status.timestamp_sample = current_time; + status.exit_reason = raptor_status_s::EXIT_REASON_NONE; + status.substep = 0; + status.active = false; + status.control_interval = NAN; + status.trajectory_setpoint_dt_mean = NAN; + status.trajectory_setpoint_dt_max = NAN; + status.trajectory_setpoint_dt_max_since_activation = NAN; + + if (trajectory_setpoint_dts_full || trajectory_setpoint_dt_index > 0) { + float trajectory_setpoint_dt_mean = 0; + float trajectory_setpoint_dt_max = 0; + + for (TI i = 0; i < (trajectory_setpoint_dts_full ? NUM_TRAJECTORY_SETPOINT_DTS : trajectory_setpoint_dt_index); i++) { + TI index = trajectory_setpoint_dts_full ? i : trajectory_setpoint_dt_index - 1 - i; + trajectory_setpoint_dt_mean += trajectory_setpoint_dts[index]; + + if (trajectory_setpoint_dts[index] > trajectory_setpoint_dt_max) { + trajectory_setpoint_dt_max = trajectory_setpoint_dts[index]; + } + } + + if (trajectory_setpoint_dt_max > trajectory_setpoint_dt_max_since_reset) { + trajectory_setpoint_dt_max_since_reset = trajectory_setpoint_dt_max; + } + + trajectory_setpoint_dt_mean /= NUM_TRAJECTORY_SETPOINT_DTS; + status.trajectory_setpoint_dt_mean = trajectory_setpoint_dt_mean; + status.trajectory_setpoint_dt_max = trajectory_setpoint_dt_max; + status.trajectory_setpoint_dt_max_since_activation = trajectory_setpoint_dt_max_since_reset; + } + + status.subscription_update_vehicle_status = _vehicle_status_sub.update(&_vehicle_status); + + if (status.subscription_update_vehicle_status) { + timestamp_last_vehicle_status = current_time; + timestamp_last_vehicle_status_set = true; + } + + bool next_active = timestamp_last_vehicle_status_set && _vehicle_status.nav_state == ext_component_mode_id; + + if (!previous_active && next_active) { + this->reset(); + PX4_INFO("Resetting Inference Executor (Recurrent State)"); + + } else { + if (previous_active && !next_active) { + PX4_INFO("inactive"); + } + } + + bool angular_velocity_update = false; + status.subscription_update_angular_velocity = _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); + + if (status.subscription_update_angular_velocity) { + timestamp_last_angular_velocity = current_time; + timestamp_last_angular_velocity_set = true; + angular_velocity_update = true; + } + + status.timestamp_last_vehicle_angular_velocity = current_time; + status.timestamp_sample = _vehicle_angular_velocity.timestamp_sample; + + status.subscription_update_local_position = _vehicle_local_position_sub.update(&_vehicle_local_position); + + if (status.subscription_update_local_position) { + timestamp_last_local_position = current_time; + timestamp_last_local_position_set = true; + } + + status.timestamp_last_vehicle_local_position = current_time; + + status.subscription_update_attitude = _vehicle_attitude_sub.update(&_vehicle_attitude); + + if (status.subscription_update_attitude) { + timestamp_last_attitude = current_time; + timestamp_last_attitude_set = true; + } + + status.timestamp_last_vehicle_attitude = timestamp_last_attitude; + + trajectory_setpoint_s temp_trajectory_setpoint; + bool use_external_reference = !use_internal_reference; + status.subscription_update_trajectory_setpoint = use_external_reference && _trajectory_setpoint_sub.update(&temp_trajectory_setpoint); + + if (status.subscription_update_trajectory_setpoint) { + if ( + PX4_ISFINITE(temp_trajectory_setpoint.position[0]) && + PX4_ISFINITE(temp_trajectory_setpoint.position[1]) && + PX4_ISFINITE(temp_trajectory_setpoint.position[2]) && + PX4_ISFINITE(temp_trajectory_setpoint.yaw) && + PX4_ISFINITE(temp_trajectory_setpoint.velocity[0]) && + PX4_ISFINITE(temp_trajectory_setpoint.velocity[1]) && + PX4_ISFINITE(temp_trajectory_setpoint.velocity[2]) && + PX4_ISFINITE(temp_trajectory_setpoint.yawspeed) + ) { + if (timestamp_last_trajectory_setpoint_set) { + trajectory_setpoint_dts[trajectory_setpoint_dt_index] = current_time - timestamp_last_trajectory_setpoint; + trajectory_setpoint_dt_index++; + + if (trajectory_setpoint_dt_index == NUM_TRAJECTORY_SETPOINT_DTS) { + if (next_active && !trajectory_setpoint_dts_full) { + PX4_INFO("trajectory_setpoint_dts_full"); + } + + trajectory_setpoint_dts_full = true; + trajectory_setpoint_dt_index = 0; + } + } + + timestamp_last_trajectory_setpoint_set = true; + status.timestamp_last_trajectory_setpoint = current_time; + timestamp_last_trajectory_setpoint = current_time; + _trajectory_setpoint = temp_trajectory_setpoint; + + } else { + trajectory_setpoint_invalid_count++; + + if (next_active && trajectory_setpoint_invalid_count % TRAJECTORY_SETPOINT_INVALID_COUNT_WARNING_INTERVAL == 0) { + PX4_WARN("trajectory_setpoint invalid, count: %d", (int)trajectory_setpoint_invalid_count); + } + } + } + + constexpr bool PUBLISH_NON_COMPLETE_STATUS = true; + + if (!angular_velocity_update) { + status.exit_reason = raptor_status_s::EXIT_REASON_NO_ANGULAR_VELOCITY_UPDATE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + updateArmingCheckReply(); + return; + } + + if (!timestamp_last_angular_velocity_set || !timestamp_last_local_position_set || !timestamp_last_attitude_set) { + status.exit_reason = raptor_status_s::EXIT_REASON_NOT_ALL_OBSERVATIONS_SET; + status.vehicle_angular_velocity_stale = !timestamp_last_angular_velocity_set; + status.vehicle_local_position_stale = !timestamp_last_local_position_set; + status.vehicle_attitude_stale = !timestamp_last_attitude_set; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + can_arm = false; + updateArmingCheckReply(); + return; + } + + if ((current_time - timestamp_last_angular_velocity) > OBSERVATION_TIMEOUT_ANGULAR_VELOCITY) { + status.exit_reason = raptor_status_s::EXIT_REASON_ANGULAR_VELOCITY_STALE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + if (!timeout_message_sent) { + PX4_ERR("angular velocity timeout"); + timeout_message_sent = true; + } + + can_arm = false; + updateArmingCheckReply(); + return; + } + + if ((current_time - timestamp_last_local_position) > OBSERVATION_TIMEOUT_LOCAL_POSITION) { + status.exit_reason = raptor_status_s::EXIT_REASON_LOCAL_POSITION_STALE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + if (!timeout_message_sent) { + PX4_ERR("local position timeout"); + timeout_message_sent = true; + } + + can_arm = false; + updateArmingCheckReply(); + return; + + } else { + position[0] = _vehicle_local_position.x; + position[1] = _vehicle_local_position.y; + position[2] = _vehicle_local_position.z; + linear_velocity[0] = _vehicle_local_position.vx; + linear_velocity[1] = _vehicle_local_position.vy; + linear_velocity[2] = _vehicle_local_position.vz; + } + + // position and linear_velocity are guaranteed to be set after this point + auto previous_internal_reference = internal_reference; + internal_reference = static_cast(_param_mc_raptor_intref.get()); + bool internal_reference_changed = previous_internal_reference != internal_reference; + + if (internal_reference_changed) { + PX4_INFO("internal reference changed from %d to %d", (int)previous_internal_reference, (int)internal_reference); + } + + if (use_internal_reference && internal_reference != InternalReference::NONE) { + if (next_active && (!previous_active || internal_reference_changed || internal_reference_params_changed)) { + internal_reference_activation_position[0] = position[0]; + internal_reference_activation_position[1] = - position[1]; + internal_reference_activation_position[2] = - position[2]; + internal_reference_activation_orientation[0] = _vehicle_attitude.q[0]; + internal_reference_activation_orientation[1] = _vehicle_attitude.q[1]; + internal_reference_activation_orientation[2] = -_vehicle_attitude.q[2]; + internal_reference_activation_orientation[3] = -_vehicle_attitude.q[3]; + internal_reference_activation_time = current_time; + PX4_INFO("internal reference activated at: %f %f %f", (double)internal_reference_activation_position[0], + (double)internal_reference_activation_position[1], (double)internal_reference_activation_position[2]); + internal_reference_params_changed = false; + } + + Setpoint setpoint{}; + + if (internal_reference == InternalReference::LISSAJOUS) { + setpoint = lissajous(static_cast(current_time - internal_reference_activation_time) / 1000000, lissajous_params); + + } else { + PX4_ERR("internal reference type not supported"); + } + + auto &q = internal_reference_activation_orientation; + matrix::Quatf q_activation_frame(q[0], q[1], q[2], q[3]); + matrix::Vector3f position_activation_frame = q_activation_frame.rotateVector(matrix::Vector3f(setpoint.position[0], setpoint.position[1], + setpoint.position[2])); + matrix::Vector3f linear_velocity_activation_frame = q_activation_frame.rotateVector(matrix::Vector3f(setpoint.linear_velocity[0], + setpoint.linear_velocity[1], setpoint.linear_velocity[2])); + + _trajectory_setpoint.position[0] = +(internal_reference_activation_position[0] + position_activation_frame(0)); + _trajectory_setpoint.position[1] = -(internal_reference_activation_position[1] + position_activation_frame(1)); + _trajectory_setpoint.position[2] = -(internal_reference_activation_position[2] + position_activation_frame(2)); + _trajectory_setpoint.yaw = - atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3])) - setpoint.yaw; + _trajectory_setpoint.velocity[0] = +linear_velocity_activation_frame(0); + _trajectory_setpoint.velocity[1] = -linear_velocity_activation_frame(1); + _trajectory_setpoint.velocity[2] = -linear_velocity_activation_frame(2); + _trajectory_setpoint.yawspeed = -setpoint.yaw_rate; + timestamp_last_trajectory_setpoint_set = true; + status.timestamp_last_trajectory_setpoint = current_time; + timestamp_last_trajectory_setpoint = current_time; + + status.internal_reference_position[0] = _trajectory_setpoint.position[0]; + status.internal_reference_position[1] = _trajectory_setpoint.position[1]; + status.internal_reference_position[2] = _trajectory_setpoint.position[2]; + status.internal_reference_linear_velocity[0] = _trajectory_setpoint.velocity[0]; + status.internal_reference_linear_velocity[1] = _trajectory_setpoint.velocity[1]; + status.internal_reference_linear_velocity[2] = _trajectory_setpoint.velocity[2]; + } + + if ((current_time - timestamp_last_attitude) > OBSERVATION_TIMEOUT_ATTITUDE) { + status.exit_reason = raptor_status_s::EXIT_REASON_ATTITUDE_STALE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + if (!timeout_message_sent) { + PX4_ERR("attitude timeout"); + timeout_message_sent = true; + } + + can_arm = false; + updateArmingCheckReply(); + return; + } + + timeout_message_sent = false; + + // is ready to control at this point + can_arm = true; + updateArmingCheckReply(); + + if (!timestamp_last_trajectory_setpoint_set || use_internal_reference + || (current_time - timestamp_last_trajectory_setpoint) > TRAJECTORY_SETPOINT_TIMEOUT) { + status.trajectory_setpoint_stale = true; + + if (!previous_trajectory_setpoint_stale || (!previous_active && next_active)) { + _trajectory_setpoint.position[0] = position[0]; + _trajectory_setpoint.position[1] = position[1]; + _trajectory_setpoint.position[2] = position[2]; + auto &q = _vehicle_attitude.q; + _trajectory_setpoint.yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3])); + _trajectory_setpoint.velocity[0] = 0; + _trajectory_setpoint.velocity[1] = 0; + _trajectory_setpoint.velocity[2] = 0; + _trajectory_setpoint.yawspeed = 0; + + if (!previous_trajectory_setpoint_stale) { + PX4_WARN("trajectory_setpoint turned stale at: %f %f %f, yaw: %f %llu / %llu us", (double)position[0], (double)position[1], + (double)position[2], + (double)_trajectory_setpoint.yaw, (unsigned long long)(current_time - timestamp_last_trajectory_setpoint), + (unsigned long long)(TRAJECTORY_SETPOINT_TIMEOUT)); + + } else { + PX4_WARN("trajectory_setpoint reset due to activation at: %f %f %f, yaw: %f", (double)position[0], (double)position[1], (double)position[2], + (double)_trajectory_setpoint.yaw); + } + } + + previous_trajectory_setpoint_stale = true; + + } else { + if (previous_trajectory_setpoint_stale) { + PX4_WARN("trajectory_setpoint turned non-stale at: %f %f %f", (double)position[0], (double)position[1], (double)position[2]); + previous_trajectory_setpoint_stale = false; + } + + status.trajectory_setpoint_stale = false; + } + + + + + + rl_tools::inference::applications::l2f::Observation observation; + rl_tools::inference::applications::l2f::Action action; + observe(observation); + hrt_abstime nanoseconds = current_time * 1000; + auto executor_status = rl_tools::control(device, executor, nanoseconds, policy, observation, action, rng); + + if (!executor_status.OK) { + if (executor_status.TIMESTAMP_INVALID) { + PX4_ERR("RLtools executor error: Timestamp invalid"); + } + + if (executor_status.LAST_CONTROL_TIMESTAMP_GREATER_THAN_LAST_OBSERVATION_TIMESTAMP) { + PX4_ERR("RLtools executor error: Last control timestamp %llu greater than last observation timestamp %llu", + (unsigned long long)executor.executor.last_control_timestamp, (unsigned long long)executor.executor.last_observation_timestamp); + } + } + + if (executor_status.source != decltype(executor_status.source)::CONTROL) { + // status.exit_reason = raptor_status_s::EXIT_REASON_EXECUTOR_STATUS_SOURCE_NOT_CONTROL; + // if constexpr(PUBLISH_NON_COMPLETE_STATUS){ + // _raptor_status_pub.publish(status); + // } + // Exit early if it is not time to control + return; + } + + + status.active = next_active; + + + // no return after this point! + + raptor_input_s input_msg; + input_msg.active = status.active; + static_assert(raptor_input_s::ACTION_DIM == EXECUTOR_CONFIG::OUTPUT_DIM); + input_msg.timestamp = current_time; + input_msg.timestamp_sample = _vehicle_angular_velocity.timestamp_sample; + + for (TI dim_i = 0; dim_i < 3; dim_i++) { + input_msg.position[dim_i] = observation.position[dim_i]; + input_msg.orientation[dim_i] = observation.orientation[dim_i]; + input_msg.linear_velocity[dim_i] = observation.linear_velocity[dim_i]; + input_msg.angular_velocity[dim_i] = observation.angular_velocity[dim_i]; + } + + input_msg.orientation[3] = observation.orientation[3]; + + for (TI dim_i = 0; dim_i < EXECUTOR_CONFIG::OUTPUT_DIM; dim_i++) { + input_msg.previous_action[dim_i] = observation.previous_action[dim_i]; + } + + _raptor_input_pub.publish(input_msg); + _raptor_status_pub.publish(status); + + actuator_motors_s actuator_motors{}; + actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp_sample = _vehicle_angular_velocity.timestamp_sample; + + for (TI action_i = 0; action_i < actuator_motors_s::NUM_CONTROLS; action_i++) { + if (action_i < EXECUTOR_CONFIG::OUTPUT_DIM) { + T value = action.action[action_i]; + this->previous_action[action_i] = value; + value = (value + 1) / 2; + constexpr T training_min = 0; + constexpr T training_max = 1.0; + T scaled_value = (training_max - training_min) * value + training_min; + actuator_motors.control[action_i] = scaled_value; + + } else { + actuator_motors.control[action_i] = NAN; + } + } + + if constexpr(Raptor::REMAP_FROM_CRAZYFLIE) { + actuator_motors_s temp = actuator_motors; + temp.control[0] = actuator_motors.control[0]; + temp.control[1] = actuator_motors.control[2]; + temp.control[2] = actuator_motors.control[3]; + temp.control[3] = actuator_motors.control[1]; + actuator_motors = temp; + } + + if (status.active) { + _actuator_motors_pub.publish(actuator_motors); + } + + perf_end(_loop_perf); + previous_active = next_active; + + if (executor_status.source == decltype(executor_status.source)::CONTROL) { + if (executor_status.step_type == decltype(executor_status.step_type)::INTERMEDIATE) { + this->last_intermediate_status = executor_status; + this->last_intermediate_status_set = true; + + } else if (executor_status.step_type == decltype(executor_status.step_type)::NATIVE) { + this->last_native_status = executor_status; + this->last_native_status_set = true; + } + } + + if (!this->timestamp_last_policy_frequency_check_set + || (current_time - timestamp_last_policy_frequency_check) > POLICY_FREQUENCY_CHECK_INTERVAL) { + if (this->timestamp_last_policy_frequency_check_set) { + if (last_intermediate_status_set) { + if (!this->last_intermediate_status.timing_bias.OK || !this->last_intermediate_status.timing_jitter.OK) { + PX4_WARN("Raptor: INTERMEDIATE: BIAS %fx JITTER %fx", (double)this->last_intermediate_status.timing_bias.MAGNITUDE, + (double)this->last_intermediate_status.timing_jitter.MAGNITUDE); + + } else { + if (ENABLE_CONTROL_FREQUENCY_INFO && this->policy_frequency_check_counter % POLICY_FREQUENCY_INFO_INTERVAL == 0) { + PX4_INFO("Raptor: INTERMEDIATE: BIAS %fx JITTER %fx", (double)this->last_intermediate_status.timing_bias.MAGNITUDE, + (double)this->last_intermediate_status.timing_jitter.MAGNITUDE); + } + } + } + + if (last_native_status_set) { + if (!this->last_native_status.timing_bias.OK || !this->last_native_status.timing_jitter.OK) { + PX4_WARN("Raptor: NATIVE: BIAS %fx JITTER %fx", (double)this->last_native_status.timing_bias.MAGNITUDE, + (double)this->last_native_status.timing_jitter.MAGNITUDE); + + } else { + if (ENABLE_CONTROL_FREQUENCY_INFO && this->policy_frequency_check_counter % POLICY_FREQUENCY_INFO_INTERVAL == 0) { + PX4_INFO("Raptor: NATIVE: BIAS %fx JITTER %fx", (double)this->last_native_status.timing_bias.MAGNITUDE, + (double)this->last_native_status.timing_jitter.MAGNITUDE); + } + } + } + } + + this->num_healthy_executor_statii_intermediate = 0; + this->num_non_healthy_executor_statii_intermediate = 0; + this->num_healthy_executor_statii_native = 0; + this->num_non_healthy_executor_statii_native = 0; + this->num_statii = 0; + this->timestamp_last_policy_frequency_check = current_time; + this->timestamp_last_policy_frequency_check_set = true; + this->policy_frequency_check_counter++; + } + + this->num_statii++; + this->num_healthy_executor_statii_intermediate += executor_status.OK && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::INTERMEDIATE; + this->num_non_healthy_executor_statii_intermediate += (!executor_status.OK) + && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::INTERMEDIATE; + this->num_healthy_executor_statii_native += executor_status.OK && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::NATIVE; + this->num_non_healthy_executor_statii_native += (!executor_status.OK) + && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::NATIVE; +} + +int Raptor::task_spawn(int argc, char *argv[]) +{ + Raptor *instance = new Raptor(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int Raptor::print_status() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + perf_print_counter(_loop_interval_policy_perf); + PX4_INFO_RAW("Checkpoint: %s\n", checkpoint_name); + return 0; +} + +int Raptor::custom_command(int argc, char *argv[]) +{ + if (argc >= 2 && strcmp(argv[0], "intref") == 0) { + if (strcmp(argv[1], "lissajous") == 0) { + // Usage: mc_raptor intref lissajous + if (argc != 10) { + PX4_ERR("Usage: mc_raptor intref lissajous "); + return PX4_ERROR; + } + + Raptor *instance = get_instance(); + + if (instance == nullptr) { + PX4_ERR("mc_raptor is not running"); + return PX4_ERROR; + } + + instance->lissajous_params.A = strtof(argv[2], nullptr); + instance->lissajous_params.B = strtof(argv[3], nullptr); + instance->lissajous_params.C = strtof(argv[4], nullptr); + instance->lissajous_params.a = strtof(argv[5], nullptr); + instance->lissajous_params.b = strtof(argv[6], nullptr); + instance->lissajous_params.c = strtof(argv[7], nullptr); + instance->lissajous_params.duration = strtof(argv[8], nullptr); + instance->lissajous_params.ramp_duration = strtof(argv[9], nullptr); + instance->internal_reference_params_changed = true; + + PX4_INFO("Lissajous params set: A=%.2f B=%.2f C=%.2f fa=%.2f fb=%.2f fc=%.2f duration=%.2f ramp=%.2f \n", + (double)instance->lissajous_params.A, + (double)instance->lissajous_params.B, + (double)instance->lissajous_params.C, + (double)instance->lissajous_params.a, + (double)instance->lissajous_params.b, + (double)instance->lissajous_params.c, + (double)instance->lissajous_params.duration, + (double)instance->lissajous_params.ramp_duration); + + + return PX4_OK; + } + } + + return print_usage("unknown command"); +} + +int Raptor::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +RAPTOR Policy Flight Mode + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_raptor", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("intref", "Modify internal reference"); + PRINT_MODULE_USAGE_ARG("lissajous", "Set Lissajous trajectory parameters", false); + PRINT_MODULE_USAGE_ARG("", "Amplitude X [m]", false); + PRINT_MODULE_USAGE_ARG("", "Amplitude Y [m]", false); + PRINT_MODULE_USAGE_ARG("", "Amplitude Z [m]", false); + PRINT_MODULE_USAGE_ARG("", "Frequency a", false); + PRINT_MODULE_USAGE_ARG("", "Frequency b", false); + PRINT_MODULE_USAGE_ARG("", "Frequency c", false); + PRINT_MODULE_USAGE_ARG("", "Total duration [s]", false); + PRINT_MODULE_USAGE_ARG("", "Ramp duration [s]", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_raptor_main(int argc, char *argv[]) +{ + return Raptor::main(argc, argv); +} diff --git a/src/modules/mc_raptor/mc_raptor.hpp b/src/modules/mc_raptor/mc_raptor.hpp new file mode 100644 index 0000000000..0ffb8e6d01 --- /dev/null +++ b/src/modules/mc_raptor/mc_raptor.hpp @@ -0,0 +1,283 @@ +#pragma once + +#include "trajectories/lissajous.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#undef OK + +#ifdef __PX4_POSIX +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "blob/policy.h" + +#include +#include +#include +#include +#include + +namespace rlt = rl_tools; + +#define MC_RAPTOR_POLICY_NAMESPACE rlt::checkpoint::actor +#define MC_RAPTOR_EXAMPLE_NAMESPACE rlt::checkpoint::example +#define MC_RAPTOR_META_NAMESPACE rlt::checkpoint::meta +// #define MC_RAPTOR_EMBED_POLICY // you can use this to directly embed the policy into the firmware instead of loading it from the sd card. To fit into the flash you might need to disable some unnecessary features in the .px4board config. + + + + + +using namespace time_literals; + +class Raptor : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + Raptor(); + ~Raptor() 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(); + + int print_status() override; + +private: +#ifdef __PX4_POSIX + using DEVICE = rlt::devices::DefaultCPU; +#else + using DEV_SPEC = rlt::devices::DefaultARMSpecification; + using DEVICE = rlt::devices::arm::OPT; +#endif + using TI = typename DEVICE::index_t; + using RNG = DEVICE::SPEC::RANDOM::ENGINE<>; + using T = float; + static constexpr uint64_t EXT_COMPONENT_REQUEST_ID = 1337; + DEVICE device; + RNG rng; + hrt_abstime init_time; + // node constants + static constexpr TI OBSERVATION_TIMEOUT_ANGULAR_VELOCITY = 10 * 1000; + static constexpr TI OBSERVATION_TIMEOUT_LOCAL_POSITION = 100 * 1000; + static constexpr TI OBSERVATION_TIMEOUT_ATTITUDE = 50 * 1000; + static constexpr TI TRAJECTORY_SETPOINT_TIMEOUT = 200 * 1000; + static constexpr T RESET_PREVIOUS_ACTION_VALUE = 0; // -1 to 1 + static constexpr bool ENABLE_CONTROL_FREQUENCY_INFO = false; + + T max_position_error = 0.5; + T max_velocity_error = 1.0; + + void Run() override; + + decltype(register_ext_component_reply_s::mode_id) ext_component_mode_id; + decltype(register_ext_component_reply_s::arming_check_id) ext_component_arming_check_id; + + enum class FlightModeState : TI { + UNREGISTERED = 0, + REGISTERED = 1, + CONFIGURED = 2 + }; + FlightModeState flightmode_state = FlightModeState::UNREGISTERED; + bool can_arm = false; + void updateArmingCheckReply(); + + // node state + vehicle_local_position_s _vehicle_local_position{}; + vehicle_angular_velocity_s _vehicle_angular_velocity{}; + vehicle_attitude_s _vehicle_attitude{}; + vehicle_status_s _vehicle_status{}; + trajectory_setpoint_s _trajectory_setpoint{}; + hrt_abstime timestamp_last_local_position, timestamp_last_angular_velocity, timestamp_last_attitude, timestamp_last_trajectory_setpoint, + timestamp_last_manual_control_input, timestamp_last_vehicle_status; + bool timestamp_last_local_position_set = false, timestamp_last_angular_velocity_set = false, timestamp_last_attitude_set = false, + timestamp_last_trajectory_setpoint_set = false, timestamp_last_manual_control_input_set = false, timestamp_last_vehicle_status_set = false; + bool timeout_message_sent = false; + bool previous_trajectory_setpoint_stale = false; + bool previous_active = false; + + T position[3]; + T linear_velocity[3]; + + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _arming_check_request_sub{ORB_ID(arming_check_request)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _raptor_status_pub{ORB_ID(raptor_status)}; + uORB::Publication _raptor_input_pub{ORB_ID(raptor_input)}; + uORB::Publication _tune_control_pub{ORB_ID(tune_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)}; + // Performance (perf) counters + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _loop_interval_policy_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval_policy")}; + + struct EXECUTOR_CONFIG { + static constexpr TI ACTION_HISTORY_LENGTH = 1; + static constexpr TI CONTROL_INTERVAL_INTERMEDIATE_NS = 2.5 * 1000 * 1000; // Inference is at 500hz + static constexpr TI CONTROL_INTERVAL_NATIVE_NS = 10 * 1000 * 1000; // Training is 100hz + static constexpr TI TIMING_STATS_NUM_STEPS = 100; + static constexpr bool FORCE_SYNC_INTERMEDIATE = true; + static constexpr bool FORCE_SYNC_NATIVE_RUNTIME = true; + static constexpr TI FORCE_SYNC_NATIVE = 8; + static constexpr bool DYNAMIC_ALLOCATION = false; + + using ACTOR_TYPE_ORIGINAL = MC_RAPTOR_POLICY_NAMESPACE ::TYPE; + using POLICY_TEST = MC_RAPTOR_POLICY_NAMESPACE ::TYPE::template CHANGE_BATCH_SIZE::template CHANGE_SEQUENCE_LENGTH; + using POLICY_BATCH_SIZE = ACTOR_TYPE_ORIGINAL::template CHANGE_BATCH_SIZE; +#ifdef MC_RAPTOR_EMBED_POLICY + using POLICY = POLICY_BATCH_SIZE; +#else + using POLICY = POLICY_BATCH_SIZE::template CHANGE_CAPABILITY>; +#endif + using TYPE_POLICY = typename POLICY::TYPE_POLICY; + +#if defined(__PX4_POSIX) + // Relax warning levels for Gazebo sitl. Because Gazebo SITL runs at 250Hz IMU rate, it is not a clean multiple of the training frequency (100hz), hence if the thresholds are set too strict, warnings will be triggered all the time. Generally, Raptor is quite robuts agains control frequency deviations. + struct WARNING_LEVELS: rlt::inference::executor::WarningLevelsDefault { + using T = typename TYPE_POLICY::DEFAULT; + static constexpr T INTERMEDIATE_TIMING_JITTER_HIGH_THRESHOLD = 2.0; + static constexpr T INTERMEDIATE_TIMING_JITTER_LOW_THRESHOLD = 0.5; + static constexpr T INTERMEDIATE_TIMING_BIAS_HIGH_THRESHOLD = 2.0; + static constexpr T INTERMEDIATE_TIMING_BIAS_LOW_THRESHOLD = 0.5; + static constexpr T NATIVE_TIMING_JITTER_HIGH_THRESHOLD = 2.0; + static constexpr T NATIVE_TIMING_JITTER_LOW_THRESHOLD = 0.5; + static constexpr T NATIVE_TIMING_BIAS_HIGH_THRESHOLD = 2.0; + static constexpr T NATIVE_TIMING_BIAS_LOW_THRESHOLD = 0.5; + }; +#else + struct WARNING_LEVELS: rlt::inference::executor::WarningLevelsDefault { + using T = typename TYPE_POLICY::DEFAULT; + static constexpr T NATIVE_TIMING_JITTER_HIGH_THRESHOLD = 1.5; + static constexpr T NATIVE_TIMING_JITTER_LOW_THRESHOLD = 0.5; + }; +#endif + using TIMESTAMP = hrt_abstime; + static constexpr TI OUTPUT_DIM = 4; + static constexpr TI TEST_SEQUENCE_LENGTH_ACTUAL = 5; + static constexpr TI TEST_BATCH_SIZE_ACTUAL = 2; + + using EXECUTOR_SPEC = + rl_tools::inference::applications::l2f::Specification; + using EXECUTOR_STATUS = rlt::inference::executor::Status; + }; + using EXECUTOR_SPEC = EXECUTOR_CONFIG::EXECUTOR_SPEC; + rl_tools::inference::applications::L2F executor; +#ifdef MC_RAPTOR_EMBED_POLICY + const decltype(MC_RAPTOR_POLICY_NAMESPACE ::module) &policy = MC_RAPTOR_POLICY_NAMESPACE::module; +#else + EXECUTOR_CONFIG::POLICY policy; +#endif + static constexpr TI CHECKPOINT_NAME_LENGTH = 100; + char checkpoint_name[CHECKPOINT_NAME_LENGTH] = "n/a"; + +#ifdef MC_RAPTOR_EMBED_POLICY + bool test_policy(); +#else + bool test_policy(FILE *f, TI input_offset, TI output_offset); +#endif + + void reset(); + void observe(rl_tools::inference::applications::l2f::Observation &observation); + + static constexpr bool REMAP_FROM_CRAZYFLIE = + true; // Policy (Crazyflie assignment) => Quadrotor (PX4 Quadrotor X assignment) PX4 SIH assumes the Quadrotor X configuration, which assumes different rotor positions than the crazyflie mapping (from crazyflie outputs to PX4): 1=>1, 2=>4, 3=>2, 4=>3 + // controller state + + // messaging state + static constexpr TI POLICY_INTERVAL_WARNING_THRESHOLD = 100; // us + static constexpr TI POLICY_FREQUENCY_CHECK_INTERVAL = 1000 * 1000; // 1s + static constexpr TI POLICY_FREQUENCY_INFO_INTERVAL = 10; // 10 x POLICY_FREQUENCY_CHECK_INTERVAL = 10x + static constexpr TI POLICY_CONTROL_FREQUENCY_TRAINING = 100; + + TI num_statii; + TI num_healthy_executor_statii_intermediate, num_non_healthy_executor_statii_intermediate, num_healthy_executor_statii_native, + num_non_healthy_executor_statii_native; + EXECUTOR_CONFIG::EXECUTOR_STATUS last_intermediate_status, last_native_status; + bool last_intermediate_status_set, last_native_status_set; + + TI policy_frequency_check_counter; + hrt_abstime timestamp_last_policy_frequency_check; + bool timestamp_last_policy_frequency_check_set = false; + + static constexpr TI NUM_TRAJECTORY_SETPOINT_DTS = 100; + int32_t trajectory_setpoint_dts[NUM_TRAJECTORY_SETPOINT_DTS]; + TI trajectory_setpoint_dt_index = 0; + TI trajectory_setpoint_dt_max_since_reset = 0; + bool trajectory_setpoint_dts_full = false; + + static constexpr TI TRAJECTORY_SETPOINT_INVALID_COUNT_WARNING_INTERVAL = 100; + TI trajectory_setpoint_invalid_count = 0; + + float previous_action[EXECUTOR_SPEC::OUTPUT_DIM]; + bool use_internal_reference = false; + bool internal_reference_params_changed = false; + T internal_reference_activation_position[3]; + T internal_reference_activation_orientation[4]; + hrt_abstime internal_reference_activation_time; + enum class InternalReference : TI { // make sure this corresponds to the enum values for MC_RAPTOR_INTREF in module.yaml + NONE = 0, + LISSAJOUS = 1 + }; + InternalReference internal_reference = InternalReference::NONE; + LissajousParameters lissajous_params{}; // Set via 'mc_raptor intref lissajous ...' command + DEFINE_PARAMETERS( + (ParamInt) _param_imu_gyro_ratemax, + (ParamBool) _param_mc_raptor_offboard, + (ParamInt) _param_mc_raptor_intref + ) + + +}; diff --git a/src/modules/mc_raptor/module.yaml b/src/modules/mc_raptor/module.yaml new file mode 100644 index 0000000000..ff036b05ea --- /dev/null +++ b/src/modules/mc_raptor/module.yaml @@ -0,0 +1,43 @@ +module_name: mc_raptor +parameters: + - group: Multicopter Raptor + definitions: + MC_RAPTOR_ENABLE: + description: + short: Enable Raptor flight mode + long: | + When enabled, the Raptor flight mode will be available. Please set MC_RAPTOR_OFFB according to your use case. + type: boolean + default: false + category: System + MC_RAPTOR_VERBOS: + description: + short: Enable verbose output + long: | + When enabled, the Raptor flight mode will print verbose output to the console. + type: boolean + default: false + category: System + + MC_RAPTOR_OFFB: + description: + short: Enable Offboard mode replacement + long: | + When enabled, the Raptor mode will replace the Offboard mode. + If disabled, the Raptor mode will be available as a separate external mode. In the latter case, Raptor will just hold the position, without requiring external setpoints. When Raptor replaces the Offboard mode, it requires external setpoints to be activated. + type: boolean + default: false + category: System + + MC_RAPTOR_INTREF: + description: + short: Use internal reference instead of trajectory_setpoint + long: | + When enabled, instead of using the trajectory_setpoint, the position and yaw of the vehicle at the point when the Raptor mode is activated will be used as reference. + Use 'mc_raptor intref lissajous ' to configure the trajectory. + type: enum + values: + 0: None + 1: Lissajous + default: 0 + category: System diff --git a/src/modules/mc_raptor/trajectories/lissajous.hpp b/src/modules/mc_raptor/trajectories/lissajous.hpp new file mode 100644 index 0000000000..340ead01bf --- /dev/null +++ b/src/modules/mc_raptor/trajectories/lissajous.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include "trajectory.hpp" +#include + +struct LissajousParameters { + float A = 0.5f; // amplitude a + float B = 1.0f; // amplitude b + float C = 0.0f; // amplitude c + float a = 2.0f; // frequency a + float b = 1.0f; // frequency b + float c = 1.0f; // frequency c + float duration = 10.0f; + float ramp_duration = 3.0f; +}; + +inline Setpoint lissajous(float time, const LissajousParameters ¶ms) +{ + float time_velocity = (params.ramp_duration > 0.0f) + ? fminf(time, params.ramp_duration) / params.ramp_duration + : 1.0f; + + float ramp_time = time_velocity * fminf(time, params.ramp_duration) / 2.0f; + float progress = (ramp_time + fmaxf(0.0f, time - params.ramp_duration)) * 2.0f * static_cast(M_PI) / params.duration; + float d_progress = 2.0f * static_cast(M_PI) * time_velocity / params.duration; + + Setpoint setpoint{}; + setpoint.position[0] = params.A * sinf(params.a * progress); + setpoint.position[1] = params.B * sinf(params.b * progress); + setpoint.position[2] = params.C * sinf(params.c * progress); + setpoint.yaw = 0.0f; + setpoint.linear_velocity[0] = params.A * cosf(params.a * progress) * params.a * d_progress; + setpoint.linear_velocity[1] = params.B * cosf(params.b * progress) * params.b * d_progress; + setpoint.linear_velocity[2] = params.C * cosf(params.c * progress) * params.c * d_progress; + setpoint.yaw_rate = 0.0f; + + return setpoint; +} diff --git a/src/modules/mc_raptor/trajectories/trajectory.hpp b/src/modules/mc_raptor/trajectories/trajectory.hpp new file mode 100644 index 0000000000..fcdf405701 --- /dev/null +++ b/src/modules/mc_raptor/trajectories/trajectory.hpp @@ -0,0 +1,6 @@ +struct Setpoint { + float position[3]; + float yaw; + float linear_velocity[3]; + float yaw_rate; +}; diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index 3d3a1cb6e6..bacf072cf0 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -76,6 +76,11 @@ void MecanumAttControl::updateAttControl() rover_attitude_setpoint_s rover_attitude_setpoint{}; _rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint); _yaw_setpoint = rover_attitude_setpoint.yaw_setpoint; + _last_yaw_setpoint_timestamp = hrt_absolute_time(); + } + + if (hrt_elapsed_time(&_last_yaw_setpoint_timestamp) > YAW_SETPOINT_TIMEOUT_US) { + _yaw_setpoint = NAN; } if (PX4_ISFINITE(_yaw_setpoint)) { diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index 327b4652ad..88879380bb 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -34,6 +34,7 @@ #pragma once // PX4 includes +#include #include #include @@ -52,6 +53,8 @@ #include #include +using namespace time_literals; + /** * @brief Class for mecanum attitude control. */ @@ -103,6 +106,10 @@ private: float _max_yaw_rate{0.f}; float _yaw_setpoint{NAN}; + hrt_abstime _last_yaw_setpoint_timestamp{0}; + /** Timeout in us for yaw setpoint to get considered invalid */ + static constexpr uint64_t YAW_SETPOINT_TIMEOUT_US = 500_ms; + // Controllers PID _pid_yaw; SlewRateYaw _adjusted_yaw_setpoint; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp index f217ada200..fec1fab36b 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp @@ -88,7 +88,10 @@ void MecanumOffboardMode::offboardControl() rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } else if (offboard_control_mode.attitude) { + } + + // For Mecanum wheel systems, attitude and position control can be decoupled + if (offboard_control_mode.attitude) { rover_attitude_setpoint_s rover_attitude_setpoint{}; rover_attitude_setpoint.timestamp = hrt_absolute_time(); rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index 3ec9be67d3..969bb28b30 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -47,8 +47,6 @@ MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent void MecanumPosControl::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; - } void MecanumPosControl::updatePosControl() @@ -89,10 +87,6 @@ void MecanumPosControl::updatePosControl() rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0); rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1); _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } else { rover_speed_setpoint_s rover_speed_setpoint{}; @@ -100,10 +94,6 @@ void MecanumPosControl::updatePosControl() rover_speed_setpoint.speed_body_x = 0.f; rover_speed_setpoint.speed_body_y = 0.f; _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { _stopped = true; @@ -124,7 +114,6 @@ void MecanumPosControl::updateSubscriptions() vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); } if (_vehicle_local_position_sub.updated()) { diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index 54880657ec..7e89f6c711 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -114,9 +114,6 @@ private: Vector2f _start_ned{}; Vector2f _target_waypoint_ned{}; float _arrival_speed{0.f}; - float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - float _yaw_setpoint{NAN}; float _vehicle_speed{0.f}; float _cruising_speed{NAN}; bool _stopped{false}; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 417001846c..27ff755741 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -36,6 +36,7 @@ #include #include #include +#include namespace sensors { @@ -95,7 +96,12 @@ void VehicleGPSPosition::ParametersUpdate(bool force) _gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC); _gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC); _gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get()); - _gps_blending.setPrimaryInstance(_param_sens_gps_prime.get()); + + const int gps_prime = _param_sens_gps_prime.get(); + + if (math::isInRange(gps_prime, -1, 1)) { + _gps_blending.setPrimaryInstance(gps_prime); + } } } @@ -113,6 +119,7 @@ void VehicleGPSPosition::Run() // Check all GPS instance bool any_gps_updated = false; bool gps_updated = false; + const int32_t gps_prime = _param_sens_gps_prime.get(); for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { gps_updated = _sensor_gps_sub[i].updated(); @@ -125,6 +132,16 @@ void VehicleGPSPosition::Run() _sensor_gps_sub[i].copy(&gps_data); _gps_blending.setGpsData(gps_data, i); + if (math::isInRange(static_cast(gps_prime), 2, 127)) { + device::Device::DeviceId device_id{}; + device_id.devid = gps_data.device_id; + + if (device_id.devid_s.bus_type == device::Device::DeviceBusType_UAVCAN + && device_id.devid_s.address == static_cast(gps_prime)) { + _gps_blending.setPrimaryInstance(i); + } + } + if (!_sensor_gps_sub[i].registered()) { _sensor_gps_sub[i].registerCallback(); } diff --git a/src/modules/sensors/vehicle_gps_position/params.c b/src/modules/sensors/vehicle_gps_position/params.c index 651e6e15fa..310c7bfc6b 100644 --- a/src/modules/sensors/vehicle_gps_position/params.c +++ b/src/modules/sensors/vehicle_gps_position/params.c @@ -70,13 +70,16 @@ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f); * send data to the EKF even if a secondary instance is already available. * The secondary instance is then only used if the primary one times out. * - * To have an equal priority of all the instances, set this parameter to -1 and - * the best receiver will be used. + * Accepted values: + * -1 : Auto (equal priority for all instances) + * 0 : Main serial GPS instance + * 1 : Secondary serial GPS instance + * 2-127 : UAVCAN module node ID * * This parameter has no effect if blending is active. * * @group Sensors * @min -1 - * @max 1 + * @max 127 */ PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0); diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 3e9d03fcc9..5988a5a92c 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -142,3 +142,14 @@ parameters: category: System reboot_required: true default: -1 + + UXRCE_DDS_FLCTRL: + description: + short: Enable serial flow control for UXRCE interface + long: | + This is used to enable flow control for the serial uXRCE instance. + Used for reliable high bandwidth communication. + type: boolean + category: System + reboot_required: true + default: 0 diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index d5f2b57042..5eff22af36 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -837,8 +837,16 @@ bool UxrceddsClient::setBaudrate(int fd, unsigned baud) // uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - /* no parity, one stop bit, disable flow control */ - uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + /* enable flow control if needed */ + if (_param_uxrce_dds_flctrl.get() > 0) { + uart_config.c_cflag |= CRTSCTS; + + } else { + uart_config.c_cflag &= ~CRTSCTS; + } /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index 7b5208a80e..c5c60b1784 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -210,6 +210,7 @@ private: (ParamInt) _param_uxrce_dds_syncc, (ParamInt) _param_uxrce_dds_synct, (ParamInt) _param_uxrce_dds_tx_to, - (ParamInt) _param_uxrce_dds_rx_to + (ParamInt) _param_uxrce_dds_rx_to, + (ParamInt) _param_uxrce_dds_flctrl ) };