From d9c5fb0500630d02bd153163344ddc540f6e0f8b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 7 Jun 2019 16:42:20 -0400 Subject: [PATCH] mc_att_control move print_usage() to bottom of file and format --- .../mc_att_control/mc_att_control_main.cpp | 124 ++++++++++-------- 1 file changed, 67 insertions(+), 57 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 74dcaa8134..013877a6c3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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(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);