diff options
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 54 | ||||
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_params.c | 29 | ||||
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 | ||||
| -rw-r--r-- | src/modules/sensors/sensor_params.c | 22 | ||||
| -rw-r--r-- | src/modules/sensors/sensors.cpp | 19 |
5 files changed, 57 insertions, 81 deletions
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 9cb8e8344..01f1631a9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 488107d58..9acf8bfa3 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -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); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 78d06ba5b..2bd2d356a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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 */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a..09afaf949 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -648,28 +648,6 @@ 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. * * @min 0 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e08d8618f..8f350751c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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)); |