mc_att_control move print_usage() to bottom of file and format

2026-10-16 07:33:35 +08:00 · 2019-06-08 03:23:50 -07:00
parent 97c2dba2fa
commit d9c5fb0500
1 changed files with 67 additions and 57 deletions
@@ -61,41 +61,6 @@

 using namespace matrix;

-
-int MulticopterAttitudeControl::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-This implements the multicopter attitude and rate controller. It takes attitude
-setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
-via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
-
-The controller has two loops: a P loop for angular error and a PID loop for angular rate error.
-
-Publication documenting the implemented Quaternion Attitude Control:
-Nonlinear Quadrocopter Attitude Control (2013)
-by Dario Brescianini, Markus Hehn and Raffaello D'Andrea
-Institute for Dynamic Systems and Control (IDSC), ETH Zurich
-
-https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
-
-### Implementation
-To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("mc_att_control", "controller");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	ModuleParams(nullptr),
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -148,10 +113,12 @@ MulticopterAttitudeControl::parameters_updated()

 	// angular rate limits
 	using math::radians;
-	_attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()), radians(_param_mc_yawrate_max.get())));
+	_attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()),
+						radians(_param_mc_yawrate_max.get())));

 	// manual rate control acro mode rate limits
-	_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get()));
+	_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
+				  radians(_param_mc_acro_y_max.get()));

 	_man_tilt_max = math::radians(_param_mpc_man_tilt_max.get());

@@ -162,9 +129,9 @@ MulticopterAttitudeControl::parameters_updated()

 	/* fine tune the rotation */
 	Dcmf board_rotation_offset(Eulerf(
-			M_DEG_TO_RAD_F * _param_sens_board_x_off.get(),
-			M_DEG_TO_RAD_F * _param_sens_board_y_off.get(),
-			M_DEG_TO_RAD_F * _param_sens_board_z_off.get()));
+					   M_DEG_TO_RAD_F * _param_sens_board_x_off.get(),
+					   M_DEG_TO_RAD_F * _param_sens_board_y_off.get(),
+					   M_DEG_TO_RAD_F * _param_sens_board_z_off.get()));
 	_board_rotation = board_rotation_offset * _board_rotation;
 }

@@ -192,6 +159,7 @@ MulticopterAttitudeControl::vehicle_status_poll()
 				_attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint);

 				int32_t vt_type = -1;
+
 				if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
 					_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
 				}
@@ -227,8 +195,10 @@ MulticopterAttitudeControl::vehicle_attitude_poll()
 			// we only extract the heading change from the delta quaternion
 			_man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi();
 		}
+
 		return true;
 	}
+
 	return false;
 }

@@ -242,10 +212,12 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)

 	default: // 0 or other: rescale to hover throttle at 0.5 stick
 		if (throttle_stick_input < 0.5f) {
-			return (_param_mpc_thr_hover.get() - _param_mpc_manthr_min.get()) / 0.5f * throttle_stick_input + _param_mpc_manthr_min.get();
+			return (_param_mpc_thr_hover.get() - _param_mpc_manthr_min.get()) / 0.5f * throttle_stick_input +
+			       _param_mpc_manthr_min.get();

 		} else {
-			return (_param_mpc_thr_max.get() - _param_mpc_thr_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + _param_mpc_thr_max.get();
+			return (_param_mpc_thr_max.get() - _param_mpc_thr_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) +
+			       _param_mpc_thr_max.get();
 		}
 	}
 }
@@ -260,7 +232,9 @@ MulticopterAttitudeControl::get_landing_gear_state()
 	if (_vehicle_land_detected.landed) {
 		_gear_state_initialized = false;
 	}
+
 	float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
+
 	if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
 		landing_gear = landing_gear_s::GEAR_UP;

@@ -364,6 +338,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_

 	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
 	attitude_setpoint.timestamp = hrt_absolute_time();
+
 	if (_attitude_sp_id != nullptr) {
 		orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
 	}
@@ -661,8 +636,8 @@ MulticopterAttitudeControl::run()
 			 * even bother running the attitude controllers */
 			if (_v_control_mode.flag_control_rattitude_enabled) {
 				_v_control_mode.flag_control_attitude_enabled =
-						fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() &&
-						fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get();
+					fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() &&
+					fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get();
 			}

 			bool attitude_setpoint_generated = false;
@@ -679,14 +654,15 @@ MulticopterAttitudeControl::run()
 				if (attitude_updated) {
 					// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
 					if (_v_control_mode.flag_control_manual_enabled &&
-							!_v_control_mode.flag_control_altitude_enabled &&
-							!_v_control_mode.flag_control_velocity_enabled &&
-							!_v_control_mode.flag_control_position_enabled) {
+					    !_v_control_mode.flag_control_altitude_enabled &&
+					    !_v_control_mode.flag_control_velocity_enabled &&
+					    !_v_control_mode.flag_control_position_enabled) {
 						generate_attitude_setpoint(attitude_dt, reset_yaw_sp);
 						attitude_setpoint_generated = true;
 					}

 					control_attitude();
+
 					if (_v_control_mode.flag_control_yawrate_override_enabled) {
 						/* Yaw rate override enabled, overwrite the yaw setpoint */
 						_v_rates_sp_sub.update(&_v_rates_sp);
@@ -703,9 +679,9 @@ MulticopterAttitudeControl::run()
 					if (manual_control_updated) {
 						/* manual rates control - ACRO mode */
 						Vector3f man_rate_sp(
-								math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
-								math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
-								math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get()));
+							math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
+							math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
+							math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get()));
 						_rates_sp = man_rate_sp.emult(_acro_rate_max);
 						_thrust_sp = _manual_control_sp.z;
 						publish_rates_setpoint();
@@ -736,8 +712,8 @@ MulticopterAttitudeControl::run()
 				// reset yaw setpoint during transitions, tailsitter.cpp generates
 				// attitude setpoint for the transition
 				reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
-						_vehicle_land_detected.landed ||
-						(_vehicle_status.is_vtol && _vehicle_status.in_transition_mode);
+					       _vehicle_land_detected.landed ||
+					       (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode);

 				attitude_dt = 0.f;
 			}
@@ -770,11 +746,11 @@ MulticopterAttitudeControl::run()
 int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])
 {
 	_task_id = px4_task_spawn_cmd("mc_att_control",
-					   SCHED_DEFAULT,
-					   SCHED_PRIORITY_ATTITUDE_CONTROL,
-					   1700,
-					   (px4_main_t)&run_trampoline,
-					   (char *const *)argv);
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_ATTITUDE_CONTROL,
+				      1700,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);

 	if (_task_id < 0) {
 		_task_id = -1;
@@ -794,6 +770,40 @@ int MulticopterAttitudeControl::custom_command(int argc, char *argv[])
 	return print_usage("unknown command");
 }

+int MulticopterAttitudeControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the multicopter attitude and rate controller. It takes attitude
+setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
+via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
+
+The controller has two loops: a P loop for angular error and a PID loop for angular rate error.
+
+Publication documenting the implemented Quaternion Attitude Control:
+Nonlinear Quadrocopter Attitude Control (2013)
+by Dario Brescianini, Markus Hehn and Raffaello D'Andrea
+Institute for Dynamic Systems and Control (IDSC), ETH Zurich
+
+https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
+
+### Implementation
+To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("mc_att_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
 int mc_att_control_main(int argc, char *argv[])
 {
 	return MulticopterAttitudeControl::main(argc, argv);