Merge branch 'main' into pr-tighter-altitude-tracking

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -15,14 +15,29 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
 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 1
+param set-default SENS_EN_ARSPDSIM 0
 # We can arm and drive in manual mode when it slides and GPS check fails:
 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
+# Rover parameters
+param set-default RD_WHEEL_TRACK 0.9
+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 1
+param set-default RD_MAX_JERK 3
+param set-default RD_MAX_SPEED 8
+param set-default RD_HEADING_P 5
+param set-default RD_HEADING_I 0.1
+param set-default RD_MAX_YAW_RATE 30
+param set-default RD_MISS_SPD_DEF 8
+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
 
 # Actuator mapping - set SITL motors/servos output parameters:
 
@@ -36,7 +51,7 @@ param set-default SIM_GZ_WH_FUNC1 101 # right wheel
 param set-default SIM_GZ_WH_FUNC2 102 # left wheel
 #param set-default SIM_GZ_WH_MIN2 0
 #param set-default SIM_GZ_WH_MAX2 200
-#aram set-default SIM_GZ_WH_DIS2 100
+#param set-default SIM_GZ_WH_DIS2 100
 #param set-default SIM_GZ_WH_FAIL2 100
 
 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels

boards/mro/ctrl-zero-classic/nuttx-config/include/board.h

@@ -197,6 +197,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h

@@ -197,6 +197,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

boards/mro/ctrl-zero-h7/nuttx-config/include/board.h

@@ -196,6 +196,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

boards/mro/pixracerpro/nuttx-config/include/board.h

@@ -196,6 +196,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

boards/px4/fmu-v6x/default.px4board

@@ -18,6 +18,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
+CONFIG_DRIVERS_GPIO_MCP23009=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y

boards/px4/fmu-v6x/init/rc.board_defaults

@@ -27,3 +27,14 @@ else
 fi
 
 safety_button start
+
+# GPIO Expander driver on external I2C3
+if ver hwbasecmp 009
+then
+	# No USB
+	mcp23009 start -b 3 -X -D 0xf0 -O 0xf0 -P 0x0f -U 10
+fi
+if ver hwbasecmp 00a 008
+then
+	mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10
+fi

boards/px4/fmu-v6x/nuttx-config/nsh/defconfig

@@ -91,6 +91,7 @@ CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_GPIO=y
 CONFIG_DEV_PIPE_MAXSIZE=1024
 CONFIG_DEV_PIPE_SIZE=70
 CONFIG_DEV_URANDOM=y

boards/px4/fmu-v6x/src/CMakeLists.txt

@@ -34,7 +34,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
 	add_compile_definitions(BOOTLOADER)
 	add_library(drivers_board
 		bootloader_main.c
-		init.c
+		init.cpp
 		usb.c
 		timer_config.cpp
 	)
@@ -52,7 +52,7 @@ else()
 	add_library(drivers_board
 		can.c
 		i2c.cpp
-		init.c
+		init.cpp
 		led.c
 		mtd.cpp
 		sdio.c
@@ -71,5 +71,6 @@ else()
 			nuttx_arch # sdio
 			nuttx_drivers # sdio
 			px4_layer
+			platform_gpio_mcp23009
 	)
 endif()

boards/px4/fmu-v6x/src/board_config.h

@@ -270,6 +270,11 @@
 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PH2  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2)
 #define GPIO_VDD_3V3_SD_CARD_EN         /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
 
+/* MCP23009 GPIO expander */
+#define BOARD_GPIO_VDD_5V_COMP_VALID           "/dev/gpio4"
+#define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID      "/dev/gpio5"
+
+
 /* Spare GPIO */
 
 #define GPIO_PG6                        /* PG6  */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6)

boards/px4/fmu-v6x/src/init.c → boards/px4/fmu-v6x/src/init.cpp

@@ -74,6 +74,7 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_determine_hw_info.h>
 #include <px4_platform/board_dma_alloc.h>
+#include <px4_platform/gpio/mcp23009.hpp>
 
 /****************************************************************************
  * Pre-Processor Definitions
@@ -159,7 +160,7 @@ __EXPORT void board_on_reset(int status)
  *
  ************************************************************************************/
 
-__EXPORT void
+extern "C" __EXPORT void
 stm32_boardinitialize(void)
 {
 	board_on_reset(-1); /* Reset PWM first thing */
@@ -280,6 +281,13 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 #  endif /* CONFIG_MMCSD */
 
+	ret = mcp23009_register_gpios(3, 0x25);
+
+	if (ret != OK) {
+		led_on(LED_RED);
+		return ret;
+	}
+
 #endif /* !defined(BOOTLOADER) */
 
 	return OK;

msg/NavigatorMissionItem.msg

@@ -1,7 +1,5 @@
 uint64 timestamp                 # time since system start (microseconds)
 
-uint32 instance_count            # Instance count of this mission. Increments monotonically whenever the mission is modified
-
 uint16 sequence_current          # Sequence of the current mission item
 
 uint16 nav_cmd

msg/VehicleAttitudeSetpoint.msg

@@ -1,9 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
 
-float32 roll_body		# body angle in NED frame (can be NaN for FW)
-float32 pitch_body		# body angle in NED frame (can be NaN for FW)
-float32 yaw_body		# body angle in NED frame (can be NaN for FW)
-
 float32 yaw_sp_move_rate	# rad/s (commanded by user)
 
 # For quaternion-based attitude control

src/lib/battery/battery.cpp

@@ -108,10 +108,6 @@ void Battery::updateCurrent(const float current_a)
 
 void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 {
-	if (!_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) {
-		resetInternalResistanceEstimation(_voltage_v, _current_a);
-	}
-
 	// Require minimum voltage otherwise override connected status
 	if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
 		_connected = false;
@@ -121,9 +117,13 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 		_last_unconnected_timestamp = timestamp;
 	}
 
-	// wait with initializing filters to avoid relying on a voltage sample from the rising edge
+	// Wait with initializing filters to avoid relying on a voltage sample from the rising edge
 	_battery_initialized = _connected && (timestamp > _last_unconnected_timestamp + 2_s);
 
+	if (_connected && !_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) {
+		resetInternalResistanceEstimation(_voltage_v, _current_a);
+	}
+
 	sumDischarged(timestamp, _current_a);
 	_state_of_charge_volt_based =
 		calculateStateOfChargeVoltageBased(_voltage_v, _current_a);

src/lib/parameters/px4params/markdownout.py

@@ -26,12 +26,18 @@ def __init__(self, groups):
             for param in group.GetParams():
                 name = param.GetName()
                 short_desc = param.GetFieldValue("short_desc") or ''
+
+                # Add fullstop to short_desc if not present
                 if short_desc:
                     if not short_desc.strip().endswith('.'):
                         short_desc += "."
-                #short_desc = html.escape(short_desc)
+
                 long_desc = param.GetFieldValue("long_desc") or ''
-                #long_desc = html.escape(long_desc)
+
+                #Strip out short text from start of long text, if it ends in fullstop
+                if long_desc.startswith(short_desc):
+                    long_desc = long_desc[len(short_desc):].lstrip()
+
                 min_val = param.GetFieldValue("min") or ''
                 max_val = param.GetFieldValue("max") or ''
                 increment = param.GetFieldValue("increment") or ''
@@ -81,7 +87,7 @@ def __init__(self, groups):
                 if bitmask_list:
                     result += bitmask_output
                 # Format the ranges as a table.
-                result += f"Reboot | minValue | maxValue | increment | default | unit\n--- | --- | --- | --- | --- | ---\n{reboot_required} | {min_val} | {max_val} | {increment} | {def_val} | {unit} \n\n"
+                result += f"Reboot | minValue | maxValue | increment | default | unit\n--- | --- | --- | --- | --- | ---\n{'✓' if reboot_required else ' ' } | {min_val} | {max_val} | {increment} | {def_val} | {unit} \n\n"
 
         self.output = result
 

src/lib/pure_pursuit/PurePursuit.hpp

@@ -95,9 +95,9 @@ class PurePursuit : public ModuleParams
 	 * @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m].
 	 * @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m].
 	 * @param vehicle_speed Vehicle speed [m/s].
-	 * @param RA_LOOKAHD_GAIN Tuning parameter [-]
-	 * @param RA_LOOKAHD_MAX Maximum lookahead distance [m]
-	 * @param RA_LOOKAHD_MIN Minimum lookahead distance [m]
+	 * @param PP_LOOKAHD_GAIN Tuning parameter [-]
+	 * @param PP_LOOKAHD_MAX Maximum lookahead distance [m]
+	 * @param PP_LOOKAHD_MIN Minimum lookahead distance [m]
 	 */
 	float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
 				 float vehicle_speed);

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -533,19 +533,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 	estimator_status_flags_s estimator_status_flags;
 
 	if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
-
-		bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
-				      || estimator_status_flags.cs_inertial_dead_reckoning;
-
-		if (!dead_reckoning) {
-			// position requirements (update if not dead reckoning)
-			bool gps             = estimator_status_flags.cs_gps;
-			bool optical_flow    = estimator_status_flags.cs_opt_flow;
-			bool vision_position = estimator_status_flags.cs_ev_pos;
-
-			_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
-		}
-
 		// Check for a magnetometer fault and notify the user
 		if (estimator_status_flags.cs_mag_fault) {
 			/* EVENT
@@ -722,15 +709,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 	// Check if quality checking of position accuracy and consistency is to be performed
 	const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get();
 
-	float lpos_eph_threshold_relaxed = lpos_eph_threshold;
-
-	// Set the allowable position uncertainty based on combination of flight and estimator state
-	// When we are in a operator demanded position control mode and are solely reliant on optical flow,
-	// do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
-	if (_position_reliant_on_optical_flow) {
-		lpos_eph_threshold_relaxed = INFINITY;
-	}
-
 	bool xy_valid = lpos.xy_valid && !_nav_test_failed;
 	bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
 
@@ -793,6 +771,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp,
 				     _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
 
+
+	// In some modes we assume that the operator will compensate for the drift so we do not need to check the position error
+	const float lpos_eph_threshold_relaxed = INFINITY;
+
 	failsafe_flags.local_position_invalid_relaxed =
 		!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
 				     _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -100,8 +100,6 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 	bool		_nav_test_passed{false};	///< true if the post takeoff navigation test has passed
 	bool		_nav_test_failed{false};	///< true if the post takeoff navigation test has failed
 
-	bool _position_reliant_on_optical_flow{false};
-
 	bool _gps_was_fused{false};
 	bool _gnss_spoofed{false};
 

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -82,11 +82,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_position_relaxed);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_manual_control);
 
-	if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);
-		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_global_position);
-	}
-
 	// NAVIGATION_STATE_AUTO_MISSION
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude);