From 7e3716b1a1f99d8e5215b6b50fa79ed469eb3a45 Mon Sep 17 00:00:00 2001 From: daohu527 Date: Sat, 10 Apr 2021 18:38:15 +0800 Subject: [PATCH] control: fix vehicle_state pitch error --- modules/common/vehicle_state/vehicle_state_provider.cc | 4 ++-- modules/control/controller/lon_controller.cc | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/common/vehicle_state/vehicle_state_provider.cc b/modules/common/vehicle_state/vehicle_state_provider.cc index 185ac43ced..f56c4dd10d 100644 --- a/modules/common/vehicle_state/vehicle_state_provider.cc +++ b/modules/common/vehicle_state/vehicle_state_provider.cc @@ -143,8 +143,8 @@ bool VehicleStateProvider::ConstructExceptLinearVelocity( } if (localization.pose().has_euler_angles()) { - vehicle_state_.set_roll(localization.pose().euler_angles().x()); - vehicle_state_.set_pitch(localization.pose().euler_angles().y()); + vehicle_state_.set_roll(localization.pose().euler_angles().y()); + vehicle_state_.set_pitch(localization.pose().euler_angles().x()); vehicle_state_.set_yaw(localization.pose().euler_angles().z()); } else { math::EulerAnglesZXYd euler_angle(orientation.qw(), orientation.qx(), diff --git a/modules/control/controller/lon_controller.cc b/modules/control/controller/lon_controller.cc index 61e89f026e..e402072607 100644 --- a/modules/control/controller/lon_controller.cc +++ b/modules/control/controller/lon_controller.cc @@ -255,14 +255,14 @@ Status LonController::ComputeControlCommand( speed_leadlag_controller_.InnerstateSaturationStatus()); } - double slope_offset_compenstaion = digital_filter_pitch_angle_.Filter( + double slope_offset_compensation = digital_filter_pitch_angle_.Filter( GRA_ACC * std::sin(injector_->vehicle_state()->pitch())); - if (std::isnan(slope_offset_compenstaion)) { - slope_offset_compenstaion = 0; + if (std::isnan(slope_offset_compensation)) { + slope_offset_compensation = 0; } - debug->set_slope_offset_compensation(slope_offset_compenstaion); + debug->set_slope_offset_compensation(slope_offset_compensation); double acceleration_cmd = acceleration_cmd_closeloop + debug->preview_acceleration_reference() + -- GitLab