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

Rover: simple mode calcs clarified and const added

This commit is contained in:
Randy Mackay 2018-09-11 14:20:39 +09:00
parent efdd3946fc
commit 14fc117cd4
1 changed files with 4 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/mode.cpp
View File

@ -149,19 +149,14 @@ void Mode::get_pilot_desired_lateral(float &lateral_out)
void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out)
{
// get steering and throttle in the -1 to +1 range
float desired_steering = constrain_float(rover.channel_steer->get_control_in() / 4500.0f, -1.0, 1.0f);
float desired_throttle = constrain_float(rover.channel_throttle->get_control_in() / 100.0f, -1.0f, 1.0f);
const float desired_steering = constrain_float(rover.channel_steer->norm_input_dz(), -1.0f, 1.0f);
const float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f);
// calculate angle of input stick vector
heading_out = wrap_360_cd(atan2f(desired_steering, desired_throttle) * DEGX100);
// calculate magnitude of input stick vector
float magnitude_max = 1.0f;
float magnitude = safe_sqrt(sq(desired_throttle) + sq(desired_steering));
if (magnitude > magnitude_max) {
magnitude = magnitude_max;
}
float throttle = magnitude / magnitude_max;
// calculate throttle using magnitude of input stick vector
const float throttle = MIN(safe_sqrt(sq(desired_throttle) + sq(desired_steering)), 1.0f);
speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
}