提交 7e3716b1 编写于 作者: D daohu527 提交者: Liu Jiaming

control: fix vehicle_state pitch error

上级 0b8eeda4
...@@ -143,8 +143,8 @@ bool VehicleStateProvider::ConstructExceptLinearVelocity( ...@@ -143,8 +143,8 @@ bool VehicleStateProvider::ConstructExceptLinearVelocity(
} }
if (localization.pose().has_euler_angles()) { if (localization.pose().has_euler_angles()) {
vehicle_state_.set_roll(localization.pose().euler_angles().x()); vehicle_state_.set_roll(localization.pose().euler_angles().y());
vehicle_state_.set_pitch(localization.pose().euler_angles().y()); vehicle_state_.set_pitch(localization.pose().euler_angles().x());
vehicle_state_.set_yaw(localization.pose().euler_angles().z()); vehicle_state_.set_yaw(localization.pose().euler_angles().z());
} else { } else {
math::EulerAnglesZXYd euler_angle(orientation.qw(), orientation.qx(), math::EulerAnglesZXYd euler_angle(orientation.qw(), orientation.qx(),
......
...@@ -255,14 +255,14 @@ Status LonController::ComputeControlCommand( ...@@ -255,14 +255,14 @@ Status LonController::ComputeControlCommand(
speed_leadlag_controller_.InnerstateSaturationStatus()); 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())); GRA_ACC * std::sin(injector_->vehicle_state()->pitch()));
if (std::isnan(slope_offset_compenstaion)) { if (std::isnan(slope_offset_compensation)) {
slope_offset_compenstaion = 0; slope_offset_compensation = 0;
} }
debug->set_slope_offset_compensation(slope_offset_compenstaion); debug->set_slope_offset_compensation(slope_offset_compensation);
double acceleration_cmd = double acceleration_cmd =
acceleration_cmd_closeloop + debug->preview_acceleration_reference() + acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册