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

multirotor_pos_control: bugs fixed

Browse Source
This commit is contained in:
Anton Babushkin
2013-06-20 17:34:09 +04:00
parent eb38ea1789
commit 7d37c2a8b3
2 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/multirotor_pos_control/multirotor_pos_control.c
+7 -7
View File
@@ -302,14 +302,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
/* move position setpoint with manual controls */
float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) {
float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl);
float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max;
pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl;
pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl;
float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max;
pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed;
pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed;
local_pos_sp.x += pos_sp_speed_x * dt;
local_pos_sp.y += pos_sp_speed_y * dt;
/* limit maximum setpoint from position offset and preserve direction */
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+1 -1
View File
@@ -68,7 +68,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
h->pos_d = param_find("MPC_POS_D");
h->pos_rate_max = param_find("MPC_POS_RATE_MAX");
h->slope_max = param_find("MPC_SLOPE_MAX");
h->slope_max = param_find("MPC_HARD");
h->hard = param_find("MPC_HARD");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
h->rc_scale_roll = param_find("RC_SCALE_ROLL");