mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 11:50:35 +08:00
move takeoff state machine flight_mode_manager -> mc_pos_control
This commit is contained in:
Daniel Agar
committed by
Matthias Grob
Matthias Grob
parent
823c6078d9
commit
266ea377da
@@ -36,24 +36,26 @@
|
||||
#include <float.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include "PositionControl/ControlMath.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
|
||||
SuperBlock(nullptr, "MPC"),
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
|
||||
_vel_x_deriv(this, "VELD"),
|
||||
_vel_y_deriv(this, "VELD"),
|
||||
_vel_z_deriv(this, "VELD"),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time"))
|
||||
_vel_z_deriv(this, "VELD")
|
||||
{
|
||||
// fetch initial parameter values
|
||||
parameters_update(true);
|
||||
|
||||
// set failsafe hysteresis
|
||||
_failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND);
|
||||
|
||||
reset_setpoint_to_nan(_setpoint);
|
||||
}
|
||||
|
||||
MulticopterPositionControl::~MulticopterPositionControl()
|
||||
@@ -68,10 +70,8 @@ bool MulticopterPositionControl::init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// limit to every other vehicle_local_position update (50 Hz)
|
||||
_local_pos_sub.set_interval_us(20_ms);
|
||||
|
||||
_time_stamp_last_loop = hrt_absolute_time();
|
||||
ScheduleNow();
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -156,9 +156,6 @@ int MulticopterPositionControl::parameters_update(bool force)
|
||||
Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()),
|
||||
Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()),
|
||||
Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get()));
|
||||
_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
|
||||
_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
|
||||
_control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!
|
||||
|
||||
// Check that the design parameters are inside the absolute maximum constraints
|
||||
if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {
|
||||
@@ -189,79 +186,68 @@ int MulticopterPositionControl::parameters_update(bool force)
|
||||
// initialize vectors from params and enforce constraints
|
||||
_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
|
||||
_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
|
||||
|
||||
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
|
||||
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
|
||||
_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void MulticopterPositionControl::poll_subscriptions()
|
||||
PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s &local_pos)
|
||||
{
|
||||
_control_mode_sub.update(&_control_mode);
|
||||
PositionControlStates states;
|
||||
|
||||
if (_param_mpc_use_hte.get()) {
|
||||
hover_thrust_estimate_s hte;
|
||||
|
||||
if (_hover_thrust_estimate_sub.update(&hte)) {
|
||||
if (hte.valid) {
|
||||
_control.updateHoverThrust(hte.hover_thrust);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
|
||||
{
|
||||
// only set position states if valid and finite
|
||||
if (PX4_ISFINITE(_local_pos.x) && PX4_ISFINITE(_local_pos.y) && _local_pos.xy_valid) {
|
||||
_states.position(0) = _local_pos.x;
|
||||
_states.position(1) = _local_pos.y;
|
||||
if (PX4_ISFINITE(local_pos.x) && PX4_ISFINITE(local_pos.y) && local_pos.xy_valid) {
|
||||
states.position(0) = local_pos.x;
|
||||
states.position(1) = local_pos.y;
|
||||
|
||||
} else {
|
||||
_states.position(0) = _states.position(1) = NAN;
|
||||
states.position(0) = NAN;
|
||||
states.position(1) = NAN;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_local_pos.z) && _local_pos.z_valid) {
|
||||
_states.position(2) = _local_pos.z;
|
||||
if (PX4_ISFINITE(local_pos.z) && local_pos.z_valid) {
|
||||
states.position(2) = local_pos.z;
|
||||
|
||||
} else {
|
||||
_states.position(2) = NAN;
|
||||
states.position(2) = NAN;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_local_pos.vx) && PX4_ISFINITE(_local_pos.vy) && _local_pos.v_xy_valid) {
|
||||
_states.velocity(0) = _local_pos.vx;
|
||||
_states.velocity(1) = _local_pos.vy;
|
||||
_states.acceleration(0) = _vel_x_deriv.update(_states.velocity(0));
|
||||
_states.acceleration(1) = _vel_y_deriv.update(_states.velocity(1));
|
||||
if (PX4_ISFINITE(local_pos.vx) && PX4_ISFINITE(local_pos.vy) && local_pos.v_xy_valid) {
|
||||
states.velocity(0) = local_pos.vx;
|
||||
states.velocity(1) = local_pos.vy;
|
||||
states.acceleration(0) = _vel_x_deriv.update(local_pos.vx);
|
||||
states.acceleration(1) = _vel_y_deriv.update(local_pos.vy);
|
||||
|
||||
} else {
|
||||
_states.velocity(0) = _states.velocity(1) = NAN;
|
||||
_states.acceleration(0) = _states.acceleration(1) = NAN;
|
||||
states.velocity(0) = NAN;
|
||||
states.velocity(1) = NAN;
|
||||
states.acceleration(0) = NAN;
|
||||
states.acceleration(1) = NAN;
|
||||
|
||||
// reset derivatives to prevent acceleration spikes when regaining velocity
|
||||
_vel_x_deriv.reset();
|
||||
_vel_y_deriv.reset();
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_local_pos.vz) && _local_pos.v_z_valid) {
|
||||
_states.velocity(2) = _local_pos.vz;
|
||||
|
||||
if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {
|
||||
// A change in velocity is demanded. Set velocity to the derivative of position
|
||||
// because it has less bias but blend it in across the landing speed range
|
||||
float weighting = fminf(fabsf(vel_sp_z) / _param_mpc_land_speed.get(), 1.0f);
|
||||
_states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting);
|
||||
}
|
||||
|
||||
_states.acceleration(2) = _vel_z_deriv.update(_states.velocity(2));
|
||||
if (PX4_ISFINITE(local_pos.vz) && local_pos.v_z_valid) {
|
||||
states.velocity(2) = local_pos.vz;
|
||||
states.acceleration(2) = _vel_z_deriv.update(states.velocity(2));
|
||||
|
||||
} else {
|
||||
_states.velocity(2) = _states.acceleration(2) = NAN;
|
||||
states.velocity(2) = NAN;
|
||||
states.acceleration(2) = NAN;
|
||||
|
||||
// reset derivative to prevent acceleration spikes when regaining velocity
|
||||
_vel_z_deriv.reset();
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_local_pos.heading)) {
|
||||
_states.yaw = _local_pos.heading;
|
||||
}
|
||||
states.yaw = local_pos.heading;
|
||||
|
||||
return states;
|
||||
}
|
||||
|
||||
void MulticopterPositionControl::Run()
|
||||
@@ -272,14 +258,16 @@ void MulticopterPositionControl::Run()
|
||||
return;
|
||||
}
|
||||
|
||||
// reschedule backup
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
parameters_update(false);
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
vehicle_local_position_s local_pos;
|
||||
|
||||
if (_local_pos_sub.update(&_local_pos)) {
|
||||
|
||||
poll_subscriptions();
|
||||
parameters_update(false);
|
||||
|
||||
const hrt_abstime time_stamp_now = _local_pos.timestamp;
|
||||
if (_local_pos_sub.update(&local_pos)) {
|
||||
const hrt_abstime time_stamp_now = local_pos.timestamp;
|
||||
const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
|
||||
_time_stamp_last_loop = time_stamp_now;
|
||||
|
||||
@@ -289,26 +277,130 @@ void MulticopterPositionControl::Run()
|
||||
const bool was_in_failsafe = _in_failsafe;
|
||||
_in_failsafe = false;
|
||||
|
||||
vehicle_local_position_setpoint_s setpoint;
|
||||
_control_mode_sub.update(&_control_mode);
|
||||
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
|
||||
|
||||
// check if any task is active
|
||||
if (_trajectory_setpoint_sub.update(&setpoint)) {
|
||||
_control.setInputSetpoint(setpoint);
|
||||
if (_param_mpc_use_hte.get()) {
|
||||
hover_thrust_estimate_s hte;
|
||||
|
||||
// check if all local states are valid and map accordingly
|
||||
set_vehicle_states(setpoint.vz);
|
||||
_control.setState(_states);
|
||||
|
||||
vehicle_constraints_s constraints;
|
||||
|
||||
if (_vehicle_constraints_sub.update(&constraints)) {
|
||||
_control.setConstraints(constraints);
|
||||
_control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get());
|
||||
|
||||
if (constraints.reset_integral) {
|
||||
_control.resetIntegral();
|
||||
if (_hover_thrust_estimate_sub.update(&hte)) {
|
||||
if (hte.valid) {
|
||||
_control.updateHoverThrust(hte.hover_thrust);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PositionControlStates states{set_vehicle_states(local_pos)};
|
||||
|
||||
if (_control_mode.flag_control_climb_rate_enabled) {
|
||||
|
||||
_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) {
|
||||
if (PX4_ISFINITE(_setpoint.vx)) {
|
||||
_setpoint.vx += local_pos.delta_vxy[0];
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_setpoint.vy)) {
|
||||
_setpoint.vy += local_pos.delta_vxy[1];
|
||||
}
|
||||
}
|
||||
|
||||
if (local_pos.vz_reset_counter != _vz_reset_counter) {
|
||||
if (PX4_ISFINITE(_setpoint.vz)) {
|
||||
_setpoint.vz += local_pos.delta_vz;
|
||||
}
|
||||
}
|
||||
|
||||
if (local_pos.xy_reset_counter != _xy_reset_counter) {
|
||||
if (PX4_ISFINITE(_setpoint.x)) {
|
||||
_setpoint.x += local_pos.delta_xy[0];
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_setpoint.y)) {
|
||||
_setpoint.y += local_pos.delta_xy[1];
|
||||
}
|
||||
}
|
||||
|
||||
if (local_pos.z_reset_counter != _z_reset_counter) {
|
||||
if (PX4_ISFINITE(_setpoint.z)) {
|
||||
_setpoint.z += local_pos.delta_z;
|
||||
}
|
||||
}
|
||||
|
||||
if (local_pos.heading_reset_counter != _heading_reset_counter) {
|
||||
if (PX4_ISFINITE(_setpoint.yaw)) {
|
||||
_setpoint.yaw += local_pos.delta_heading;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update vehicle constraints and handle smooth takeoff
|
||||
_vehicle_constraints_sub.update(&_vehicle_constraints);
|
||||
|
||||
// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
|
||||
// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
|
||||
if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
|
||||
_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
|
||||
}
|
||||
|
||||
// handle smooth takeoff
|
||||
_takeoff.updateTakeoffState(_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 flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact);
|
||||
|
||||
if (not_taken_off || flying_but_ground_contact) {
|
||||
// we are not flying yet and need to avoid any corrections
|
||||
reset_setpoint_to_nan(_setpoint);
|
||||
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
|
||||
|
||||
// prevent any integrator windup
|
||||
_control.resetIntegral();
|
||||
}
|
||||
|
||||
// limit tilt during takeoff ramupup
|
||||
if (_takeoff.getTakeoffState() < TakeoffState::flight) {
|
||||
_control.setTiltLimit(math::radians(_param_mpc_tiltmax_lnd.get()));
|
||||
|
||||
} else {
|
||||
_control.setTiltLimit(math::radians(_param_mpc_tiltmax_air.get()));
|
||||
}
|
||||
|
||||
const float speed_up = _takeoff.updateRamp(dt, _vehicle_constraints.speed_up);
|
||||
const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :
|
||||
_param_mpc_z_vel_max_dn.get();
|
||||
const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy :
|
||||
_param_mpc_xy_vel_max.get();
|
||||
|
||||
// Allow ramping from zero thrust on takeoff
|
||||
const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
|
||||
|
||||
_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
|
||||
|
||||
_control.setVelocityLimits(
|
||||
math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()),
|
||||
math::constrain(speed_up, 0.f, _param_mpc_z_vel_max_up.get()),
|
||||
math::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get()));
|
||||
|
||||
_control.setInputSetpoint(_setpoint);
|
||||
|
||||
// update states
|
||||
if (PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
|
||||
&& PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) {
|
||||
// A change in velocity is demanded. Set velocity to the derivative of position
|
||||
// because it has less bias but blend it in across the landing speed range
|
||||
// < MPC_LAND_SPEED: ramp up using altitude derivative without a step
|
||||
// >= MPC_LAND_SPEED: use altitude derivative
|
||||
float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f);
|
||||
states.velocity(2) = local_pos.z_deriv * weighting + local_pos.vz * (1.f - weighting);
|
||||
}
|
||||
|
||||
_control.setState(states);
|
||||
|
||||
// Run position control
|
||||
if (_control.update(dt)) {
|
||||
@@ -321,13 +413,13 @@ void MulticopterPositionControl::Run()
|
||||
_last_warn = time_stamp_now;
|
||||
}
|
||||
|
||||
failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe);
|
||||
failsafe(time_stamp_now, _setpoint, states, !was_in_failsafe);
|
||||
|
||||
_control.setInputSetpoint(setpoint);
|
||||
|
||||
constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
|
||||
_control.setConstraints(constraints);
|
||||
// reset constraints
|
||||
_vehicle_constraints = {0, NAN, NAN, NAN, false, {}};
|
||||
|
||||
_control.setInputSetpoint(_setpoint);
|
||||
_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
|
||||
_control.update(dt);
|
||||
}
|
||||
|
||||
@@ -335,26 +427,39 @@ void MulticopterPositionControl::Run()
|
||||
// on top of the input/feed-forward setpoints these containt the PID corrections
|
||||
// This message is used by other modules (such as Landdetector) to determine vehicle intention.
|
||||
vehicle_local_position_setpoint_s local_pos_sp{};
|
||||
local_pos_sp.timestamp = time_stamp_now;
|
||||
_control.getLocalPositionSetpoint(local_pos_sp);
|
||||
local_pos_sp.timestamp = hrt_absolute_time();
|
||||
_local_pos_sp_pub.publish(local_pos_sp);
|
||||
|
||||
// Publish attitude setpoint output
|
||||
// It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized.
|
||||
// Not publishing when not running a flight task
|
||||
// in stabilized mode attitude setpoints get ignored
|
||||
// in offboard with attitude setpoints they come from MAVLink directly
|
||||
vehicle_attitude_setpoint_s attitude_setpoint{};
|
||||
attitude_setpoint.timestamp = time_stamp_now;
|
||||
_control.getAttitudeSetpoint(attitude_setpoint);
|
||||
attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
|
||||
|
||||
} else {
|
||||
// reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation
|
||||
_vel_x_deriv.reset();
|
||||
_vel_y_deriv.reset();
|
||||
_vel_z_deriv.reset();
|
||||
// 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);
|
||||
}
|
||||
|
||||
// Publish takeoff status
|
||||
const uint8_t takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState());
|
||||
|
||||
if (takeoff_state != _old_takeoff_state) {
|
||||
takeoff_status_s takeoff_status{};
|
||||
takeoff_status.takeoff_state = takeoff_state;
|
||||
takeoff_status.timestamp = hrt_absolute_time();
|
||||
_takeoff_status_pub.publish(takeoff_status);
|
||||
|
||||
_old_takeoff_state = takeoff_state;
|
||||
}
|
||||
|
||||
// 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);
|
||||
@@ -374,7 +479,7 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
|
||||
if (_failsafe_land_hysteresis.get_state()) {
|
||||
reset_setpoint_to_nan(setpoint);
|
||||
|
||||
if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) {
|
||||
if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
|
||||
// don't move along xy
|
||||
setpoint.vx = setpoint.vy = 0.f;
|
||||
|
||||
@@ -392,7 +497,7 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
|
||||
}
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_states.velocity(2))) {
|
||||
if (PX4_ISFINITE(states.velocity(2))) {
|
||||
// don't move along z if we can stop in all dimensions
|
||||
if (!PX4_ISFINITE(setpoint.vz)) {
|
||||
setpoint.vz = 0.f;
|
||||
|
||||
Reference in New Issue
Block a user