mirror of https://github.com/ArduPilot/ardupilot
Rover: Allow ACRO mode for skid steer rovers without GPS
This commit is contained in:
parent
753b710477
commit
e782ce2b51
|
@ -3,34 +3,41 @@
|
|||
|
||||
void ModeAcro::update()
|
||||
{
|
||||
// get speed forward
|
||||
float speed;
|
||||
if (!attitude_control.get_forward_speed(speed)) {
|
||||
// no valid speed so stop
|
||||
g2.motors.set_throttle(0.0f);
|
||||
g2.motors.set_steering(0.0f);
|
||||
return;
|
||||
}
|
||||
|
||||
// convert pilot stick input into desired steering and throttle
|
||||
float desired_steering, desired_throttle;
|
||||
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
|
||||
|
||||
// convert pilot throttle input to desired speed
|
||||
float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
|
||||
// set reverse flag backing up
|
||||
const bool reversed = is_negative(desired_throttle);
|
||||
rover.set_reverse(reversed);
|
||||
|
||||
// convert pilot steering input to desired turn rate in radians/sec
|
||||
const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
|
||||
|
||||
// set reverse flag backing up
|
||||
const bool reversed = is_negative(target_speed);
|
||||
rover.set_reverse(reversed);
|
||||
|
||||
// apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
|
||||
rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
|
||||
|
||||
// run steering turn rate controller and throttle controller
|
||||
const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
|
||||
const float steering_out = attitude_control.get_steering_out_rate(
|
||||
target_turn_rate,
|
||||
g2.motors.have_skid_steering(),
|
||||
g2.motors.limit.steer_left,
|
||||
g2.motors.limit.steer_right,
|
||||
reversed);
|
||||
|
||||
g2.motors.set_steering(steering_out * 4500.0f);
|
||||
calc_throttle(target_speed, false);
|
||||
|
||||
// get speed forward
|
||||
float speed;
|
||||
if (!attitude_control.get_forward_speed(speed)) {
|
||||
// no valid speed, just use the provided throttle
|
||||
g2.motors.set_throttle(desired_throttle);
|
||||
} else {
|
||||
|
||||
// convert pilot throttle input to desired speed
|
||||
float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
|
||||
|
||||
// apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
|
||||
rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
|
||||
|
||||
calc_throttle(target_speed, false);
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue