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

Rover: re-order acro mode to add early return 

non-functional change
This commit is contained in:
Randy Mackay 2017-11-29 11:55:11 +09:00
parent 060f1d36dd
commit 69dc68d621
1 changed files with 19 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
APMrover2/mode_acro.cpp
View File

@ -3,12 +3,6 @@
void ModeAcro::update()
{
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed)
float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
@ -18,23 +12,31 @@ void ModeAcro::update()
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 (up to twice the cruise speed)
float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// convert pilot steering input to desired turn rate in radians/sec
float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
// determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
// convert steering request to turn rate in radians/sec
float turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
// stop vehicle if target speed is zero and not pivot turning
if (is_zero(target_speed) && !is_pivot_turning) {
stop_vehicle();
return;
}
// set reverse flag backing up
bool reversed = is_negative(target_speed);
rover.set_reverse(reversed);
// run speed to throttle output controller
if (is_zero(target_speed) && !is_pivot_turning) {
stop_vehicle();
} else {
// run steering turn rate controller and throttle controller
float steering_out = attitude_control.get_steering_out_rate(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);
}
// run steering turn rate controller and throttle controller
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);
}