zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

mc_pos_control: require current trajectory setpoint to run controller

main
Daniel Agar 3 years ago committed by Matthias Grob
parent
138772386f
commit
ad6592f669
3 changed files with 91 additions and 60 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 121
      src/modules/mc_pos_control/MulticopterPositionControl.cpp
  2. 28
      src/modules/mc_pos_control/MulticopterPositionControl.hpp
  3. 2
      src/modules/mc_pos_control/PositionControl/PositionControl.cpp

121
src/modules/mc_pos_control/MulticopterPositionControl.cpp
Unescape View File

@ -319,9 +319,21 @@ void MulticopterPositionControl::Run()
// set _dt in controllib Block for BlockDerivative // set _dt in controllib Block for BlockDerivative
setDt(dt); 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); _vehicle_land_detected_sub.update(&_vehicle_land_detected);
if (_param_mpc_use_hte.get()) { if (_param_mpc_use_hte.get()) {
@ -334,36 +346,66 @@ void MulticopterPositionControl::Run()
} }
} }
PositionControlStates states{set_vehicle_states(local_pos)}; const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint);
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) { // 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];
}
const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint); if (local_pos.vz_reset_counter != _vz_reset_counter) {
_setpoint.vz += local_pos.delta_vz;
}
// adjust existing (or older) setpoint with any EKF reset deltas if (local_pos.xy_reset_counter != _xy_reset_counter) {
if (_setpoint.timestamp < local_pos.timestamp) { _setpoint.x += local_pos.delta_xy[0];
if (local_pos.vxy_reset_counter != _vxy_reset_counter) { _setpoint.y += local_pos.delta_xy[1];
_setpoint.vx += local_pos.delta_vxy[0]; }
_setpoint.vy += local_pos.delta_vxy[1];
}
if (local_pos.vz_reset_counter != _vz_reset_counter) { if (local_pos.z_reset_counter != _z_reset_counter) {
_setpoint.vz += local_pos.delta_vz; _setpoint.z += local_pos.delta_z;
} }
if (local_pos.xy_reset_counter != _xy_reset_counter) { if (local_pos.heading_reset_counter != _heading_reset_counter) {
_setpoint.x += local_pos.delta_xy[0]; _setpoint.yaw += local_pos.delta_heading;
_setpoint.y += local_pos.delta_xy[1]; }
} }
if (local_pos.z_reset_counter != _z_reset_counter) { if (local_pos.vxy_reset_counter != _vxy_reset_counter) {
_setpoint.z += local_pos.delta_z; _vel_x_deriv.reset();
} _vel_y_deriv.reset();
}
if (local_pos.heading_reset_counter != _heading_reset_counter) { if (local_pos.vz_reset_counter != _vz_reset_counter) {
_setpoint.yaw += local_pos.delta_heading; _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)) {
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 // update vehicle constraints and handle smooth takeoff
_vehicle_constraints_sub.update(&_vehicle_constraints); _vehicle_constraints_sub.update(&_vehicle_constraints);
@ -377,7 +419,7 @@ void MulticopterPositionControl::Run()
if (_vehicle_control_mode.flag_control_offboard_enabled) { if (_vehicle_control_mode.flag_control_offboard_enabled) {
bool want_takeoff = _vehicle_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; && (time_stamp_now < _setpoint.timestamp + 1_s);
if (want_takeoff && PX4_ISFINITE(_setpoint.z) if (want_takeoff && PX4_ISFINITE(_setpoint.z)
&& (_setpoint.z < states.position(2))) { && (_setpoint.z < states.position(2))) {
@ -474,17 +516,9 @@ void MulticopterPositionControl::Run()
} else { } else {
// Failsafe // 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{}; 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 // reset constraints
_vehicle_constraints = {0, NAN, NAN, false, {}}; _vehicle_constraints = {0, NAN, NAN, false, {}};
@ -524,21 +558,22 @@ void MulticopterPositionControl::Run()
_takeoff_status_pub.get().timestamp = hrt_absolute_time(); _takeoff_status_pub.get().timestamp = hrt_absolute_time();
_takeoff_status_pub.update(); _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); perf_end(_cycle_perf);
} }
void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, 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 // Only react after a short delay
_failsafe_land_hysteresis.set_state_and_update(true, now); _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"); PX4_WARN("Failsafe: blind descend");
} }
} }
_in_failsafe = true;
} }
} }

28
src/modules/mc_pos_control/MulticopterPositionControl.hpp
Unescape View File

@ -92,21 +92,22 @@ private:
orb_advert_t _mavlink_log_pub{nullptr}; orb_advert_t _mavlink_log_pub{nullptr};
uORB::PublicationData<takeoff_status_s> _takeoff_status_pub {ORB_ID(takeoff_status)}; 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_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::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 _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::Subscription _trajectory_setpoint_sub {ORB_ID(trajectory_setpoint)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_constraints_sub {ORB_ID(vehicle_constraints)}; uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
uORB::Subscription _vehicle_control_mode_sub {ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_land_detected_sub {ORB_ID(vehicle_land_detected)}; 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_local_position_setpoint_s _setpoint {};
vehicle_control_mode_s _vehicle_control_mode {}; 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 */ 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}; bool _hover_thrust_initialized{false};
/** Timeout in us for trajectory data to get considered invalid */ /** 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 * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
* to true, the failsafe will be initiated immediately. * to true, the failsafe will be initiated immediately.
*/ */
void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states);
bool warn);
/** /**
* Reset setpoints to NAN * Reset setpoints to NAN

2
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
Unescape 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 _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(_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)); valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));

Write Preview
Loading…
Cancel
Save