From fef2c4339577c1bccd33c649d63a4a31c8005ef7 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 22 Jun 2021 16:26:18 -0600 Subject: [PATCH] Modify occurrences of control_mode to vehicle_control_mode in MulticopterPositionControl.cpp/hpp to reflect the existing naming convention in the files. --- .../MulticopterPositionControl.cpp | 20 ++-- .../MulticopterPositionControl.hpp | 91 ++++++++++--------- 2 files changed, 58 insertions(+), 53 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 674d44b48b..173f0937b4 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -273,7 +273,7 @@ void MulticopterPositionControl::Run() const bool was_in_failsafe = _in_failsafe; _in_failsafe = false; - _control_mode_sub.update(&_control_mode); + _vehicle_control_mode_sub.update(&_vehicle_control_mode); _vehicle_land_detected_sub.update(&_vehicle_land_detected); if (_param_mpc_use_hte.get()) { @@ -288,7 +288,7 @@ void MulticopterPositionControl::Run() PositionControlStates states{set_vehicle_states(local_pos)}; - if (_control_mode.flag_multicopter_position_control_enabled) { + if (_vehicle_control_mode.flag_multicopter_position_control_enabled) { _trajectory_setpoint_sub.update(&_setpoint); @@ -326,9 +326,9 @@ void MulticopterPositionControl::Run() _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); } - if (_control_mode.flag_control_offboard_enabled) { + if (_vehicle_control_mode.flag_control_offboard_enabled) { - bool want_takeoff = _control_mode.flag_armed && _vehicle_land_detected.landed + bool want_takeoff = _vehicle_control_mode.flag_armed && _vehicle_land_detected.landed && hrt_elapsed_time(&_setpoint.timestamp) < 1_s; if (want_takeoff && PX4_ISFINITE(_setpoint.z) @@ -357,11 +357,12 @@ void MulticopterPositionControl::Run() } // handle smooth takeoff - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff, + _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, + _vehicle_constraints.want_takeoff, _vehicle_constraints.speed_up, false, time_stamp_now); - const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup); - const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); + const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup); + const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact); // make sure takeoff ramp is not amended by acceleration feed-forward @@ -456,7 +457,8 @@ void MulticopterPositionControl::Run() } else { // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, time_stamp_now); + _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, + time_stamp_now); } // Publish takeoff status @@ -485,7 +487,7 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_ const PositionControlStates &states, bool warn) { // do not warn while we are disarmed, as we might not have valid setpoints yet - if (!_control_mode.flag_armed) { + if (!_vehicle_control_mode.flag_armed) { warn = false; } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 1e6b591a42..9d5f11be36 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -92,27 +92,28 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - uORB::PublicationData _takeoff_status_pub{ORB_ID(takeoff_status)}; - uORB::Publication _vehicle_attitude_setpoint_pub; - uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::PublicationData _takeoff_status_pub {ORB_ID(takeoff_status)}; + uORB::Publication _vehicle_attitude_setpoint_pub {ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _local_pos_sp_pub {ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ - uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::SubscriptionCallbackWorkItem _local_pos_sub {this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _parameter_update_sub {ORB_ID(parameter_update), 1_s}; - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; - uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)}; + uORB::Subscription _hover_thrust_estimate_sub {ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub {ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_constraints_sub {ORB_ID(vehicle_constraints)}; + uORB::Subscription _vehicle_control_mode_sub {ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub {ORB_ID(vehicle_land_detected)}; hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ int _task_failure_count{0}; /**< counter for task failures */ - vehicle_control_mode_s _control_mode{}; - vehicle_local_position_setpoint_s _setpoint{}; - vehicle_constraints_s _vehicle_constraints{ + vehicle_local_position_setpoint_s _setpoint {}; + vehicle_control_mode_s _vehicle_control_mode {}; + + vehicle_constraints_s _vehicle_constraints { .timestamp = 0, .speed_xy = NAN, .speed_up = NAN, @@ -131,66 +132,68 @@ private: DEFINE_PARAMETERS( // Position Control - (ParamFloat) _param_mpc_xy_p, - (ParamFloat) _param_mpc_z_p, + (ParamFloat) _param_mpc_xy_p, + (ParamFloat) _param_mpc_z_p, (ParamFloat) _param_mpc_xy_vel_p_acc, (ParamFloat) _param_mpc_xy_vel_i_acc, (ParamFloat) _param_mpc_xy_vel_d_acc, - (ParamFloat) _param_mpc_z_vel_p_acc, - (ParamFloat) _param_mpc_z_vel_i_acc, - (ParamFloat) _param_mpc_z_vel_d_acc, - (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_vel_p_acc, + (ParamFloat) _param_mpc_z_vel_i_acc, + (ParamFloat) _param_mpc_z_vel_d_acc, + (ParamFloat) _param_mpc_xy_vel_max, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_tiltmax_air, - (ParamFloat) _param_mpc_thr_hover, - (ParamBool) _param_mpc_use_hte, + (ParamFloat) _param_mpc_tiltmax_air, + (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte, // Takeoff / Land (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ - (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ - (ParamFloat) _param_mpc_tko_speed, - (ParamFloat) _param_mpc_land_speed, + (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ + (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) _param_mpc_land_speed, - (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_xy_cruise, - (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ - (ParamInt) _param_mpc_pos_mode, - (ParamInt) _param_mpc_alt_mode, - (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ - (ParamFloat) _param_mpc_thr_min, - (ParamFloat) _param_mpc_thr_max, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_xy_cruise, + (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ + (ParamInt) _param_mpc_pos_mode, + (ParamInt) _param_mpc_alt_mode, + (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat) _param_mpc_thr_min, + (ParamFloat) _param_mpc_thr_max, (ParamFloat) _param_sys_vehicle_resp, - (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_hor, (ParamFloat) _param_mpc_acc_down_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_hor_max, - (ParamFloat) _param_mpc_jerk_auto, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_man_y_max, - (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_hor_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, - (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all + (ParamFloat) _param_mpc_xy_vel_all, + (ParamFloat) _param_mpc_z_vel_all ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - PositionControl _control; /**< class for core PID position control */ + PositionControl _control; /**< class for core PID position control */ hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ - bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */ + bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */ bool _hover_thrust_initialized{false}; /** Timeout in us for trajectory data to get considered invalid */ static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + /** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */ static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms; + /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off and tilt is limited */ static constexpr float ALTITUDE_THRESHOLD = 0.3f;