Compare commits

..

2 Commits

Author SHA1 Message Date
Daniel Agar 153b683a0e module.h fix trivial whitespace astyle failure 2024-06-20 15:50:25 -04:00
Thomas Debrunner eba9276e6f system: never call task_delete on nuttx, never force stop tasks 2024-06-20 13:31:51 +02:00
706 changed files with 8217 additions and 13616 deletions
-1
View File
@@ -16,7 +16,6 @@ jobs:
matrix:
check: [
"check_format",
"check_newlines",
"tests",
"tests_coverage",
"px4_fmu-v2_default stack_check",
+1
View File
@@ -54,3 +54,4 @@ jobs:
AWS_REGION: 'us-west-1'
SOURCE_DIR: 'build/${{ matrix.target }}/_metadata/'
DEST_DIR: 'Firmware/${{ env.version }}/${{ matrix.target }}/'
@@ -11,8 +11,6 @@ on:
jobs:
build:
runs-on: ubuntu-latest
env:
ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
strategy:
fail-fast: false
matrix:
@@ -11,8 +11,6 @@ on:
jobs:
build:
runs-on: ubuntu-latest
env:
ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
strategy:
fail-fast: false
matrix:
+1 -1
View File
@@ -71,7 +71,7 @@
[submodule "src/modules/zenoh/zenoh-pico"]
path = src/modules/zenoh/zenoh-pico
url = https://github.com/px4/zenoh-pico
branch = dev/1.0.0-px4
branch = pr-zubf-werror-fix
[submodule "src/lib/heatshrink/heatshrink"]
path = src/lib/heatshrink/heatshrink
url = https://github.com/PX4/heatshrink.git
+1 -1
View File
@@ -2,4 +2,4 @@
{
"name": "PX4 detect"
}
]
]
-11
View File
@@ -185,17 +185,6 @@ menu "Serial ports"
string "EXT2 tty port"
endmenu
menu "File paths"
config BOARD_ROOT_PATH
string "PX4 Root file path"
default "/fs/microsd"
config BOARD_PARAM_FILE
string "Parameter file"
default "/fs/mtd_params"
endmenu
menu "drivers"
source "src/drivers/Kconfig"
endmenu
+2 -6
View File
@@ -379,9 +379,9 @@ doxygen:
@$(PX4_MAKE) -C "$(SRC_DIR)"/build/doxygen
@touch "$(SRC_DIR)"/build/doxygen/Documentation/.nojekyll
# Style
# Astyle
# --------------------------------------------------------------------
.PHONY: check_format format check_newlines
.PHONY: check_format format
check_format:
$(call colorecho,'Checking formatting with astyle')
@@ -392,10 +392,6 @@ format:
$(call colorecho,'Formatting with astyle')
@"$(SRC_DIR)"/Tools/astyle/check_code_style_all.sh --fix
check_newlines:
$(call colorecho,'Checking for missing or duplicate newlines at the end of files')
@"$(SRC_DIR)"/Tools/astyle/check_newlines.sh
# Testing
# --------------------------------------------------------------------
.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance
-5
View File
@@ -120,7 +120,6 @@ add_custom_command(
${romfs_gen_root_dir}/init.d/rc.serial
${romfs_gen_root_dir}/init.d/rc.autostart
${romfs_gen_root_dir}/init.d/rc.autostart.post
${romfs_gen_root_dir}/init.d/rc.filepaths
${romfs_copy_stamp}
COMMAND ${CMAKE_COMMAND} -E remove_directory ${romfs_gen_root_dir}/*
COMMAND ${CMAKE_COMMAND} -E tar xf ${romfs_tar_file}
@@ -132,9 +131,6 @@ add_custom_command(
--rc-dir ${romfs_gen_root_dir}/init.d
--serial-ports ${board_serial_ports} ${added_arguments}
--config-files ${module_config_files} #--verbose
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/filepaths/generate_config.py
--rc-dir ${romfs_gen_root_dir}/init.d
--params-file ${CONFIG_BOARD_PARAM_FILE}
COMMAND ${CMAKE_COMMAND} -E touch ${romfs_copy_stamp}
WORKING_DIRECTORY ${romfs_gen_root_dir}
DEPENDS ${romfs_tar_file}
@@ -324,7 +320,6 @@ add_custom_target(romfs_gen_files_target
DEPENDS
${romfs_copy_stamp}
${romfs_gen_root_dir}/init.d/rc.serial
${romfs_gen_root_dir}/init.d/rc.filepaths
romfs_extras.stamp
)
+14 -3
View File
@@ -21,14 +21,25 @@ set R /
#
ver all
# Load param file location from kconfig
. ${R}etc/init.d/rc.filepaths
if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params
then
set PARAM_FILE /fs/mtd_params
fi
if mft query -q -k MTD -s MTD_PARAMETERS -v /dev/eeeprom0
then
set PARAM_FILE /dev/eeeprom0
fi
if mft query -q -k MTD -s MTD_PARAMETERS -v /mnt/qspi/params
then
set PARAM_FILE /mnt/qspi/params
fi
#
# Load parameters.
#
param select $PARAM_FILE
if ! param load
then
param reset_all
@@ -29,3 +29,4 @@ param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
@@ -30,3 +30,4 @@ param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default EKF2_RNG_A_HMAX 10
@@ -94,3 +94,4 @@ param set-default CA_METHOD 0
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
@@ -12,3 +12,4 @@ param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
@@ -30,3 +30,4 @@ param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default LPE_FUSION 242
@@ -18,3 +18,4 @@ param set-default LPE_FUSION 132
# AEQ: External heading set to use vision input
param set-default ATT_EXT_HDG_M 1
@@ -41,3 +41,4 @@ param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
@@ -61,3 +61,4 @@ param set-default PWM_MAIN_FUNC5 105
param set-default PWM_MAIN_FUNC6 106
param set-default PWM_MAIN_FUNC7 107
param set-default PWM_MAIN_FUNC8 108
@@ -68,3 +68,5 @@ param set-default PWM_MAIN_FUNC7 202
param set-default PWM_MAIN_FUNC8 203
param set-default PWM_MAIN_FUNC9 206
param set-default PWM_MAIN_REV 256
@@ -39,6 +39,7 @@ param set-default FW_T_SINK_MIN 2.2
param set-default FW_W_EN 1
param set-default MIS_LTRMIN_ALT 30
param set-default MIS_TAKEOFF_ALT 30
param set-default NAV_ACC_RAD 15
@@ -41,6 +41,7 @@ param set-default FW_T_SINK_MIN 2.2
param set-default FW_W_EN 1
param set-default MIS_LTRMIN_ALT 30
param set-default MIS_TAKEOFF_ALT 30
param set-default NAV_ACC_RAD 15
@@ -70,3 +71,4 @@ param set-default PWM_MAIN_FUNC7 202
param set-default PWM_MAIN_FUNC8 203
param set-default PWM_MAIN_FUNC9 206
param set-default PWM_MAIN_REV 256
@@ -55,3 +55,4 @@ param set-default PWM_MAIN_FUNC7 202
param set-default PWM_MAIN_FUNC8 203
param set-default PWM_MAIN_FUNC9 206
param set-default PWM_MAIN_REV 256
@@ -54,3 +54,4 @@ param set-default PWM_MAIN_FUNC7 202
param set-default PWM_MAIN_FUNC8 203
param set-default PWM_MAIN_FUNC9 206
param set-default PWM_MAIN_REV 256
@@ -55,3 +55,4 @@ param set-default PWM_MAIN_FUNC7 202
param set-default PWM_MAIN_FUNC8 203
param set-default PWM_MAIN_FUNC9 206
param set-default PWM_MAIN_REV 256
@@ -62,3 +62,4 @@ param set-default PWM_MAIN_FUNC7 202
param set-default PWM_MAIN_FUNC8 203
param set-default PWM_MAIN_FUNC9 206
param set-default PWM_MAIN_REV 256
@@ -39,6 +39,7 @@ param set-default FW_T_SINK_MIN 2.2
param set-default FW_W_EN 1
param set-default MIS_LTRMIN_ALT 30
param set-default MIS_TAKEOFF_ALT 30
param set-default NAV_ACC_RAD 15
@@ -23,6 +23,7 @@ param set-default FW_RR_P 0.085
param set-default FW_W_EN 1
param set-default MIS_LTRMIN_ALT 30
param set-default MIS_TAKEOFF_ALT 20
param set-default NAV_ACC_RAD 15
@@ -78,3 +78,4 @@ param set-default VT_FWD_THRUST_EN 4
param set-default VT_FWD_THRUST_SC 1
param set-default VT_F_TRANS_THR 0.75
param set-default VT_TYPE 2
@@ -75,3 +75,4 @@ param set-default VT_F_TRANS_DUR 1.5
param set-default VT_TYPE 0
param set-default WV_EN 0
@@ -88,3 +88,4 @@ param set-default PWM_MAIN_FUNC11 422
param set-default RC_MAP_AUX1 8
param set-default RC_MAP_AUX2 9
param set-default RC_MAP_AUX3 10
@@ -39,6 +39,7 @@ param set-default FW_T_SINK_MIN 2.2
param set-default FW_W_EN 1
param set-default MIS_LTRMIN_ALT 30
param set-default MIS_TAKEOFF_ALT 30
param set-default NAV_ACC_RAD 15
@@ -31,3 +31,4 @@ param set-default PWM_MAIN_FUNC1 201
param set-default PWM_MAIN_FUNC2 201
param set-default PWM_MAIN_FUNC6 101
param set-default PWM_MAIN_FUNC7 101
@@ -38,3 +38,4 @@ param set-default PWM_MAIN_FUNC1 201
param set-default PWM_MAIN_FUNC2 201
param set-default PWM_MAIN_FUNC6 101
param set-default PWM_MAIN_FUNC7 101
@@ -41,3 +41,4 @@ param set-default CA_R_REV 3
param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
@@ -64,3 +64,4 @@ param set-default PWM_MAIN_FUNC4 203
param set-default PWM_MAIN_FUNC5 407
param set-default PWM_MAIN_FUNC6 408
param set-default PWM_MAIN_FUNC7 409
@@ -69,3 +69,4 @@ param set-default PWM_MAIN_FUNC4 203
param set-default PWM_MAIN_FUNC5 407
param set-default PWM_MAIN_FUNC6 408
param set-default PWM_MAIN_FUNC7 409
@@ -36,3 +36,4 @@ param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 201
param set-default PWM_MAIN_FUNC4 103
@@ -6,36 +6,16 @@
. ${R}etc/init.d/rc.rover_differential_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
param set-default SIM_GZ_EN 1 # Gazebo bridge
# Rover parameters
param set-default RD_WHEEL_TRACK 0.3
param set-default RD_MAN_YAW_SCALE 0.1
param set-default RD_YAW_RATE_I 0.1
param set-default RD_YAW_RATE_P 5
param set-default RD_MAX_ACCEL 6
param set-default RD_MAX_JERK 30
param set-default RD_MAX_SPEED 7
param set-default RD_HEADING_P 5
param set-default RD_HEADING_I 0.1
param set-default RD_MAX_YAW_RATE 180
param set-default RD_MISS_SPD_DEF 7
param set-default RD_TRANS_DRV_TRN 0.349066
param set-default RD_TRANS_TRN_DRV 0.174533
# Pure pursuit parameters
param set-default PP_LOOKAHD_MAX 30
param set-default PP_LOOKAHD_MIN 2
param set-default PP_LOOKAHD_GAIN 1
# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 0
param set-default SENS_EN_ARSPDSIM 1
# Actuator mapping
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
@@ -20,9 +20,9 @@ param set-default SENS_EN_ARSPDSIM 1
param set-default COM_ARM_WO_GPS 1
# Set Differential Drive Kinematics Library parameters:
param set RD_WHEEL_BASE 0.9
param set RD_WHEEL_RADIUS 0.22
param set RD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h
param set RDD_WHEEL_BASE 0.9
param set RDD_WHEEL_RADIUS 0.22
param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h
# Actuator mapping - set SITL motors/servos output parameters:
@@ -41,9 +41,9 @@ param set-default SIM_GZ_WH_FUNC2 102 # left wheel
param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation.
# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate
# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator
# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation.
# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate
# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator
# controls in practical scenarios.
# Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
@@ -1,49 +0,0 @@
#!/bin/sh
# @name Rover Ackermann
# @type Rover
# @class Rover
. ${R}etc/init.d/rc.rover_ackermann_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann}
param set-default SIM_GZ_EN 1 # Gazebo bridge
# Rover parameters
param set-default NAV_ACC_RAD 0.5
param set-default RA_ACC_RAD_GAIN 2
param set-default RA_ACC_RAD_MAX 3
param set-default RA_MAX_ACCEL 1.5
param set-default RA_MAX_JERK 15
param set-default RA_MAX_SPEED 3
param set-default RA_MAX_STR_ANG 0.5236
param set-default RA_MAX_STR_RATE 360
param set-default RA_MISS_VEL_DEF 3
param set-default RA_MISS_VEL_GAIN 5
param set-default RA_MISS_VEL_MIN 1
param set-default RA_SPEED_I 0.01
param set-default RA_SPEED_P 2
param set-default RA_WHEEL_BASE 0.321
# Pure Pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
param set-default PP_LOOKAHD_MAX 10
param set-default PP_LOOKAHD_MIN 1
# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 0
# Wheels
param set-default SIM_GZ_WH_FUNC1 101
param set-default SIM_GZ_WH_MIN1 0
param set-default SIM_GZ_WH_MAX1 200
param set-default SIM_GZ_WH_DIS1 100
# Steering
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_REV 1
@@ -1,10 +0,0 @@
#!/bin/sh
#
# @name Gazebo x500 lidar
#
# @type Quadrotor
#
PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar}
. ${R}etc/init.d-posix/airframes/4001_gz_x500
@@ -63,3 +63,4 @@ param set-default PWM_MAIN_FUNC9 422
# Landing gear
param set-default PWM_MAIN_FUNC10 400
param set-default PWM_MAIN_FUNC11 400
@@ -119,3 +119,4 @@ param set-default CA_METHOD 0
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
@@ -83,8 +83,6 @@ px4_add_romfs_files(
4009_gz_r1_rover
4010_gz_x500_mono_cam
4011_gz_lawnmower
4012_gz_rover_ackermann
4013_gz_x500_lidar
6011_gazebo-classic_typhoon_h480
6011_gazebo-classic_typhoon_h480.post
@@ -15,9 +15,6 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
if [ -n "${PX4_HOME_LON}" ]; then
param set SIH_LOC_LON0 ${PX4_HOME_LON}
fi
if [ -n "${PX4_HOME_ALT}" ]; then
param set SIH_LOC_H0 ${PX4_HOME_ALT}
fi
if simulator_sih start; then
-1
View File
@@ -157,7 +157,6 @@ param set-default CBRK_SUPPLY_CHK 894281
# disable check, no CPU load reported on posix yet
param set-default COM_CPU_MAX -1
param set-default COM_RAM_MAX -1
# Don't require RC calibration and configuration
param set-default COM_RC_IN_MODE 1
+1 -2
View File
@@ -44,7 +44,6 @@ px4_add_romfs_files(
# TODO
rc.balloon_apps
rc.balloon_defaults
rc.sysinit
)
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
@@ -78,7 +77,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
)
endif()
if(CONFIG_MODULES_ROVER_DIFFERENTIAL)
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
px4_add_romfs_files(
rc.rover_differential_apps
rc.rover_differential_defaults
@@ -44,3 +44,4 @@ param set-default CA_ROTOR4_KM -0.05
param set-default CA_ROTOR5_PX 0.25
param set-default CA_ROTOR5_PY -0.433
param set-default CA_ROTOR5_PZ 0.05
@@ -119,3 +119,5 @@ param set-default PWM_MAIN_TIM0 50
param set-default PWM_MAIN_DIS1 1500
param set-default PWM_MAIN_DIS2 1500
param set-default PWM_MAIN_DIS4 1500
@@ -32,3 +32,4 @@ param set-default CA_SV_TL0_MAXA 45
param set-default CA_SV_TL0_MINA -45
param set-default CA_SV_TL0_TD 90
param set-default CA_SV_TL_COUNT 1
@@ -24,3 +24,5 @@ param set-default MC_PITCHRATE_D 0
param set-default MC_PITCHRATE_FF 0.1
param set-default CA_AIRFRAME 10
@@ -23,3 +23,4 @@ param set-default MAV_0_MODE 1
param set-default MAV_0_CONFIG 102
param set-default GPS_UBX_DYNMODEL 8
param set-default SER_TEL2_BAUD 9600
@@ -72,3 +72,5 @@ param set-default PWM_MAIN_DIS5 1000
param set-default PWM_MAIN_DIS6 1500
param set-default PWM_MAIN_DIS7 1500
param set-default PWM_MAIN_DIS8 1500
@@ -66,3 +66,4 @@ param set-default CA_ROTOR11_PX -0.344
param set-default CA_ROTOR11_PY -0.25
param set-default CA_ROTOR11_PZ -0.05
param set-default CA_ROTOR11_KM -0.05
@@ -38,3 +38,4 @@ param set-default CA_SV_CS_COUNT 1
param set-default CA_SV_CS0_TRQ_P 1
param set-default CA_R_REV 7
@@ -6,7 +6,6 @@
# @class Copter
#
# @board px4_fmu-v2 exclude
# @board px4_fmu-v5x exclude
# @board bitcraze_crazyflie exclude
#
# @maintainer Iain Galloway <iain.galloway@nxp.com>
@@ -7,10 +7,6 @@
#
# @maintainer
# @board px4_fmu-v2 exclude
# @board cuav_x7pro exclude
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5x exclude
# @board px4_fmu-v6x exclude
#
. ${R}etc/init.d/rc.mc_defaults
@@ -40,6 +36,7 @@ param set-default COM_RC_LOSS_T 3
# ekf2
param set-default EKF2_TERR_MASK 1
param set-default EKF2_BARO_NOISE 2
param set-default EKF2_BCOEF_X 31.5
@@ -12,7 +12,6 @@
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5 exclude
# @board px4_fmu-v5x exclude
# @board px4_fmu-v6x exclude
# @board bitcraze_crazyflie exclude
# @board cuav_x7pro exclude
#
@@ -10,7 +10,6 @@
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5 exclude
# @board px4_fmu-v5x exclude
# @board px4_fmu-v6x exclude
# @board bitcraze_crazyflie exclude
# @board cuav_x7pro exclude
#
@@ -8,7 +8,6 @@
# @maintainer Oleg Kalachev <okalachev@gmail.com>
#
# @board px4_fmu-v2 exclude
# @board px4_fmu-v4pro exclude
# @board bitcraze_crazyflie exclude
#
@@ -25,6 +25,7 @@
param set-default BAT1_CAPACITY 4000
param set-default BAT1_N_CELLS 6
param set-default BAT1_V_EMPTY 3.3
param set-default BAT1_V_LOAD_DROP 0.5
param set-default BAT_AVRG_CURRENT 13
# Square quadrotor X PX4 numbering
@@ -68,9 +69,6 @@ param set-default MPC_THR_MIN 0.025
param set-default MPC_VEL_MANUAL 5.0
param set-default MPC_XY_VEL_MAX 8.0
param set-default RC_CRSF_PRT_CFG 300
param set-default RC_CRSF_TEL_EN 1
param set-default RTL_RETURN_ALT 15
param set-default SENS_FLOW_MINHGT 0.0
@@ -13,7 +13,6 @@
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5 exclude
# @board px4_fmu-v5x exclude
# @board px4_fmu-v6x exclude
# @board diatone_mamba-f405-mk2 exclude
#
. ${R}etc/init.d/rc.mc_defaults
@@ -1,6 +1,6 @@
#!/bin/sh
#
# @name Generic Ground Vehicle (Deprecated)
# @name Generic Ground Vehicle (Ackermann)
#
# @type Rover
# @class Rover
@@ -48,3 +48,5 @@ param set-default PWM_MAIN_FUNC1 201
param set-default PWM_MAIN_FUNC2 201
param set-default PWM_MAIN_FUNC6 101
param set-default PWM_MAIN_FUNC7 101
@@ -1,12 +0,0 @@
#!/bin/sh
#
# @name Generic Rover Differential
#
# @type Rover
# @class Rover
#
# @board px4_fmu-v2 exclude
# @board bitcraze_crazyflie exclude
#
. ${R}etc/init.d/rc.rover_differential_defaults
@@ -15,6 +15,9 @@
param set-default BAT1_N_CELLS 4
param set-default EKF2_GBIAS_INIT 0.01
param set-default EKF2_ANGERR_INIT 0.01
# Set geometry & output configration
param set-default RBCLW_ADDRESS 128
param set-default RBCLW_FUNC1 101
@@ -1,6 +1,6 @@
#!/bin/sh
#
# @name NXP Cup car: DF Robot GPX (Deprecated)
# @name NXP Cup car: DF Robot GPX
#
# @type Rover
@@ -1,6 +1,6 @@
#!/bin/sh
#
# @name Generic Rover Ackermann
# @name Generic ackermann rover
#
# @type Rover
# @class Rover
@@ -1,37 +0,0 @@
#!/bin/sh
#
# @name Axial SCX10 2 Trail Honcho
#
# @url https://www.axialadventure.com/product/1-10-scx10-ii-trail-honcho-4wd-rock-crawler-brushed-rtr/AXID9059.html
#
# @type Rover
# @class Rover
#
# @board px4_fmu-v2 exclude
# @board bitcraze_crazyflie exclude
#
. ${R}etc/init.d/rc.rover_ackermann_defaults
param set-default BAT1_N_CELLS 3
# Rover parameters
param set-default NAV_ACC_RAD 0.5
param set-default RA_ACC_RAD_GAIN 2
param set-default RA_ACC_RAD_MAX 3
param set-default RA_MAX_ACCEL 0.5
param set-default RA_MAX_JERK 10
param set-default RA_MAX_SPEED 2.7
param set-default RA_MAX_STR_ANG 0.5236
param set-default RA_MAX_STR_RATE 270
param set-default RA_MISS_VEL_DEF 2.7
param set-default RA_MISS_VEL_GAIN 3.5
param set-default RA_MISS_VEL_MIN 1
param set-default RA_SPEED_I 0.1
param set-default RA_SPEED_P 0.5
param set-default RA_WHEEL_BASE 0.321
# Pure pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
param set-default PP_LOOKAHD_MAX 10
param set-default PP_LOOKAHD_MIN 1.5
@@ -42,3 +42,4 @@ param set-default CA_ROTOR3_KM 0
param set-default CA_ROTOR3_PX 0
param set-default CA_ROTOR3_PY -0.3
param set-default CA_ROTOR3_PZ 0.3
@@ -31,3 +31,5 @@ param set-default CA_ROTOR4_PY 0.25
param set-default CA_ROTOR5_PX -0.43
param set-default CA_ROTOR5_PY -0.25
param set-default CA_ROTOR5_KM -0.05
@@ -123,3 +123,4 @@ param set-default PWM_MAIN_FUNC6 106
param set-default PWM_MAIN_TIM0 -1
param set-default PWM_MAIN_TIM1 -1
param set-default PWM_MAIN_TIM2 -1
@@ -31,3 +31,5 @@ param set-default CA_ROTOR4_PY -0.43
param set-default CA_ROTOR5_PX -0.25
param set-default CA_ROTOR5_PY 0.43
param set-default CA_ROTOR5_KM -0.05
@@ -36,3 +36,5 @@ param set-default CA_ROTOR6_PY -0.46
param set-default CA_ROTOR7_KM -0.05
param set-default CA_ROTOR7_PX -0.19
param set-default CA_ROTOR7_PY 0.46
@@ -36,3 +36,4 @@ param set-default CA_ROTOR6_PY -0.5
param set-default CA_ROTOR7_KM -0.05
param set-default CA_ROTOR7_PX 0
param set-default CA_ROTOR7_PY 0.5
@@ -136,27 +136,22 @@ if(CONFIG_MODULES_VTOL_ATT_CONTROL)
)
endif()
if(CONFIG_MODULES_ROVER_DIFFERENTIAL)
if(CONFIG_MODULES_ROVER_POS_CONTROL)
px4_add_romfs_files(
# [50000, 50999] Differential rovers
50000_generic_rover_differential
50001_aion_robotics_r1_rover
50000_generic_ground_vehicle
50004_nxpcup_car_dfrobot_gpx
)
endif()
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
px4_add_romfs_files(
50003_aion_robotics_r1_rover
)
endif()
if(CONFIG_MODULES_ROVER_ACKERMANN)
px4_add_romfs_files(
# [51000, 51999] Ackermann rovers
51000_generic_rover_ackermann
51001_axial_scx10_2_trail_honcho
)
endif()
if(CONFIG_MODULES_ROVER_POS_CONTROL)
px4_add_romfs_files(
# [59000, 59999] Rover position control (deprecated)
59000_generic_ground_vehicle
59001_nxpcup_car_dfrobot_gpx
50010_ackermann_rover_generic
)
endif()
@@ -1,7 +1,10 @@
#!/bin/sh
# Standard apps for an ackermann rover.
# Standard apps for a ackermann drive rover.
# Start rover ackermann module.
# Start the attitude and position estimator.
ekf2 start &
# Start rover ackermann drive controller.
rover_ackermann start
# Start Land Detector.
@@ -2,10 +2,12 @@
# Ackermann rover parameters.
set VEHICLE_TYPE rover_ackermann
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
param set-default CA_AIRFRAME 5 # Rover (Ackermann)
param set-default CA_R_REV 1 # Motor is assumed to be reversible
param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying
param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
param set-default CA_AIRFRAME 5 # Rover (Ackermann)
param set-default CA_R_REV 1 # Motor is assumed to be reversible
param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
param set-default EKF2_GBIAS_INIT 0.01
param set-default EKF2_ANGERR_INIT 0.01
param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
param set-default NAV_RCL_ACT 6 # Disarm on manual control loss
param set-default COM_FAIL_ACT_T 1 # Delay before failsafe after rc loss
@@ -1,8 +1,8 @@
#!/bin/sh
# Standard apps for a differential rover.
# Standard apps for a differential drive rover.
# Start rover differential module.
rover_differential start
# Start rover differential drive controller.
differential_drive start
# Start Land Detector.
land_detector start rover
@@ -2,10 +2,10 @@
# Differential rover parameters.
set VEHICLE_TYPE rover_differential
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
param set-default CA_AIRFRAME 6 # Rover (Differential)
param set-default CA_R_REV 3 # Right and left motors reversible
param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying
param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
param set-default EKF2_GBIAS_INIT 0.01
param set-default EKF2_ANGERR_INIT 0.01
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
param set-default CA_AIRFRAME 6 # Rover (Differential)
param set-default CA_R_REV 3 # Right and left motors reversible
param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
-4
View File
@@ -1,4 +0,0 @@
#!/bin/sh
#
# Standard system init script
#
+18 -18
View File
@@ -120,8 +120,14 @@ then
. $FRC
else
# Load param file location from kconfig
. ${R}etc/init.d/rc.filepaths
#
# Set the parameter file the board supports params on
# MTD device.
#
if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params
then
set PARAM_FILE /fs/mtd_params
fi
# Check if /fs/mtd_params is a valid BSON file
if ! bsondump docsize /fs/mtd_caldata
@@ -211,31 +217,25 @@ else
fi
unset BOARD_RC_DEFAULTS
# Load airframe configuration based on SYS_AUTOSTART parameter
#
# Set parameters and env variables for selected SYS_AUTOSTART.
#
set AUTOSTART_PATH etc/init.d/rc.autostart
if ! param compare SYS_AUTOSTART 0
then
# rc.autostart directly run the right airframe script which sets the VEHICLE_TYPE
# Look for airframe in ROMFS
. ${R}etc/init.d/rc.autostart
if [ ${VEHICLE_TYPE} == none ]
if param greater SYS_AUTOSTART 1000000
then
# Look for airframe on SD card
# Use external startup file
if [ $SDCARD_AVAILABLE = yes ]
then
. ${R}etc/init.d/rc.autostart_ext
set AUTOSTART_PATH etc/init.d/rc.autostart_ext
else
echo "ERROR [init] SD card not mounted - can't load external airframe"
echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
fi
fi
if [ ${VEHICLE_TYPE} == none ]
then
echo "ERROR [init] No airframe file found for SYS_AUTOSTART value"
param set SYS_AUTOSTART 0
tune_control play error
fi
. ${R}$AUTOSTART_PATH
fi
unset AUTOSTART_PATH
# Check parameter version and reset upon airframe configuration version mismatch.
# Reboot required because "param reset_all" would reset all "param set" lines from airframe.
+8 -6
View File
@@ -45,7 +45,7 @@ function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals )
% * center - ellispoid center coordinates [xc; yc; zc]
% * ax - ellipsoid radii [a; b; c]
% * evecs - ellipsoid radii directions as columns of the 3x3 matrix
% * v - the 9 parameters describing the ellipsoid algebraically:
% * v - the 9 parameters describing the ellipsoid algebraically:
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
%
% Author:
@@ -59,7 +59,7 @@ end
if flag == 2 && nargin == 2
equals = 'xy';
end
if size( X, 2 ) ~= 3
error( 'Input data must have three columns!' );
else
@@ -69,7 +69,7 @@ else
end
% need nine or more data points
if length( x ) < 9 && flag == 0
if length( x ) < 9 && flag == 0
error( 'Must have at least 9 points to fit a unique ellipsoid' );
end
if length( x ) < 6 && flag == 1
@@ -91,7 +91,7 @@ if flag == 0
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * y, ...
2 * z ]; % ndatapoints x 9 ellipsoid parameters
elseif flag == 1
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
@@ -99,7 +99,7 @@ elseif flag == 1
y .* y, ...
z .* z, ...
2 * x, ...
2 * y, ...
2 * y, ...
2 * z ]; % ndatapoints x 6 ellipsoid parameters
elseif flag == 2
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
@@ -127,7 +127,7 @@ else
% fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
D = [ x .* x + y .* y + z .* z, ...
2 * x, ...
2 * y, ...
2 * y, ...
2 * z ]; % ndatapoints x 4 sphere parameters
end
@@ -170,3 +170,5 @@ else
radii = ( sqrt( gam ./ v( 1:3 ) ) )';
evecs = eye( 3 );
end
-19
View File
@@ -1,19 +0,0 @@
#!/usr/bin/env bash
return_value=0
# Check if there are files checked in that don't end in a newline (POSIX requirement)
git grep --cached -Il '' | xargs -L1 bash -c 'if test "$(tail -c 1 "$0")"; then echo "No new line at end of $0"; exit 1; fi'
if [ $? -ne 0 ]; then
return_value=1
fi
# Check if there are files checked in that have duplicate newlines at the end (fail trailing whitespace checks)
git grep --cached -Il '' | xargs -L1 bash -c 'if tail -c 2 "$0" | ( read x && read y && [ x"$x" = x ] && [ x"$y" = x ]); then echo "Multiple newlines at the end of $0"; exit 1; fi'
if [ $? -ne 0 ]; then
return_value=1
fi
exit $return_value
+1 -2
View File
@@ -18,8 +18,7 @@ exec find boards msg src platforms test \
-path src/lib/events/libevents -prune -o \
-path src/lib/parameters/uthash -prune -o \
-path src/lib/wind_estimator/python/generated -prune -o \
-path src/modules/ekf2/EKF/python/ekf_derivation/generated -prune -o \
-path src/modules/ekf2/EKF/yaw_estimator/derivation/generated -prune -o \
-path src/modules/ekf2/EKF -prune -o \
-path src/modules/gyro_fft/CMSIS_5 -prune -o \
-path src/modules/mavlink/mavlink -prune -o \
-path test/mavsdk_tests/catch2 -prune -o \
+1
View File
@@ -170,3 +170,4 @@ if(__name__ == "__main__"):
fs.write(f.read())
except:
pass
+4
View File
@@ -139,3 +139,7 @@ def find_checks_that_apply(
innov_fail_checks.append('ofy')
return sensor_checks, innov_fail_checks
+1 -1
View File
@@ -54,7 +54,7 @@ def calculate_sensor_metrics(
# calculates peak, mean, percentage above 0.5 std, and percentage above std metrics for
# estimator status variables
for signal, result_id in [('hgt_test_ratio', 'hgt'),
('hdg_test_ratio', 'mag'),
('mag_test_ratio', 'mag'),
('vel_test_ratio', 'vel'),
('pos_test_ratio', 'pos'),
('tas_test_ratio', 'tas'),
+1 -1
View File
@@ -84,4 +84,4 @@ def main() -> None:
if __name__ == '__main__':
main()
main()
+2 -2
View File
@@ -160,7 +160,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
# plot normalised innovation test levels
# define variables to plot
variables = [['hdg_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'], ['hgt_test_ratio']]
variables = [['mag_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'], ['hgt_test_ratio']]
y_labels = ['mag', 'vel, pos', 'hgt']
legend = [['mag'], ['vel', 'pos'], ['hgt']]
if np.amax(estimator_status['hagl_test_ratio']) > 0.0: # plot hagl test ratio, if applicable
@@ -223,7 +223,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
data_plot.save()
data_plot.close()
# plot innovation flags summary
# plot innovation_check_flags summary
data_plot = CheckFlagsPlot(
status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos',
'reject_hagl'],
-61
View File
@@ -1,61 +0,0 @@
#!/usr/bin/env python3
""" Script to generate Serial rc.filepaths for the ROMFS startup script """
import argparse
import os
import sys
try:
from jinja2 import Environment, FileSystemLoader
except ImportError as e:
print("Failed to import jinja2: " + str(e))
print("")
print("You may need to install it using:")
print(" pip3 install --user jinja2")
print("")
sys.exit(1)
try:
import yaml
except ImportError as e:
print("Failed to import yaml: " + str(e))
print("")
print("You may need to install it using:")
print(" pip3 install --user pyyaml")
print("")
sys.exit(1)
parser = argparse.ArgumentParser(description='Generate PX4 ROMFS filepaths')
parser.add_argument('--config-files', type=str, nargs='*', default=[],
help='YAML module config file(s)')
parser.add_argument('--constrained-flash', action='store_true',
help='Reduce verbosity in ROMFS scripts to reduce flash size')
parser.add_argument('--rc-dir', type=str, action='store',
help='ROMFS output directory', default=None)
parser.add_argument('--params-file', type=str, action='store',
help='Parameter output file', default=None)
parser.add_argument('-v', '--verbose', dest='verbose', action='store_true',
help='Verbose Output')
args = parser.parse_args()
verbose = args.verbose
constrained_flash = args.constrained_flash
rc_filepaths_output_dir = args.rc_dir
rc_filepaths_template = 'rc.filepaths.jinja'
jinja_env = Environment(loader=FileSystemLoader(
os.path.dirname(os.path.realpath(__file__))))
# generate the ROMFS script using a jinja template
if rc_filepaths_output_dir is not None:
rc_filepath_output_file = os.path.join(rc_filepaths_output_dir, "rc.filepaths")
if verbose: print("Generating {:}".format(rc_filepath_output_file))
template = jinja_env.get_template(rc_filepaths_template)
with open(rc_filepath_output_file, 'w') as fid:
fid.write(template.render(constrained_flash=constrained_flash, params_file=args.params_file))
else:
raise Exception("--rc-dir needs to be specified")
-6
View File
@@ -1,6 +0,0 @@
{# jinja template to generate the serial autostart script. #}
# serial autostart script generated with generate_serial_config.py
set PARAM_FILE {{ params_file }}
+1
View File
@@ -243,3 +243,4 @@ def main():
if __name__ == '__main__':
main()
+1
View File
@@ -282,3 +282,4 @@ def main():
if __name__ == '__main__':
main()
+1 -1
View File
@@ -107,4 +107,4 @@ plt.show()
##plt.plot(airspeed_corrected,(airspeed_corrected-airspeed_raw)/airspeed_corrected)
##plt.xlabel('airspeed [m/s]')
##plt.ylabel('relative error [-]')
##plt.show()
##plt.show()
+1 -1
View File
@@ -47,4 +47,4 @@ for field in spec.parsed_fields():
"main_orb_id": @( all_topics.index(name_snake_case) if name_snake_case in all_topics else -1 ),
"dependencies": @( json.dumps(list(set(dependencies))) ),
"name": "@( spec.full_name )"
}
}
+1 -1
View File
@@ -1 +1 @@
__all__ = ["srcscanner", "srcparser", "xmlout", "rcout"]
__all__ = ["srcscanner", "srcparser", "xmlout", "rcout"]
+6
View File
@@ -79,6 +79,12 @@ class RCOutput():
result += "then\n"
result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n"
result += "\t. /etc/init.d/airframes/${AIRFRAME}\n"
if not post_start:
result += "else\n"
result += "\techo \"ERROR [init] No file matches SYS_AUTOSTART value found in : /etc/init.d/airframes\"\n"
# Reset the configuration
result += "\tparam set SYS_AUTOSTART 0\n"
result += "\ttone_alarm ${TUNE_ERR}\n"
result += "fi\n"
result += "unset AIRFRAME"
self.output = result
+1
View File
@@ -330,3 +330,4 @@ if serial_params_output_file is not None:
fid.write(template.render(serial_devices=serial_devices,
ethernet_configuration=ethernet_configuration,
commands=commands, serial_ports=serial_ports))
+1
View File
@@ -30,3 +30,4 @@ unset PRT
unset PRT_F
unset BAUD_PARAM
unset MAV_ARGS
+1
View File
@@ -70,3 +70,4 @@ PARAM_DEFINE_INT32({{ command.port_param_name }}, {{ command.default_port }});
{% endfor -%}
{% endif %}

Some files were not shown because too many files have changed in this diff Show More