diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-06-20 17:32:29 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-06-20 17:34:09 +0400 |
| commit | 7d37c2a8b36309f27d7001ba035d30c72620ba05 (patch) | |
| tree | 4aaf00d975707adfe28a6cdc1124c5983e1fab65 /src/modules/multirotor_pos_control | |
| parent | eb38ea17896e9970e9bdf532557ebcd87f81e34a (diff) | |
| download | px4-firmware-7d37c2a8b36309f27d7001ba035d30c72620ba05.tar.gz px4-firmware-7d37c2a8b36309f27d7001ba035d30c72620ba05.tar.bz2 px4-firmware-7d37c2a8b36309f27d7001ba035d30c72620ba05.zip | |
multirotor_pos_control: bugs fixed
Diffstat (limited to 'src/modules/multirotor_pos_control')
| -rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 14 | ||||
| -rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control_params.c | 2 |
2 files changed, 8 insertions, 8 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c94972e7d..508879ae2 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -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 */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7c3296cae..d284de433 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -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"); |