Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 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/flight_mode_manager/FlightModeManager.cpp
+60 -11
View File
@@ -37,15 +37,22 @@
using namespace time_literals;
FlightModeManager::FlightModeManager() :
FlightModeManager::FlightModeManager(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
if (vtol) {
// if vehicle is a VTOL we want to enable weathervane capabilities
_wv_controller = new WeatherVane();
}
updateParams();
}
FlightModeManager::~FlightModeManager()
{
delete _wv_controller;
perf_free(_loop_perf);
}
@@ -56,8 +63,8 @@ bool FlightModeManager::init()
return false;
}
// limit to every other vehicle_local_position update (~62.5 Hz)
_vehicle_local_position_sub.set_interval_us(16_ms);
// limit to every other vehicle_local_position update (50 Hz)
_vehicle_local_position_sub.set_interval_us(20_ms);
_time_stamp_last_loop = hrt_absolute_time();
return true;
}
@@ -77,9 +84,7 @@ void FlightModeManager::Run()
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
_flight_tasks.handleParameterUpdate();
}
// generate setpoints on local position changes
@@ -92,6 +97,29 @@ void FlightModeManager::Run()
_vehicle_local_position_setpoint_sub.update();
_vehicle_status_sub.update();
// // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes TODO
// _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f,
// !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
// // 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 TODO
// // 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_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z(); // TODO
// _wv_controller->update(_wv_dcm_z_sp_prev, _states.yaw);
// }
const hrt_abstime time_stamp_now = hrt_absolute_time();
// Guard against too small (< 0.2ms) and too large (> 100ms) dt's.
const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f);
@@ -108,6 +136,20 @@ void FlightModeManager::Run()
perf_end(_loop_perf);
}
void FlightModeManager::updateParams()
{
ModuleParams::updateParams();
_flight_tasks.handleParameterUpdate();
_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();
}
}
void FlightModeManager::start_flight_task()
{
bool task_failure = false;
@@ -494,7 +536,15 @@ void FlightModeManager::reset_setpoint_to_nan(vehicle_local_position_setpoint_s
int FlightModeManager::task_spawn(int argc, char *argv[])
{
FlightModeManager *instance = new FlightModeManager();
bool vtol = false;
if (argc > 1) {
if (strcmp(argv[1], "vtol") == 0) {
vtol = true;
}
}
FlightModeManager *instance = new FlightModeManager(vtol);
if (instance) {
_object.store(instance);
@@ -542,15 +592,14 @@ int FlightModeManager::print_usage(const char *reason)
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements the multicopter rate controller. It takes rate setpoints (in acro mode
via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
The controller has a PID loop for angular rate error.
This implements the setpoint generation for all modes. It takes the current mode state of the vehicle as input
and outputs setpoints for controllers.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("flight_mode_manager", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
src/modules/flight_mode_manager/FlightModeManager.hpp
+7 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -51,7 +51,7 @@
class FlightModeManager : public ModuleBase<FlightModeManager>, public ModuleParams, public px4::WorkItem
{
public:
FlightModeManager();
FlightModeManager(bool vtol = false);
~FlightModeManager() override;
/** @see ModuleBase */
@@ -70,6 +70,7 @@ public:
private:
void Run() override;
void updateParams() override;
void start_flight_task();
void check_failure(bool task_failure, uint8_t nav_state);
void send_vehicle_cmd_do(uint8_t nav_state);
@@ -105,6 +106,9 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time,
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t,
(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc
);
};
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();
src/modules/mc_pos_control/MulticopterPositionControl.hpp
-13
View File
@@ -104,10 +104,8 @@ private:
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */
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)};
@@ -150,8 +148,6 @@ private:
(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,
@@ -189,9 +185,6 @@ private:
systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
WeatherVane *_wv_controller{nullptr};
Vector3f _wv_dcm_z_sp_prev{0, 0, 1};
perf_counter_t _cycle_perf;
/**
@@ -212,12 +205,6 @@ private:
*/
void set_vehicle_states(const float &vel_sp_z);
/**
* Limit altitude based on land-detector.
* @param setpoint needed to detect vehicle intention.
*/
void limit_altitude(vehicle_local_position_setpoint_s &setpoint);
/**
* Adjust the setpoint during landing.
* Thrust is adjusted to support the land-detector during detection.