diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 6d343638ef..8a18c5f7e1 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -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(¶m_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; diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 26584ba93f..71f71b8a21 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -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, 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) _param_mpc_pos_mode, (ParamFloat) _param_mpc_tiltmax_lnd, - (ParamFloat) _param_mpc_z_vel_max_up + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_spoolup_time, + (ParamFloat) _param_mpc_tko_ramp_t, + (ParamFloat) _param_mpc_z_vel_p_acc ); }; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 468352d9c9..860cfeca72 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -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(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index aeee26d2ff..685831518c 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -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) _param_mpc_use_hte, // Takeoff / Land - (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ - (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ (ParamFloat) _param_mpc_tko_speed, (ParamFloat) _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.