Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: acro and steering use get_pilot_desired_steering_and_speed

Browse Source
This commit is contained in:
Randy Mackay 2018-05-06 10:48:26 +09:00
parent 957458ca56
commit 3645e18243
2 changed files with 24 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
APMrover2/mode_acro.cpp
View File

@ -3,14 +3,24 @@
void ModeAcro::update() void ModeAcro::update()
{ {
// get speed forward
// convert pilot stick input into desired steering and throttle float speed, desired_steering;
float desired_steering, desired_throttle; if (!attitude_control.get_forward_speed(speed)) {
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); float desired_throttle;
// convert pilot stick input into desired steering and throttle
// set reverse flag backing up get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
const bool reversed = is_negative(desired_throttle); // no valid speed, just use the provided throttle
rover.set_reverse(reversed); g2.motors.set_throttle(desired_throttle);
// set reverse flag if negative throttle
rover.set_reverse(is_negative(desired_throttle));
} else {
float desired_speed;
// convert pilot stick input into desired steering and speed
get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
calc_throttle(desired_speed, false, true);
// set reverse flag if negative desired speed
rover.set_reverse(is_negative(desired_speed));
}
// convert pilot steering input to desired turn rate in radians/sec // 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); const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
@ -22,19 +32,6 @@ void ModeAcro::update()
g2.motors.limit.steer_right); g2.motors.limit.steer_right);
g2.motors.set_steering(steering_out * 4500.0f); g2.motors.set_steering(steering_out * 4500.0f);
// 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);
calc_throttle(target_speed, false, true);
}
} }
bool ModeAcro::requires_velocity() const bool ModeAcro::requires_velocity() const

15
APMrover2/mode_steering.cpp
View File

@ -3,12 +3,6 @@
void ModeSteering::update() void ModeSteering::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 // get speed forward
float speed; float speed;
if (!attitude_control.get_forward_speed(speed)) { if (!attitude_control.get_forward_speed(speed)) {
@ -18,8 +12,11 @@ void ModeSteering::update()
return; return;
} }
float desired_steering, desired_speed;
get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
// determine if pilot is requesting pivot turn // determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering)); bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(desired_speed) && (!is_zero(desired_steering));
// In steering mode we control lateral acceleration directly. // In steering mode we control lateral acceleration directly.
// For pivot steering vehicles we use the TURN_MAX_G parameter // For pivot steering vehicles we use the TURN_MAX_G parameter
@ -39,7 +36,7 @@ void ModeSteering::update()
// reverse target lateral acceleration if backing up // reverse target lateral acceleration if backing up
bool reversed = false; bool reversed = false;
if (is_negative(target_speed)) { if (is_negative(desired_speed)) {
reversed = true; reversed = true;
desired_lat_accel = -desired_lat_accel; desired_lat_accel = -desired_lat_accel;
} }
@ -51,5 +48,5 @@ void ModeSteering::update()
calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
// run speed to throttle controller // run speed to throttle controller
calc_throttle(target_speed, false, true); calc_throttle(desired_speed, false, true);
} }