fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 07:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC pos control: Use separate params for manual control

Browse Source
This commit is contained in:
Lorenz Meier
2015-07-22 17:12:43 +02:00
parent 033c512fbf
commit d94f2aa407
1 changed files with 15 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+15 -5
View File
@@ -80,6 +80,9 @@
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h> #include <platforms/px4_defines.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#define TILT_COS_MAX 0.7f #define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f #define SIGMA 0.000001f
#define MIN_DIST 0.01f #define MIN_DIST 0.01f
@@ -92,7 +95,7 @@
*/ */
extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
class MulticopterPositionControl class MulticopterPositionControl : public control::SuperBlock
{ {
public: public:
/** /**
@@ -146,6 +149,8 @@ private:
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
control::BlockParamFloat _manual_thr_min;
control::BlockParamFloat _manual_thr_max;
struct { struct {
param_t thr_min; param_t thr_min;
@@ -290,7 +295,7 @@ MulticopterPositionControl *g_control;
} }
MulticopterPositionControl::MulticopterPositionControl() : MulticopterPositionControl::MulticopterPositionControl() :
SuperBlock(NULL, "MPC"),
_task_should_exit(false), _task_should_exit(false),
_control_task(-1), _control_task(-1),
_mavlink_fd(-1), _mavlink_fd(-1),
@@ -310,7 +315,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_att_sp_pub(-1), _att_sp_pub(-1),
_local_pos_sp_pub(-1), _local_pos_sp_pub(-1),
_global_vel_sp_pub(-1), _global_vel_sp_pub(-1),
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_ref_alt(0.0f), _ref_alt(0.0f),
_ref_timestamp(0), _ref_timestamp(0),
@@ -410,6 +416,10 @@ MulticopterPositionControl::parameters_update(bool force)
} }
if (updated || force) { if (updated || force) {
/* update C++ param system */
updateParams();
/* update legacy C interface params */
param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
@@ -1413,11 +1423,11 @@ MulticopterPositionControl::task_main()
/* control throttle directly if no climb rate controller is active */ /* control throttle directly if no climb rate controller is active */
if (!_control_mode.flag_control_climb_rate_enabled) { if (!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, _params.thr_max); _att_sp.thrust = math::min(_manual.z, _manual_thr_max.get());
/* enforce minimum throttle if not landed */ /* enforce minimum throttle if not landed */
if (!_vehicle_status.condition_landed) { if (!_vehicle_status.condition_landed) {
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); _att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
} }
} }