Modalai v1.12.3 testing

ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb

@@ -16,10 +16,29 @@ param set-default BAT1_N_CELLS 6
 # Log everything
 param set-default SDLOG_PROFILE 5
 param set-default SDLOG_MODE 2
+param set-default COM_ARM_SDCARD 1
 
 # update control defaults
-param set-default MPC_Z_VEL_MAX_UP 3
-param set-default MPC_Z_VEL_MAX_DN 3
+param set-default MPC_Z_VEL_MAX_UP 1.5
+param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_POS_MODE 1
+param set-default MPC_MAN_TILT_MAX 20
+param set-default MPC_THR_HOVER 0.1
+
+# selective disabling of various arming / disarming checks
+param set-default FD_FAIL_R 0		# allow arming at any Roll angle
+param set-default FD_FAIL_P 0		# allow arming at any Pitch angle
+param set-default COM_ARM_CHK_ESCS 0	# ignore ESCS arming check
+param set-default COM_ARM_CHK_ACC 0	# ignore Accelerometer arming checking
+param set-default COM_ARM_CHK_BIAS 0	# ignore EKF2 sensor bias arming check
+param set-default COM_DISARM_PRFLT -1.0	# don't automatically disarm if preflight checks fail
+param set-default COM_DISARM_LAND -1.0	# don't automatically disarm when landed
+#param set-default COM_ARM_CHK_PREF 0	# disable all pre-flight arm checks
+
+# misc.
+param set-default MC_AIRMODE 0 		# can consider using '1' for roll/pitch. Don't use '2' b/c yaw has issues.
+param set-default EKF2_GND_EFF_DZ 4 	# this is default but we initially had it set to 0. Should we go back to 0?
+#param set-default CAL_MAG0_ROT 6 	# YAW 270 for yhid pixhawk/holybro board
+param set-default LND_FLIGHT_UNCRT 1 	# don't trust the landing detector for arm / disarm checks.
 
 set MIXER quad_w

boards/modalai/fc-v2/src/hw_config.h

@@ -71,7 +71,7 @@
 #define INTERFACE_USART                1
 #define INTERFACE_USART_CONFIG         "/dev/ttyS0,115200"
 #define BOOT_DELAY_ADDRESS             0x000001a0
-#define BOARD_TYPE                     41776
+#define BOARD_TYPE                     53
 #define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
 #define BOARD_FLASH_SECTORS            (15)
 #define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)

boards/modalai/sitl/default.cmake

@@ -16,38 +16,38 @@ px4_add_board(
 	# 	TEL2:/dev/ttyS4 # UART5  / J1
 	# 	TEL3:/dev/ttyS1 # USART2 / J4
 	DRIVERS
-		adc/ads1115
+		#adc/ads1115
 		# adc/board_adc
-		barometer # all available barometer drivers
-		batt_smbus
+		#barometer # all available barometer drivers
+		#batt_smbus
 		camera_capture
 		camera_trigger
-		differential_pressure # all available differential pressure drivers
-		distance_sensor # all available distance sensor drivers
+		#differential_pressure # all available differential pressure drivers
+		#distance_sensor # all available distance sensor drivers
 		# dshot
 		gps
 		# imu/bosch/bmi088
-		imu/invensense/icm20602
-		imu/invensense/icm20948 # required for ak09916 mag
-		imu/invensense/icm42688p
-		irlock
-		lights # all available light drivers
+		#imu/invensense/icm20602
+		#imu/invensense/icm20948 # required for ak09916 mag
+		#imu/invensense/icm42688p
+		#irlock
+		#lights # all available light drivers
 		# magnetometer # all available magnetometer drivers
 		# optical_flow # all available optical flow drivers
-		osd
+		#osd
 		# pca9685
-		pca9685_pwm_out
-		power_monitor/ina226
-		power_monitor/voxlpm
+		#pca9685_pwm_out
+		#power_monitor/ina226
+		#power_monitor/voxlpm
 		# protocol_splitter
 		# pwm_input
 		pwm_out_sim
 		# pwm_out
-		rc_input
-		roboclaw
-		rpm
+		#rc_input
+		#roboclaw
+		#rpm
 		rpm/rpm_simulator
-		smart_battery/batmon
+		#smart_battery/batmon
 		# safety_button
 		# telemetry # all available telemetry drivers
 		# uavcan

src/drivers/uart_esc/modalai_esc/modalai_esc.cpp

@@ -57,6 +57,8 @@ ModalaiEsc::ModalaiEsc() :
 	_esc_status.counter            = 0;
 	_esc_status.esc_count          = MODALAI_ESC_OUTPUT_CHANNELS;
 	_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
+	_esc_status.esc_online_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1;
+	_esc_status.esc_armed_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1;
 
 	for (unsigned i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) {
 		_esc_status.esc[i].timestamp       = 0;
@@ -67,7 +69,7 @@ ModalaiEsc::ModalaiEsc() :
 		_esc_status.esc[i].esc_voltage     = 0;
 		_esc_status.esc[i].esc_current     = 0;
 		_esc_status.esc[i].esc_temperature = 0;
-	  _esc_status.esc[i].esc_errorcount	 = 0;
+		_esc_status.esc[i].esc_errorcount	 = 0;
 		_esc_status.esc[i].failures        = 0;
 	}
 
@@ -450,7 +452,7 @@ int ModalaiEsc::custom_command(int argc, char *argv[])
 	uint32_t repeat_delay_us = 10000;
 
 	if (argc < 3) {
-		return print_usage("unknown command");
+		return print_usage("Not enough input arguments.");
 	}
 
 	const char *verb = argv[argc - 1];
@@ -513,7 +515,7 @@ int ModalaiEsc::custom_command(int argc, char *argv[])
 			break;
 
 		default:
-			print_usage("Unknown command");
+			print_usage("Unknown argument flag.");
 			return 0;
 		}
 	}
@@ -702,7 +704,7 @@ int ModalaiEsc::custom_command(int argc, char *argv[])
 
 			PX4_INFO("ESC map: %d %d %d %d",map[0].number,map[1].number,map[2].number,map[3].number);
 			PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb);
-      PX4_INFO("Sending UART ESC power command %i", rate);
+      			PX4_INFO("Sending UART ESC power command %i", rate);
 
 
 			return get_instance()->sendCommandThreadSafe(&cmd);
@@ -1083,7 +1085,7 @@ bool ModalaiEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS]
 	}
 
 	// publish the actual command that we sent and the feedback received
-	if (0) {
+	{
 		actuator_outputs_s actuator_outputs{};
 		actuator_outputs.noutputs = num_outputs;
 

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -100,19 +100,25 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	/* ---- ACCEL ---- */
 	{
-		/* check all sensors individually, but fail only for mandatory ones */
-		for (unsigned i = 0; i < max_optional_accel_count; i++) {
-			const bool required = (i < max_mandatory_accel_count);
-			bool report_fail = report_failures;
 
-			int32_t device_id = -1;
+		int32_t check_accelerometer = 1;
+		param_get(param_find("COM_ARM_CHK_ACC"), &check_accelerometer);
 
-			if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-				if (required) {
-					failed = true;
-				}
+		if (check_accelerometer != 0) {
+			/* check all sensors individually, but fail only for mandatory ones */
+			for (unsigned i = 0; i < max_optional_accel_count; i++) {
+				const bool required = (i < max_mandatory_accel_count);
+				bool report_fail = report_failures;
 
-				report_fail = false; // only report the first failure
+				int32_t device_id = -1;
+
+				if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+					if (required) {
+						failed = true;
+					}
+
+					report_fail = false; // only report the first failure
+				}
 			}
 		}
 
@@ -236,9 +242,12 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}
 
 	if (estimator_type == 2) {
+		// check if we care about sensor biases during arming
+		int32_t check_sensor_biases = 1;
+		param_get(param_find("COM_ARM_CHK_BIAS"), &check_sensor_biases);
 
 		const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) &&
-					 ekf2CheckSensorBias(mavlink_log_pub, report_failures);
+					 (check_sensor_biases == 1) ? ekf2CheckSensorBias(mavlink_log_pub, report_failures) : true;
 
 		// For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
 		// as not present.

src/modules/commander/Commander.cpp

@@ -502,11 +502,12 @@ static constexpr const char *main_state_str(uint8_t main_state)
 transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
 {
 	// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
-	if (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
+	if (!_param_preflt_checks_required.get() || (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s))) {
 		run_preflight_checks = false;
 	}
 
 	if (run_preflight_checks) {
+		_manual_control.update();
 		if (_vehicle_control_mode.flag_control_manual_enabled) {
 			if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) {
 				mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center");
@@ -638,6 +639,10 @@ Commander::try_mode_change(main_state_t desired_mode)
 			desired_mode = commander_state_s::MAIN_STATE_MANUAL;
 			res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
 		}
+
+		// indicate that we're changing to something other than our originally desired mode
+		if (res == TRANSITION_CHANGED)
+			res = TRANSITION_CHANGED_FALLBACK;
 	}
 
 	return res;
@@ -1886,7 +1891,7 @@ Commander::run()
 
 					// prevent disarming via safety button if not landed
 					if (hrt_elapsed_time(&_land_detector.timestamp) < 10_s) {
-						if (!_land_detector.landed) {
+						if (!_land_detector.maybe_landed) {
 							safety_disarm_allowed = false;
 						}
 					}
@@ -2265,11 +2270,11 @@ Commander::run()
 			const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF);
 
 			if (rc_arming_enabled) {
-				if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
+				if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.maybe_landed)) {
 					disarm(arm_disarm_reason_t::RC_STICK);
 				}
 
-				if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
+				if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.maybe_landed)) {
 					if (_vehicle_control_mode.flag_control_manual_enabled) {
 						arm(arm_disarm_reason_t::RC_STICK);
 
@@ -2333,13 +2338,13 @@ Commander::run()
 						_landing_gear_pub.publish(landing_gear);
 					}
 				}
+			}
 
-				// evaluate the main state machine according to mode switches
-				if (set_main_state(_status_changed) == TRANSITION_CHANGED) {
-					// play tune on mode change only if armed, blink LED always
-					tune_positive(_armed.armed);
-					_status_changed = true;
-				}
+			// evaluate the main state machine according to mode switches
+			if (set_main_state(_status_changed) == TRANSITION_CHANGED) {
+				// play tune on mode change only if armed, blink LED always
+				tune_positive(_armed.armed);
+				_status_changed = true;
 			}
 
 			/* check throttle kill switch */
@@ -2985,8 +2990,6 @@ Commander::set_main_state_rc()
 		return TRANSITION_NOT_CHANGED;
 	}
 
-	_last_manual_control_switches = _manual_control_switches;
-
 	// reset the position and velocity validity calculation to give the best change of being able to select
 	// the desired mode
 	reset_posvel_validity();
@@ -3052,8 +3055,6 @@ Commander::set_main_state_rc()
 			res = try_mode_change(new_mode);
 		}
 
-		return res;
-
 	} else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) {
 		/* offboard and RTL switches off or denied, check main mode switch */
 		switch (_manual_control_switches.mode_switch) {
@@ -3133,6 +3134,9 @@ Commander::set_main_state_rc()
 
 	}
 
+	// check if we've transitioned; if not, keep around our previous state to allow the next iteration to try
+	if (res == TRANSITION_CHANGED)
+		_last_manual_control_switches = _manual_control_switches;
 	return res;
 }
 

src/modules/commander/Commander.hpp

@@ -243,6 +243,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
 		(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
 		(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
+		(ParamBool<px4::params::COM_ARM_CHK_PREF>) _param_preflt_checks_required,
 		(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
 
 		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,

src/modules/commander/commander_params.c

@@ -925,6 +925,36 @@ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0);
  */
 PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0);
 
+/**
+ * Enable pre-flight checks.
+ *
+ * If this parameter is set, the system will perform pre-flight checks.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_CHK_PREF, 1);
+
+/**
+ * Enable EKF2 sensor bias arming check.
+ *
+ * If this parameter is set, the system will disallow arming if sensor biases are large.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_CHK_BIAS, 1);
+
+/**
+ * Enable accelerometer status checks for arming.
+ *
+ * If this parameter is set, the system will disallow arming if accelerometer ranges are outside bounds.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_CHK_ACC, 1);
+
 /**
  * Condition to enter prearmed mode
  *

src/modules/commander/state_machine_helper.h

@@ -57,7 +57,8 @@
 typedef enum {
 	TRANSITION_DENIED = -1,
 	TRANSITION_NOT_CHANGED = 0,
-	TRANSITION_CHANGED
+	TRANSITION_CHANGED,
+	TRANSITION_CHANGED_FALLBACK
 } transition_result_t;
 
 enum class link_loss_actions_t {