fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 15:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

MulticopterPositionControl: 2nd pass to move to FlightModeManager

Browse Source
This commit is contained in:
Matthias Grob
2020-10-24 12:32:32 +02:00
committed by Daniel Agar
parent f52bad87e2
commit 88c274b3cd
4 changed files with 81 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+14 -83
View File
@@ -46,11 +46,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
_vel_z_deriv(this, "VELD"),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time"))
{
if (vtol) {
// if vehicle is a VTOL we want to enable weathervane capabilities
_wv_controller = new WeatherVane();
}
// fetch initial parameter values
parameters_update(true);
@@ -60,8 +55,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
MulticopterPositionControl::~MulticopterPositionControl()
{
delete _wv_controller;
perf_free(_cycle_perf);
}
@@ -142,15 +135,6 @@ 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()));
// set trigger time for takeoff delay
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
if (_wv_controller != nullptr) {
_wv_controller->update_parameters();
}
}
return OK;
@@ -158,9 +142,7 @@ int MulticopterPositionControl::parameters_update(bool force)
void MulticopterPositionControl::poll_subscriptions()
{
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
_control_mode_sub.update(&_control_mode);
_home_pos_sub.update(&_home_pos);
if (_param_mpc_use_hte.get()) {
hover_thrust_estimate_s hte;
@@ -173,23 +155,6 @@ void MulticopterPositionControl::poll_subscriptions()
}
}
void MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &setpoint)
{
if (_vehicle_land_detected.alt_max < 0.0f || !_home_pos.valid_alt || !_local_pos.v_z_valid) {
// there is no altitude limitation present or the required information not available
return;
}
// maximum altitude == minimal z-value (NED)
const float min_z = _home_pos.z + (-_vehicle_land_detected.alt_max);
if (_states.position(2) < min_z) {
// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
setpoint.z = min_z;
setpoint.vz = math::max(setpoint.vz, 0.f);
}
}
void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
{
// only set position states if valid and finite
@@ -264,49 +229,27 @@ void MulticopterPositionControl::Run()
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;
// set _dt in controllib Block - the time difference since the last loop iteration in seconds
// set _dt in controllib Block for BlockDerivative
setDt(dt);
const bool was_in_failsafe = _in_failsafe;
_in_failsafe = false;
// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
// that turns the nose of the vehicle into the wind
if (_wv_controller != nullptr) {
// in manual mode we just want to use weathervane if position is controlled as well
// in mission, enabling wv is done in flight task
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
_wv_controller->activate();
} else {
_wv_controller->deactivate();
}
}
_wv_controller->update(_wv_dcm_z_sp_prev, _states.yaw);
}
// 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,
!_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
vehicle_local_position_setpoint_s setpoint;
// check if any task is active
if (_trajectory_setpoint_sub.update(&setpoint)) {
vehicle_constraints_s constraints;
_vehicle_constraints_sub.update(&constraints);
_control.setInputSetpoint(setpoint);
// check if all local states are valid and map accordingly
set_vehicle_states(setpoint.vz);
_control.setState(_states);
vehicle_constraints_s constraints;
_vehicle_constraints_sub.update(&constraints);
_control.setConstraints(constraints);
// Run position control
_control.setState(_states);
_control.setConstraints(constraints);
_control.setInputSetpoint(setpoint);
if (!_control.update(dt)) {
if ((time_stamp_now - _last_warn) > 1_s) {
PX4_WARN("invalid setpoints");
@@ -319,36 +262,24 @@ void MulticopterPositionControl::Run()
_control.update(dt);
}
// Fill local position, velocity and thrust setpoint.
// This message contains setpoints where each type of setpoint is either the input to the PositionController
// or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID).
// Example:
// If the desired setpoint is position-setpoint, _local_pos_sp will contain
// position-, velocity- and thrust-setpoint where the velocity- and thrust-setpoint were generated by the PositionControlller.
// If the desired setpoint has a velocity-setpoint only, then _local_pos_sp will contain valid velocity- and thrust-setpoint, but the position-setpoint
// will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the
// PositionController.
// Publish internal position control setpoints
// 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);
// Publish local position setpoint
// This message will be used by other modules (such as Landdetector) to determine vehicle intention.
_local_pos_sp_pub.publish(local_pos_sp);
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(attitude_setpoint);
// publish attitude setpoint
// 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);
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
_wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z();
} else {
// reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation
_vel_x_deriv.reset();