Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: circle uses lower accel and fix forward-back accel

This commit is contained in:
Randy Mackay 2023-06-29 19:31:44 +09:00
parent 752ef898d4
commit 1cbbaeb283
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Rover/mode_circle.cpp
View File

@ -155,8 +155,9 @@ void ModeCircle::update()
// accelerate speed up to desired speed
const float speed_max = reached_edge ? config.speed : 0.0;
const float speed_change_max = (g2.pos_control.get_accel_max() * rover.G_Dt);
target.speed = constrain_float(speed_max, target.speed - speed_change_max, target.speed + speed_change_max);
const float speed_change_max = (g2.pos_control.get_accel_max() * 0.5 * rover.G_Dt);
const float accel_fb = constrain_float(speed_max - target.speed, -speed_change_max, speed_change_max);
target.speed += accel_fb;
// calculate angular rate and update target angle
const float circumference = 2.0 * M_PI * config.radius;
@ -174,8 +175,8 @@ void ModeCircle::update()
target.vel.rotate(target.yaw_rad + radians(90));
// acceleration is towards center of circle and is speed^2 / radius
target.accel = Vector2f(sq(target.speed) / config.radius, 0);
target.accel.rotate(target.yaw_rad + radians(180));
target.accel = Vector2f(-sq(target.speed) / config.radius, accel_fb / rover.G_Dt);
target.accel.rotate(target.yaw_rad);
g2.pos_control.set_pos_vel_accel_target(target.pos, target.vel, target.accel);
g2.pos_control.update(rover.G_Dt);