mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
Rover: use Y accel computed from gyro and velocity
this should be less susceptible to noise and attitude errors, hopefully leading to better learning
This commit is contained in:
parent
ecccc05eed
commit
887942471e
@ -240,7 +240,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
|||||||
mavlink_msg_nav_controller_output_send(
|
mavlink_msg_nav_controller_output_send(
|
||||||
chan,
|
chan,
|
||||||
lateral_acceleration, // use nav_roll to hold demanded Y accel
|
lateral_acceleration, // use nav_roll to hold demanded Y accel
|
||||||
ins.get_accel().y, // use nav_pitch to hold actual Y accel
|
g_gps->ground_speed_cm * 0.01f * ins.get_gyro().z, // use nav_pitch to hold actual Y accel
|
||||||
nav_controller->nav_bearing_cd() * 0.01f,
|
nav_controller->nav_bearing_cd() * 0.01f,
|
||||||
nav_controller->target_bearing_cd() * 0.01f,
|
nav_controller->target_bearing_cd() * 0.01f,
|
||||||
wp_distance,
|
wp_distance,
|
||||||
|
@ -286,10 +286,16 @@ static void steering_learning(void)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
/*
|
/*
|
||||||
the idea is to slowly adjust the turning circle
|
the idea is to slowly adjust the turning circle to bring the
|
||||||
|
actual and desired turn rates into line
|
||||||
*/
|
*/
|
||||||
float demanded = lateral_acceleration;
|
float demanded = lateral_acceleration;
|
||||||
float actual = ins.get_accel().y;
|
/*
|
||||||
|
for Y accel use the gyro times the velocity, as that is less
|
||||||
|
noise sensitive, and is a more direct measure of steering rate,
|
||||||
|
which is what we are really trying to control
|
||||||
|
*/
|
||||||
|
float actual = ins.get_gyro().z * 0.01f * g_gps->ground_speed_cm;
|
||||||
if (fabsf(actual) < 0.1f) {
|
if (fabsf(actual) < 0.1f) {
|
||||||
// too little acceleration to really measure accurately
|
// too little acceleration to really measure accurately
|
||||||
return;
|
return;
|
||||||
|
Loading…
Reference in New Issue
Block a user