fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 09:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Modify occurrences of control_mode to vehicle_control_mode in MulticopterPositionControl.cpp/hpp to reflect the existing naming convention in the files.

Browse Source
This commit is contained in:
mcsauder
2021-06-22 16:26:18 -06:00
committed by Daniel Agar
parent 1ee3484827
commit fef2c43395
2 changed files with 58 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+11 -9
View File
@@ -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;
}
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+47 -44
View File
@@ -92,27 +92,28 @@ private:
orb_advert_t _mavlink_log_pub{nullptr};
uORB::PublicationData<takeoff_status_s> _takeoff_status_pub{ORB_ID(takeoff_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
uORB::PublicationData<takeoff_status_s> _takeoff_status_pub {ORB_ID(takeoff_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub {ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_local_position_setpoint_s> _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<px4::params::MPC_XY_P>) _param_mpc_xy_p,
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_xy_vel_p_acc,
(ParamFloat<px4::params::MPC_XY_VEL_I_ACC>) _param_mpc_xy_vel_i_acc,
(ParamFloat<px4::params::MPC_XY_VEL_D_ACC>) _param_mpc_xy_vel_d_acc,
(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>) _param_mpc_z_vel_i_acc,
(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>) _param_mpc_z_vel_d_acc,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>) _param_mpc_z_vel_i_acc,
(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>) _param_mpc_z_vel_d_acc,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
// Takeoff / Land
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
(ParamFloat<px4::params::SYS_VEHICLE_RESP>) _param_sys_vehicle_resp,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max,
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max,
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
(ParamFloat<px4::params::MPC_XY_VEL_ALL>) _param_mpc_xy_vel_all,
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all
(ParamFloat<px4::params::MPC_XY_VEL_ALL>) _param_mpc_xy_vel_all,
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _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;