Custom module unexpectedly DISARM during flight

Uncategorized
1

I wrote a custom SO3 control flight mode based on the NN (neural network) control module in the PX4 code. In actual flight, I used as External Mode. After taking off with throttle up, if a Preflight Failed warning (such as strong magnetic interference) occurs, the aircraft will immediately DISARM in mid-air and crash. In addition, in this mode, the landing detector does not seem to work properly, showing as continuous detection of takeoff and landing. The main code for this custom SO3 flight mode is shown below. I really don’t know whether it’s due to incorrect message parameter settings or other logic issues. I hope someone can help me. Thank you.

#include "mc_so3_control.hpp"
#include <lib/mathlib/mathlib.h>

#ifdef __PX4_NUTTX
#include <drivers/drv_hrt.h>
#else
#include <chrono>
#endif

MulticopterSO3Control::MulticopterSO3Control() :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers){}

MulticopterSO3Control::~MulticopterSO3Control(){}

bool MulticopterSO3Control::init()
{
	if(!_angular_velocity_sub.registerCallback()){
		PX4_ERR("Callback Registration Failed.");
		return false;
	}
	SyncControllerGains();
	return true;
}

void MulticopterSO3Control::RegisterSO3ControlMode()
{
	PX4_INFO("Wakkk: RegisterSO3ControlMode()");
	register_ext_component_request_s register_ext_component_request{};
	register_ext_component_request.timestamp = hrt_absolute_time();
	strncpy(register_ext_component_request.name, "SO3 Attitude Control", sizeof(register_ext_component_request.name) - 1);
	register_ext_component_request.request_id = modeRequestID;
	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_pub.publish(register_ext_component_request);
}

void MulticopterSO3Control::UnregisterSO3ControlMode(int8_t arming_check_id, int8_t mode_id)
{
	PX4_INFO("Wakkk: UnregisterSO3ControlMode()");
	unregister_ext_component_s unregister_ext_component{};
	unregister_ext_component.timestamp = hrt_absolute_time();
	strncpy(unregister_ext_component.name, "SO3 Attitude Control", sizeof(unregister_ext_component.name) - 1);
	unregister_ext_component.arming_check_id = arming_check_id;
	unregister_ext_component.mode_id = mode_id;
	_unregister_ext_component_pub.publish(unregister_ext_component);
}

void MulticopterSO3Control::ConfigureSO3ControlMode(int8_t mode_id)
{
	vehicle_control_mode_s config_control_setpoints{};
	config_control_setpoints.timestamp = hrt_absolute_time();
	config_control_setpoints.source_id = mode_id;
	config_control_setpoints.flag_multicopter_position_control_enabled = false;
	config_control_setpoints.flag_control_manual_enabled = true;
	config_control_setpoints.flag_control_offboard_enabled = true;
	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 = false;
	_config_control_setpoints_pub.publish(config_control_setpoints);
}

void MulticopterSO3Control::ConfigureSO3Overrides(int8_t mode_id)
{
	config_overrides_s config_overrides{};
	config_overrides.timestamp = hrt_absolute_time();
	config_overrides.disable_auto_disarm = true;
	config_overrides.defer_failsafes = true;
	config_overrides.defer_failsafes_timeout_s = -1;
	config_overrides.source_type = config_overrides_s::SOURCE_TYPE_MODE;
	config_overrides.source_id = mode_id;
	config_overrides_request_pub.publish(config_overrides);
}

void MulticopterSO3Control::ReplyToArmingCheck(int8_t request_id)
{
	arming_check_reply_s arming_check_reply{};
	arming_check_reply.timestamp = hrt_absolute_time();
	arming_check_reply.request_id = request_id;
	arming_check_reply.registration_id = armingCheckID;
	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 = true;
	// Mode requirements
	arming_check_reply.mode_req_angular_velocity = true;
	arming_check_reply.mode_req_attitude = false;
	arming_check_reply.mode_req_local_alt = false;
	arming_check_reply.mode_req_local_position = false;
	arming_check_reply.mode_req_local_position_relaxed = false;
	arming_check_reply.mode_req_global_position = false;
	arming_check_reply.mode_req_mission = false;
	arming_check_reply.mode_req_home_position = false;
	arming_check_reply.mode_req_prevent_arming = false;
	arming_check_reply.mode_req_manual_control = true;
	_arming_check_reply_pub.publish(arming_check_reply);
}

void MulticopterSO3Control::CheckModeRegistration()
{
	register_ext_component_reply_s register_ext_component_reply;
	int tries = register_ext_component_reply.ORB_QUEUE_LENGTH;
	while (_register_ext_component_reply_sub.update(&register_ext_component_reply) && --tries >= 0) {
		if (register_ext_component_reply.request_id == modeRequestID && register_ext_component_reply.success) {
			armingCheckID = register_ext_component_reply.arming_check_id;
			modeID = register_ext_component_reply.mode_id;
			PX4_INFO("SO3 Attitude Control Mode Registration Successful, ArmingCheckID: %d, ModeID: %d", armingCheckID, modeID);
			ConfigureSO3ControlMode(modeID);
			ConfigureSO3Overrides(modeID);
			break;
		}
	}
}

void MulticopterSO3Control::SyncControllerGains()
{
	_so3Controller.setGains(
		_param_so3_kr_rp.get(),
		_param_so3_kr_y.get(),
		_param_so3_kw_rp.get(),
		_param_so3_kw_y.get());
}

void MulticopterSO3Control::PublishOutput(void)
{
	actuator_motors_s actuator_motors{};
	actuator_motors.timestamp = hrt_absolute_time();
	actuator_motors.control[0] = math::constrain(thrust2norm(_motorThrust[0]), 0.0f, 1.0f);
	actuator_motors.control[1] = math::constrain(thrust2norm(_motorThrust[1]), 0.0f, 1.0f);
	actuator_motors.control[2] = math::constrain(thrust2norm(_motorThrust[2]), 0.0f, 1.0f);
	actuator_motors.control[3] = math::constrain(thrust2norm(_motorThrust[3]), 0.0f, 1.0f);
	actuator_motors.control[4] = -NAN;
	actuator_motors.control[5] = -NAN;
	actuator_motors.control[6] = -NAN;
	actuator_motors.control[7] = -NAN;
	actuator_motors.control[8] = -NAN;
	actuator_motors.control[9] = -NAN;
	actuator_motors.control[10] = -NAN;
	actuator_motors.control[11] = -NAN;
	actuator_motors.reversible_flags = 0;
	_actuator_motors_pub.publish(actuator_motors);
}

float MulticopterSO3Control::thrust2norm(float thrust)
{
	if(thrust <= 0.0f)return 0.0f;
	float a = _param_so3_motor_c.get();
	float b = _param_so3_motor_b.get();
	float c = _param_so3_motor_a.get();
	if(fabsf(a) < 1e-9f) return 0.0f;
	float delta = b*b - 4.0f * a * c + 4.0f * a * thrust;
	if(delta < 0.0f)return 0.0f;
	float x2 = (-b + sqrtf(delta)) / (2.0f * a);
	return math::constrain(x2, 0.0f, 1.0f);
}

float MulticopterSO3Control::norm2thrust(float norm)
{
	float thrust = 	_param_so3_motor_a.get() +
			_param_so3_motor_b.get() * norm +
			_param_so3_motor_c.get() * norm * norm;
	if(thrust < 0.0f)thrust = 0.0f;
	return thrust;
}

void MulticopterSO3Control::Run()
{
	if(should_exit()){
		_angular_velocity_sub.unregisterCallback();
		if(sentModeRegistration){
			UnregisterSO3ControlMode(armingCheckID, modeID);
		}
		exit_and_cleanup();
		return;
	}

	// Register the flight mode with the commander
	if(!sentModeRegistration){
		RegisterSO3ControlMode();
		sentModeRegistration = true;
		return;
	}

	// Check if registration was successful
	if(modeID == -1 || armingCheckID == -1){
		CheckModeRegistration();
		return;
	}

	// Check if an arming check request is received
	if(_arming_check_request_sub.updated()){
		arming_check_request_s arming_check_request;
		_arming_check_request_sub.copy(&arming_check_request);
		ReplyToArmingCheck(arming_check_request.request_id);
	}

	// Check if currently in SO3 control mode
	vehicle_status_s vehicle_status;
	if(_vehicle_status_sub.updated()){
		_vehicle_status_sub.copy(&vehicle_status);
		useCurrentMode = vehicle_status.nav_state == modeID;
	}

	// Parameter update
	if(_parameter_update_sub.updated()){
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		updateParams();
		SyncControllerGains();
	}

	if(!useCurrentMode){
		return;
	}

	// Run SO3 control on every angular velocity update
	if(_angular_velocity_sub.update(&_angular_velocity)){
		const float dt = math::constrain(((_angular_velocity.timestamp_sample - lastRun) * 1e-6f), 0.0002f, 0.02f);
		lastRun = _angular_velocity.timestamp_sample;

		// Update attitude estimate
		if(_attitude_sub.updated()){
			_attitude_sub.copy(&_attitude);
		}

		float hoverVar = 0.0f;
		if(_hover_thrust_estimate_sub.updated()){
			_hover_thrust_estimate_sub.copy(&_hover_thrust_estimate);
			if(_hover_thrust_estimate.valid){
				hoverThrottle = _hover_thrust_estimate.hover_thrust; // estimated hover thrust [0.1, 0.9]
				hoverVar = _hover_thrust_estimate.hover_thrust_var; // estimated hover thrust variance
			}else{
				hoverThrottle = _param_so3_hov_thr_dft.get(); // default hover thrust
			}
		}

		// User manual control input
		_manual_control_setpoint_sub.update(&_manual_control_setpoint);

		// 1. Current attitude quaternion [w, x, y, z]
		matrix::Quatf q_curr(_attitude.q);

		// 2. Current body angular velocity
		matrix::Vector3f omega(_angular_velocity.xyz[0],
				       _angular_velocity.xyz[1],
				       _angular_velocity.xyz[2]);

		// 3. Goal Euler angles from manual stick input
		float maxAngleRad = math::radians(_param_so3_max_ang_deg.get());
		float yawRateRadps = math::radians(_param_so3_yaw_degps.get());
		yawGoalAngle += _manual_control_setpoint.yaw * dt * yawRateRadps;
		yawGoalAngle = matrix::wrap_pi(yawGoalAngle);

		matrix::Vector3f euler_d;
		euler_d(0) = maxAngleRad * _manual_control_setpoint.roll;
		euler_d(1) = -maxAngleRad * _manual_control_setpoint.pitch;
		euler_d(2) = yawGoalAngle;

		// 4. Goal angular velocity (zero for attitude-hold)
		matrix::Vector3f omega_d(0.0f, 0.0f, 0.0f);

		// 5. SO(3) geometric control -> body torque [Mx, My, Mz]
		matrix::Vector3f torque = _so3Controller.controlUpdate(
			q_curr, omega, euler_d, omega_d);

		// 6. User total thrust from throttle stick [-1, 1] -> [0, 1] -> thrust (N)
		float userThrottle = _manual_control_setpoint.throttle * 0.5f + 0.5f;
		userThrottle = math::constrain(userThrottle, 0.0f, 1.0f);
		float totalThrustN = userThrottle * _param_so3_motor_tmax.get() * 4.0f;

		// 7. Motor mixer: total thrust + torque -> 4 motor thrusts (N)
		MotorMixer mixer(0.0f, _param_so3_motor_tmax.get());
		mixer.mix(totalThrustN, torque, _motorThrust);

		logCounter++;
		if(logCounter >= 40){ // 10Hz
			logCounter = 0;
			PX4_INFO("Hover Throttle: %f, Variance: %f\n", (double)hoverThrottle, (double)hoverVar);
		}

		PublishOutput();
	}
}

int MulticopterSO3Control::task_spawn(int argc, char *argv[])
{
	MulticopterSO3Control *instance = new MulticopterSO3Control();
	if(instance){
		_object.store(instance);
		_task_id = task_id_is_work_queue;
		if(instance->init()){
			return PX4_OK;
		}else{
			PX4_ERR("init failed");
		}
	}else{
		PX4_ERR("alloc failed");
	}
	delete instance;
	_object.store(nullptr);
	_task_id = -1;
	return PX4_ERROR;
}

int MulticopterSO3Control::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

int MulticopterSO3Control::print_status()
{
	if(modeID == -1){
		PX4_INFO("SO3 Attitude Control Flight Mode: Mode Registration Failed");
		PX4_INFO("SO3 Attitude Control Flight Mode: Request Sent: %d", sentModeRegistration);
	}else{
		PX4_INFO("SO3 Attitude Control Flight Mode: Registered, Mode ID: %d, Arming Check ID: %d", modeID, armingCheckID);
	}
	return 0;
}

int MulticopterSO3Control::print_usage(const char *reason)
{
	if (reason) {
		PX4_ERR("%s", reason);
	}
	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
	### Description
	Multicopter SO3 Attitude Control Module.
	)DESCR_STR");
	PRINT_MODULE_USAGE_NAME("mc_so3_control", "controller");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
	return 0;
}

extern "C" __EXPORT int mc_so3_control_main(int argc, char *argv[])
{
	return MulticopterSO3Control::main(argc, argv);
}

logs:

INFO  [commander] Armed by RC switch
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2026-07-21/03_56_37.ulg
INFO  [logger] Opened full log file: ./log/2026-07-21/03_56_37.ulg
INFO  [commander] Takeoff detected
INFO  [commander] Landing detected
INFO  [commander] Takeoff detected
INFO  [commander] Landing detected
INFO  [commander] Takeoff detected
INFO  [commander] Landing detected
INFO  [commander] Disarmed by RC switch
WARN  [health_and_arming_checks] Preflight Fail: heading estimate not stable
INFO  [logger] closed logfile, bytes written: 2308618
INFO  [commander] Armed by RC switch
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2026-07-21/03_57_06.ulg
INFO  [logger] Opened full log file: ./log/2026-07-21/03_57_06.ulg
INFO  [commander] Disarmed by auto preflight disarming
WARN  [health_and_arming_checks] Preflight Fail: Strong magnetic interference
INFO  [logger] closed logfile, bytes written: 1050697
WARN  [health_and_arming_checks] Preflight Fail: heading estimate not stable
WARN  [health_and_arming_checks] Preflight Fail: heading estimate not stable
2

This topic was automatically closed 90 days after the last reply. New replies are no longer allowed.