aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: steering mark floats as float and fix parenthesis

Browse Source
This commit is contained in:
Pierre Kancir 2017-06-19 17:48:08 +02:00 committed by Randy Mackay
parent 97595f98ac
commit ee28e49790
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
APMrover2/Steering.cpp
View File

@ -118,7 +118,7 @@ void Rover::calc_throttle(float target_speed) {
// use g.speed_turn_gain for a 90 degree turn, and in proportion
// for other turn angles
const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0.0f, 1.0f);
const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
float reduction = 1.0f - steer_rate * speed_turn_reduction;
@ -136,7 +136,7 @@ void Rover::calc_throttle(float target_speed) {
groundspeed_error = fabsf(target_speed) - ground_speed;
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100.0f) / 100.0f);
// also reduce the throttle by the reduction factor. This gives a
// much faster response in turns
@ -156,7 +156,7 @@ void Rover::calc_throttle(float target_speed) {
// We use a linear gain, with 0 gain at a ground speed error
// of braking_speederr, and 100% gain when groundspeed_error
// is 2*braking_speederr
const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
@ -238,8 +238,8 @@ void Rover::calc_nav_steer() {
*/
void Rover::mix_skid_steering(void)
{
const float steering_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0;
const float throttle_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0;
const float steering_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0f;
const float throttle_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f;
float motor1 = throttle_scaled + 0.5f * steering_scaled;
float motor2 = throttle_scaled - 0.5f * steering_scaled;
@ -250,8 +250,8 @@ void Rover::mix_skid_steering(void)
}
// first new-style skid steering
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000 * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000 * motor2);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000.0f * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motor2);
// now old style skid steering with skid_steer_out
if (g.skid_steer_out) {
@ -326,7 +326,7 @@ void Rover::set_servos(void) {
if (have_skid_steering()) {
mix_skid_steering();
}
}
if (!arming.is_armed()) {
// Some ESCs get noisy (beep error msgs) if PWM == 0.