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

mc_att_control, mc_pos_control: update manual_control_setpoint usage

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-03 20:54:28 +04:00
parent 6f38ed3b4b
commit e2ac5222d8
5 changed files with 57 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+26 -28
View File
@@ -157,7 +157,9 @@ private:
param_t yaw_rate_d;
param_t yaw_ff;
param_t rc_scale_yaw;
param_t man_scale_roll;
param_t man_scale_pitch;
param_t man_scale_yaw;
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -167,7 +169,9 @@ private:
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
float rc_scale_yaw;
float man_scale_roll;
float man_scale_pitch;
float man_scale_yaw;
} _params;
/**
@@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_params_handles.man_scale_roll = param_find("MC_SCALE_ROLL");
_params_handles.man_scale_pitch = param_find("MC_SCALE_PITCH");
_params_handles.man_scale_yaw = param_find("MC_SCALE_YAW");
/* fetch initial parameter values */
parameters_update();
@@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll */
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
@@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
/* pitch */
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v);
@@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
/* yaw */
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
@@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
/* manual control scale */
param_get(_params_handles.man_scale_roll, &_params.man_scale_roll);
param_get(_params_handles.man_scale_pitch, &_params.man_scale_pitch);
param_get(_params_handles.man_scale_yaw, &_params.man_scale_yaw);
_params.man_scale_roll *= M_PI / 180.0;
_params.man_scale_pitch *= M_PI / 180.0;
_params.man_scale_yaw *= M_PI / 180.0;
return OK;
}
@@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
@@ -483,24 +495,10 @@ MulticopterAttitudeControl::control_attitude(float dt)
// reset_yaw_sp = true;
//}
} else {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
if (_manual_control_sp.yaw > 0.0f) {
yaw_sp_move_rate -= YAW_DEADZONE;
} else {
yaw_sp_move_rate += YAW_DEADZONE;
}
yaw_sp_move_rate *= _params.rc_scale_yaw;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
/* move yaw setpoint */
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_scale_yaw * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
/* reset yaw setpint to current position if needed */
@@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_v_att_sp.roll_body = _manual_control_sp.roll;
_v_att_sp.pitch_body = _manual_control_sp.pitch;
_v_att_sp.roll_body = _manual_control_sp.roll * _params.man_scale_roll;
_v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_scale_pitch;
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
src/modules/mc_att_control/mc_att_control_params.c
+29
View File
@@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
/**
* Manual control scaling factor for roll
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_SCALE_ROLL, 35.0f);
/**
* Manual control scaling factor for pitch
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_SCALE_PITCH, 35.0f);
/**
* Manual control scaling factor for yaw
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_SCALE_YAW, 120.0f);