second batch of 1.15 changes

Tools/astyle/check_code_style.sh

@@ -3,20 +3,22 @@
 FILE=$1
 DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
 
-CHECK_FAILED=$(${DIR}/fix_code_style.sh --dry-run --formatted $FILE)
-if [ -n "$CHECK_FAILED" ]; then
-	${DIR}/fix_code_style.sh --quiet < $FILE > $FILE.pretty
+if [ -f "$FILE" ]; then
+	CHECK_FAILED=$(${DIR}/fix_code_style.sh --dry-run --formatted $FILE)
+	if [ -n "$CHECK_FAILED" ]; then
+		${DIR}/fix_code_style.sh --quiet < $FILE > $FILE.pretty
 
-	echo -e 'Formatting issue found in' $FILE
-	echo
-	git --no-pager diff --no-index --minimal --histogram --color=always $FILE $FILE.pretty | grep -vE -e "^.{,4}diff.*\.pretty.{,3}$" -e "^.{,4}--- a/.*$" -e "^.{,4}\+\+\+ b/.*$" -e "^.{,5}@@ .* @@.*$" -e "^.{,4}index .{10}\.\."
-	rm -f $FILE.pretty
-	echo
+		echo -e 'Formatting issue found in' $FILE
+		echo
+		git --no-pager diff --no-index --minimal --histogram --color=always $FILE $FILE.pretty | grep -vE -e "^.{,4}diff.*\.pretty.{,3}$" -e "^.{,4}--- a/.*$" -e "^.{,4}\+\+\+ b/.*$" -e "^.{,5}@@ .* @@.*$" -e "^.{,4}index .{10}\.\."
+		rm -f $FILE.pretty
+		echo
 
-	if [[ $PX4_ASTYLE_FIX -eq 1 ]]; then
-		${DIR}/fix_code_style.sh $FILE
-	else
-		echo 'to fix automatically run "make format" or "./Tools/astyle/fix_code_style.sh' $FILE'"'
-		exit 1
+		if [[ $PX4_ASTYLE_FIX -eq 1 ]]; then
+			${DIR}/fix_code_style.sh $FILE
+		else
+			echo 'to fix automatically run "make format" or "./Tools/astyle/fix_code_style.sh' $FILE'"'
+			exit 1
+		fi
 	fi
 fi

Tools/astyle/files_to_check_code_style.sh

@@ -31,4 +31,4 @@ exec find boards msg src platforms test \
     -path src/lib/cdrstream/rosidl -prune -o \
     -path src/modules/zenoh/zenoh-pico -prune -o \
     -path boards/modalai/voxl2/libfc-sensor-api -prune -o \
-    \( -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) -print \) | grep $PATTERN
+    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

Tools/check_submodules.sh

@@ -6,7 +6,7 @@ function check_git_submodule {
 if [[ -f $1"/.git" || -d $1"/.git" ]]; then
 
 	# always update within CI environment or configuring withing VSCode CMake where you can't interact
-	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ] || [ -n "${CLION_IDE+set}" ]; then
+	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ]; then
 		git submodule --quiet sync --recursive -- $1
 		git submodule --quiet update --init --recursive --jobs=8 -- $1  || true
 		git submodule --quiet sync --recursive -- $1

Tools/module_config/output_groups_from_timer_config.py

@@ -34,41 +34,33 @@ def extract_timer(line):
     if search:
         return search.group(1), 'generic'
 
-    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
-    search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,\)]', line, re.IGNORECASE)
+    # nxp rt1062 format: initIOPWM(PWM::FlexPWM2),
+    search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
     if search:
-        return (search.group(1) + '_' +  search.group(2)), 'imxrt'
+        return search.group(1), 'imxrt'
 
     return None, 'unknown'
 
-def extract_timer_from_channel(line, timer_names):
+def extract_timer_from_channel(line, num_channels_already_found):
     # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
     search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE)
     if search:
         return search.group(1)
 
-    # NXP FlexPWM format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
-    search = re.search('PWM::(PWM[0-9]+).*PWM::Submodule([0-9])', line, re.IGNORECASE)
+    # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
+    search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE)
     if search:
-        return str(timer_names.index((search.group(1) + '_' +  search.group(2))))
+        # imxrt uses a 1:1 timer group to channel association
+        return str(num_channels_already_found)
 
     return None
 
-def imxrt_is_dshot(line):
-
-    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
-    search = re.search('(initIOPWMDshot)', line, re.IGNORECASE)
-    if search:
-        return True
-
-    return False
-
 def get_timer_groups(timer_config_file, verbose=False):
     with open(timer_config_file, 'r') as f:
         timer_config = f.read()
 
     # timers
-    dshot_support = {str(i): False for i in range(16)}
+    dshot_support = {} # key: timer
     timers_start_marker = 'io_timers_t io_timers'
     timers_start = timer_config.find(timers_start_marker)
     if timers_start == -1:
@@ -77,7 +69,6 @@ def get_timer_groups(timer_config_file, verbose=False):
     open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose)
     timers_str = timer_config[open_idx:close_idx]
     timers = []
-    timer_names = []
     for line in timers_str.splitlines():
         line = line.strip()
         if len(line) == 0 or line.startswith('//'):
@@ -86,11 +77,14 @@ def get_timer_groups(timer_config_file, verbose=False):
 
         if timer_type == 'imxrt':
             if verbose: print('imxrt timer found')
-            timer_names.append(timer)
-            if imxrt_is_dshot(line):
-                dshot_support[str(len(timers))] = True
-            timers.append(str(len(timers)))
-        elif timer:
+            max_num_channels = 16 # Just add a fixed number of timers
+            timers = [str(i) for i in range(max_num_channels)]
+            dshot_support = {str(i): False for i in range(max_num_channels)}
+            for i in range(8): # First 8 channels support dshot
+                dshot_support[str(i)] = True
+            break
+
+        if timer:
             if verbose: print('found timer def: {:}'.format(timer))
             dshot_support[timer] = 'DMA' in line
             timers.append(timer)
@@ -117,7 +111,7 @@ def get_timer_groups(timer_config_file, verbose=False):
             continue
 
         if verbose: print('--'+line+'--')
-        timer = extract_timer_from_channel(line, timer_names)
+        timer = extract_timer_from_channel(line, len(channel_timers))
 
         if timer:
             if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line))

Tools/px4moduledoc/srcparser.py

@@ -15,7 +15,7 @@ class ModuleDocumentation(object):
     # TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md
     valid_categories = ['driver', 'estimator', 'controller', 'system',
                         'communication', 'command', 'template', 'simulation', 'autotune']
-    valid_subcategories = ['', 'camera', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
+    valid_subcategories = ['', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
                            'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']
 
     max_line_length = 80 # wrap lines that are longer than this

Tools/setup/arch.sh

@@ -95,7 +95,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 
 	# java (jmavsim)
 	sudo pacman -S --noconfirm --needed \
-		ant \
+		ant
 		;
 
 	# Gazebo setup

Tools/setup/requirements.txt

@@ -1,4 +1,5 @@
 argcomplete
+argparse>=1.2
 cerberus
 coverage
 empy==3.3.4

Tools/setup/ubuntu.sh

@@ -66,8 +66,6 @@ elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 	echo "Ubuntu 20.04"
 elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
 	echo "Ubuntu 22.04"
-elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
-	echo "Linux Mint 21.3"
 fi
 
 
@@ -148,7 +146,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		util-linux \
 		vim-common \
 		;
-	if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" || "${UBUNTU_RELEASE}" == "21.3" ]]; then
+	if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" ]]; then
 		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		kconfig-frontends \
 		;
@@ -207,8 +205,6 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		java_version=13
 	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
 		java_version=11
-	elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
-		java_version=11
 	else
 		java_version=14
 	fi
@@ -232,17 +228,6 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
 		sudo apt-get update -y --quiet
 
-		# Install Gazebo
-		gazebo_packages="gz-garden"
-	elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
-		echo "Gazebo (Garden) will be installed"
-		echo "Earlier versions will be removed"
-		# Add Gazebo binary repository
-		sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
-		echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable jammy main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
-
-		sudo apt-get update -y --quiet
-
 		# Install Gazebo
 		gazebo_packages="gz-garden"
 	else

src/modules/airspeed_selector/AirspeedValidator.cpp

@@ -50,16 +50,16 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	// get indicated airspeed from input data (raw airspeed)
 	_IAS = input_data.airspeed_indicated_raw;
 
-	update_CAS_scale_validated(input_data.gnss_valid, input_data.ground_velocity, input_data.airspeed_true_raw);
+	update_CAS_scale_validated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw);
 	update_CAS_scale_applied();
 	update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
-	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.gnss_valid,
+	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid,
 			      input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.q_att);
 	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
 	check_airspeed_data_stuck(input_data.timestamp);
 	check_load_factor(input_data.accel_z);
 	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
-				  input_data.ground_velocity, input_data.gnss_valid);
+				  input_data.ground_velocity, input_data.lpos_valid);
 	update_airspeed_valid_status(input_data.timestamp);
 }
 
@@ -71,12 +71,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)
 }
 
 void
-AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool gnss_valid,
+AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
 		const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att)
 {
 	_wind_estimator.update(time_now_usec);
 
-	if (gnss_valid && _in_fixed_wing_flight) {
+	if (lpos_valid && _in_fixed_wing_flight) {
 
 		// airspeed fusion (with raw TAS)
 		const float hor_vel_variance =  lpos_evh * lpos_evh;
@@ -109,9 +109,9 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
 }
 
 void
-AirspeedValidator::update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw)
+AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw)
 {
-	if (!_in_fixed_wing_flight || !gnss_valid) {
+	if (!_in_fixed_wing_flight || !lpos_valid) {
 		return;
 	}
 
@@ -212,7 +212,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
 
 void
 AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
-		float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid)
+		float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid)
 {
 	// Check normalised innovation levels with requirement for continuous data and use of hysteresis
 	// to prevent false triggering.
@@ -222,13 +222,16 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 	}
 
 	// reset states if check is disabled, we are not flying or wind estimator was just initialized/reset
-	if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s) {
+	if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s
+	    || _tas_innov_integ_threshold <= 0.f) {
 		_innovations_check_failed = false;
+		_time_last_tas_pass = time_now;
 		_aspd_innov_integ_state = 0.f;
 
-	} else if (!gnss_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
+	} else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
 		//nav velocity data is likely not good
 		//don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good
+		_time_last_tas_pass = time_now;
 		_aspd_innov_integ_state = 0.f;
 
 	} else {
@@ -246,7 +249,11 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 			_aspd_innov_integ_state = 0.f;
 		}
 
-		_innovations_check_failed = _aspd_innov_integ_state > _tas_innov_integ_threshold;
+		if (_tas_innov_integ_threshold > 0.f && _aspd_innov_integ_state < _tas_innov_integ_threshold) {
+			_time_last_tas_pass = time_now;
+		}
+
+		_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;
 	}
 
 	_time_last_aspd_innov_check = time_now;

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -56,7 +56,7 @@ struct airspeed_validator_update_data {
 	float airspeed_true_raw;
 	uint64_t airspeed_timestamp;
 	matrix::Vector3f ground_velocity;
-	bool gnss_valid;
+	bool lpos_valid;
 	float lpos_evh;
 	float lpos_evv;
 	matrix::Quatf q_att;
@@ -150,7 +150,9 @@ class AirspeedValidator
 	float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure
 	float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure
 	uint64_t	_time_last_aspd_innov_check{0};	///< time airspeed innovation was last checked (uSec)
+	uint64_t	_time_last_tas_pass{0};		///< last time innovation checks passed
 	float		_aspd_innov_integ_state{0.0f};	///< integral of excess normalised airspeed innovation (sec)
+	static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s};	///< time required for innovation levels to pass or fail (usec)
 	uint64_t	_time_wind_estimator_initialized{0};		///< time last time wind estimator was initialized (uSec)
 
 	// states of load factor check
@@ -160,8 +162,8 @@ class AirspeedValidator
 
 	// states of airspeed valid declaration
 	bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed)
-	float _checks_fail_delay{2.f}; ///< delay for airspeed invalid declaration after single check failure (Sec)
-	float _checks_clear_delay{-1.f}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
+	int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec)
+	int _checks_clear_delay{-1}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
 	uint64_t	_time_checks_passed{0};	///< time the checks have last passed (uSec)
 	uint64_t	_time_checks_failed{0};	///< time the checks have last not passed (uSec)
 
@@ -175,15 +177,15 @@ class AirspeedValidator
 
 	void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; }
 
-	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool gnss_valid,
+	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid,
 				   const matrix::Vector3f &vI,
 				   float lpos_evh, float lpos_evv, const Quatf &q_att);
-	void update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw);
+	void update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw);
 	void update_CAS_scale_applied();
 	void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
 	void check_airspeed_data_stuck(uint64_t timestamp);
 	void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
-				       float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid);
+				       float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid);
 	void check_load_factor(float accel_z);
 	void update_airspeed_valid_status(const uint64_t timestamp);
 	void reset();