From 0dcad42f578ccbe08c4ad7d87777eab30e93d79f Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 27 Feb 2018 11:33:26 +0100
Subject: [PATCH] mc_pos_control: refactor, indent control block
individual commit for the indentation because otherwise
the diff gets unreadable. this indentation is made because
afterwards the entire legacy position control functionality
is in an else case.
---
.../mc_pos_control/mc_pos_control_main.cpp | 120 +++++++++---------
1 file changed, 61 insertions(+), 59 deletions(-)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 903234613d..8a7d4d6b3c 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -3136,75 +3136,77 @@ MulticopterPositionControl::task_main()
_alt_hold_engaged = false;
}
- if (_control_mode.flag_control_altitude_enabled ||
- _control_mode.flag_control_position_enabled ||
- _control_mode.flag_control_climb_rate_enabled ||
- _control_mode.flag_control_velocity_enabled ||
- _control_mode.flag_control_acceleration_enabled) {
-
- do_control();
-
- /* fill local position, velocity and thrust setpoint */
- _local_pos_sp.timestamp = hrt_absolute_time();
- _local_pos_sp.x = _pos_sp(0);
- _local_pos_sp.y = _pos_sp(1);
- _local_pos_sp.z = _pos_sp(2);
- _local_pos_sp.yaw = _att_sp.yaw_body;
- _local_pos_sp.vx = _vel_sp(0);
- _local_pos_sp.vy = _vel_sp(1);
- _local_pos_sp.vz = _vel_sp(2);
-
- /* publish local position setpoint */
- if (_local_pos_sp_pub != nullptr) {
- orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+ {
+ if (_control_mode.flag_control_altitude_enabled ||
+ _control_mode.flag_control_position_enabled ||
+ _control_mode.flag_control_climb_rate_enabled ||
+ _control_mode.flag_control_velocity_enabled ||
+ _control_mode.flag_control_acceleration_enabled) {
+
+ do_control();
+
+ /* fill local position, velocity and thrust setpoint */
+ _local_pos_sp.timestamp = hrt_absolute_time();
+ _local_pos_sp.x = _pos_sp(0);
+ _local_pos_sp.y = _pos_sp(1);
+ _local_pos_sp.z = _pos_sp(2);
+ _local_pos_sp.yaw = _att_sp.yaw_body;
+ _local_pos_sp.vx = _vel_sp(0);
+ _local_pos_sp.vy = _vel_sp(1);
+ _local_pos_sp.vz = _vel_sp(2);
+
+ /* publish local position setpoint */
+ if (_local_pos_sp_pub != nullptr) {
+ orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+
+ } else {
+ _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
+ }
} else {
- _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
+ /* position controller disabled, reset setpoints */
+ _reset_pos_sp = true;
+ _reset_alt_sp = true;
+ _do_reset_alt_pos_flag = true;
+ _mode_auto = false;
+ _reset_int_z = true;
+ _reset_int_xy = true;
+
+ /* store last velocity in case a mode switch to position control occurs */
+ _vel_sp_prev = _vel;
}
- } else {
- /* position controller disabled, reset setpoints */
- _reset_pos_sp = true;
- _reset_alt_sp = true;
- _do_reset_alt_pos_flag = true;
- _mode_auto = false;
- _reset_int_z = true;
- _reset_int_xy = true;
-
- /* store last velocity in case a mode switch to position control occurs */
- _vel_sp_prev = _vel;
- }
-
- /* generate attitude setpoint from manual controls */
- if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
+ /* generate attitude setpoint from manual controls */
+ if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
- generate_attitude_setpoint();
+ generate_attitude_setpoint();
- } else {
- _reset_yaw_sp = true;
- _att_sp.yaw_sp_move_rate = 0.0f;
- }
+ } else {
+ _reset_yaw_sp = true;
+ _att_sp.yaw_sp_move_rate = 0.0f;
+ }
- /* update previous velocity for velocity controller D part */
- _vel_prev = _vel;
+ /* update previous velocity for velocity controller D part */
+ _vel_prev = _vel;
- /* publish attitude setpoint
- * Do not publish if
- * - offboard is enabled but position/velocity/accel control is disabled,
- * in this case the attitude setpoint is published by the mavlink app.
- * - if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
- * attitude setpoints for the transition).
- */
- if (!(_control_mode.flag_control_offboard_enabled &&
- !(_control_mode.flag_control_position_enabled ||
- _control_mode.flag_control_velocity_enabled ||
- _control_mode.flag_control_acceleration_enabled))) {
+ /* publish attitude setpoint
+ * Do not publish if
+ * - offboard is enabled but position/velocity/accel control is disabled,
+ * in this case the attitude setpoint is published by the mavlink app.
+ * - if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
+ * attitude setpoints for the transition).
+ */
+ if (!(_control_mode.flag_control_offboard_enabled &&
+ !(_control_mode.flag_control_position_enabled ||
+ _control_mode.flag_control_velocity_enabled ||
+ _control_mode.flag_control_acceleration_enabled))) {
- if (_att_sp_pub != nullptr) {
- orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
+ if (_att_sp_pub != nullptr) {
+ orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
- } else if (_attitude_setpoint_id) {
- _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
+ } else if (_attitude_setpoint_id) {
+ _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
+ }
}
}
}
--
GitLab