dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_att_control, mc_pos_control: update manual_control_setpoint usage

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/modules/mc_att_control/mc_att_control_main.cpp
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;
}

29
src/modules/mc_att_control/mc_att_control_params.c
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);

14
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -148,9 +148,6 @@ private:
param_t tilt_max;
param_t land_speed;
param_t land_tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;
} _params_handles; /**< handles for interesting parameters */
struct {
@ -160,9 +157,6 @@ private:
float land_speed;
float land_tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
math::Vector<3> vel_i;
@ -306,8 +300,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.tilt_max = param_find("MPC_TILT_MAX");
_params_handles.land_speed = param_find("MPC_LAND_SPEED");
_params_handles.land_tilt_max = param_find("MPC_LAND_TILT");
_params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
/* fetch initial parameter values */
parameters_update(true);
@ -354,8 +346,6 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.tilt_max, &_params.tilt_max);
param_get(_params_handles.land_speed, &_params.land_speed);
param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
float v;
param_get(_params_handles.xy_p, &v);
@ -608,8 +598,8 @@ MulticopterPositionControl::task_main()
reset_lat_lon_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
sp_move_rate(0) = _manual.pitch;
sp_move_rate(1) = _manual.roll;
}
/* limit setpoint move rate */

22
src/modules/sensors/sensor_params.c
View File

@ -647,28 +647,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
/**
* Roll scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
/**
* Pitch scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
/**
* Yaw scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
/**
* Failsafe channel mapping.
*

19
src/modules/sensors/sensors.cpp
View File

@ -268,11 +268,6 @@ private:
int rc_map_aux4;
int rc_map_aux5;
float rc_scale_roll;
float rc_scale_pitch;
float rc_scale_yaw;
float rc_scale_flaps;
int rc_fs_ch;
int rc_fs_mode;
float rc_fs_thr;
@ -318,11 +313,6 @@ private:
param_t rc_map_aux4;
param_t rc_map_aux5;
param_t rc_scale_roll;
param_t rc_scale_pitch;
param_t rc_scale_yaw;
param_t rc_scale_flaps;
param_t rc_fs_ch;
param_t rc_fs_mode;
param_t rc_fs_thr;
@ -536,11 +526,6 @@ Sensors::Sensors() :
_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
/* RC failsafe */
_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
@ -696,10 +681,6 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));