Rover: reporting gets lat accel from attitude control

This commit is contained in:
Randy Mackay 2017-12-08 10:21:44 +09:00
parent b778546eda
commit 8e80490098
2 changed files with 2 additions and 2 deletions

View File

@ -131,7 +131,7 @@ void Rover::send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
control_mode->lateral_acceleration, // use nav_roll to hold demanded Y accel
g2.attitude_control.get_desired_lat_accel(),
ahrs.groundspeed() * ins.get_gyro().z, // use nav_pitch to hold actual Y accel
nav_controller->nav_bearing_cd() * 0.01f,
nav_controller->target_bearing_cd() * 0.01f,

View File

@ -180,7 +180,7 @@ void Rover::Log_Write_Rangefinder()
struct log_Rangefinder pkt = {
LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG),
time_us : AP_HAL::micros64(),
lateral_accel : control_mode->lateral_acceleration,
lateral_accel : g2.attitude_control.get_desired_lat_accel(),
rangefinder1_distance : s0 ? s0->distance_cm() : (uint16_t)0,
rangefinder2_distance : s1 ? s1->distance_cm() : (uint16_t)0,
detected_count : obstacle.detected_count,