Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

mc_pos_control: require current trajectory setpoint to run controller

Browse Source
This commit is contained in:
Daniel Agar
2022-02-09 18:24:30 -05:00
committed by Matthias Grob
parent 138772386f
commit ad6592f669
3 changed files with 94 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+80 -47
View File
@@ -319,9 +319,21 @@ void MulticopterPositionControl::Run()
// set _dt in controllib Block for BlockDerivative
setDt(dt);
_in_failsafe = false;
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
if (_vehicle_control_mode_sub.updated()) {
const bool position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled;
if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) {
if (!position_control_enabled && _vehicle_control_mode.flag_multicopter_position_control_enabled) {
_time_position_control_enabled = _vehicle_control_mode.timestamp;
} else if (position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
// clear existing setpoint when controller is no longer active
_setpoint = {};
}
}
}
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
if (_param_mpc_use_hte.get()) {
@@ -334,36 +346,66 @@ void MulticopterPositionControl::Run()
}
}
const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint);
// adjust existing (or older) setpoint with any EKF reset deltas
if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < local_pos.timestamp)) {
if (local_pos.vxy_reset_counter != _vxy_reset_counter) {
_setpoint.vx += local_pos.delta_vxy[0];
_setpoint.vy += local_pos.delta_vxy[1];
}
if (local_pos.vz_reset_counter != _vz_reset_counter) {
_setpoint.vz += local_pos.delta_vz;
}
if (local_pos.xy_reset_counter != _xy_reset_counter) {
_setpoint.x += local_pos.delta_xy[0];
_setpoint.y += local_pos.delta_xy[1];
}
if (local_pos.z_reset_counter != _z_reset_counter) {
_setpoint.z += local_pos.delta_z;
}
if (local_pos.heading_reset_counter != _heading_reset_counter) {
_setpoint.yaw += local_pos.delta_heading;
}
}
if (local_pos.vxy_reset_counter != _vxy_reset_counter) {
_vel_x_deriv.reset();
_vel_y_deriv.reset();
}
if (local_pos.vz_reset_counter != _vz_reset_counter) {
_vel_z_deriv.reset();
}
// save latest reset counters
_vxy_reset_counter = local_pos.vxy_reset_counter;
_vz_reset_counter = local_pos.vz_reset_counter;
_xy_reset_counter = local_pos.xy_reset_counter;
_z_reset_counter = local_pos.z_reset_counter;
_heading_reset_counter = local_pos.heading_reset_counter;
PositionControlStates states{set_vehicle_states(local_pos)};
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
// set failsafe setpoint if there hasn't been a new
// trajectory setpoint since position control started
if ((_setpoint.timestamp < _time_position_control_enabled)
&& (time_stamp_now > _time_position_control_enabled)) {
const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint);
// adjust existing (or older) setpoint with any EKF reset deltas
if (_setpoint.timestamp < local_pos.timestamp) {
if (local_pos.vxy_reset_counter != _vxy_reset_counter) {
_setpoint.vx += local_pos.delta_vxy[0];
_setpoint.vy += local_pos.delta_vxy[1];
}
if (local_pos.vz_reset_counter != _vz_reset_counter) {
_setpoint.vz += local_pos.delta_vz;
}
if (local_pos.xy_reset_counter != _xy_reset_counter) {
_setpoint.x += local_pos.delta_xy[0];
_setpoint.y += local_pos.delta_xy[1];
}
if (local_pos.z_reset_counter != _z_reset_counter) {
_setpoint.z += local_pos.delta_z;
}
if (local_pos.heading_reset_counter != _heading_reset_counter) {
_setpoint.yaw += local_pos.delta_heading;
}
failsafe(time_stamp_now, _setpoint, states);
_setpoint.timestamp = local_pos.timestamp;
}
}
if (_vehicle_control_mode.flag_multicopter_position_control_enabled
&& (_setpoint.timestamp >= _time_position_control_enabled)) {
// update vehicle constraints and handle smooth takeoff
_vehicle_constraints_sub.update(&_vehicle_constraints);
@@ -377,7 +419,7 @@ void MulticopterPositionControl::Run()
if (_vehicle_control_mode.flag_control_offboard_enabled) {
bool want_takeoff = _vehicle_control_mode.flag_armed && _vehicle_land_detected.landed
&& hrt_elapsed_time(&_setpoint.timestamp) < 1_s;
&& (time_stamp_now < _setpoint.timestamp + 1_s);
if (want_takeoff && PX4_ISFINITE(_setpoint.z)
&& (_setpoint.z < states.position(2))) {
@@ -474,17 +516,9 @@ void MulticopterPositionControl::Run()
} else {
// Failsafe
// do not warn while we are disarmed, as we might not have valid setpoints yet
const bool warn_failsafe = ((time_stamp_now - _last_warn) > 2_s) && _vehicle_control_mode.flag_armed;
if (warn_failsafe) {
PX4_WARN("invalid setpoints");
_last_warn = time_stamp_now;
}
vehicle_local_position_setpoint_s failsafe_setpoint{};
failsafe(time_stamp_now, failsafe_setpoint, states, warn_failsafe);
failsafe(time_stamp_now, failsafe_setpoint, states);
// reset constraints
_vehicle_constraints = {0, NAN, NAN, false, {}};
@@ -524,21 +558,22 @@ void MulticopterPositionControl::Run()
_takeoff_status_pub.get().timestamp = hrt_absolute_time();
_takeoff_status_pub.update();
}
// save latest reset counters
_vxy_reset_counter = local_pos.vxy_reset_counter;
_vz_reset_counter = local_pos.vz_reset_counter;
_xy_reset_counter = local_pos.xy_reset_counter;
_z_reset_counter = local_pos.z_reset_counter;
_heading_reset_counter = local_pos.heading_reset_counter;
}
perf_end(_cycle_perf);
}
void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint,
const PositionControlStates &states, bool warn)
const PositionControlStates &states)
{
// do not warn while we are disarmed, as we might not have valid setpoints yet
bool warn = _vehicle_control_mode.flag_armed && ((now - _last_warn) > 2_s);
if (warn) {
PX4_WARN("invalid setpoints");
_last_warn = now;
}
// Only react after a short delay
_failsafe_land_hysteresis.set_state_and_update(true, now);
@@ -578,8 +613,6 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
PX4_WARN("Failsafe: blind descend");
}
}
_in_failsafe = true;
}
}
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+13 -15
View File
@@ -92,21 +92,22 @@ 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 {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::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 _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)};
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 */
hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */
hrt_abstime _time_position_control_enabled{0};
vehicle_local_position_setpoint_s _setpoint {};
vehicle_control_mode_s _vehicle_control_mode {};
@@ -183,8 +184,6 @@ private:
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 _hover_thrust_initialized{false};
/** Timeout in us for trajectory data to get considered invalid */
@@ -228,8 +227,7 @@ private:
* setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
* to true, the failsafe will be initiated immediately.
*/
void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
bool warn);
void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states);
/**
* Reset setpoints to NAN
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+1 -1
View File
@@ -114,7 +114,7 @@ bool PositionControl::update(const float dt)
_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
}
// There has to be a valid output accleration and thrust setpoint otherwise something went wrong
// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));