diff options
| -rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 3 | ||||
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 |
2 files changed, 4 insertions, 3 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6252178d1..10f21c55d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -259,7 +259,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); + // TODO 1000.0 is hotfix + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 42b5fcb61..d4b2f0e43 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -453,8 +453,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) gps_corr[1][0] = gps_proj[1] - y_est[0]; if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s; - gps_corr[1][1] = gps.vel_e_m_s; + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; } else { gps_corr[0][1] = 0.0f; |