Browse Source

Rover: refactor motor.set_steering() to mode.set_steering()

mission-4.1.18
Tom Pittenger 6 years ago committed by Tom Pittenger
parent
commit
a798f9eb27
  1. 9
      APMrover2/mode.cpp
  2. 1
      APMrover2/mode.h
  3. 2
      APMrover2/mode_acro.cpp
  4. 2
      APMrover2/mode_guided.cpp
  5. 2
      APMrover2/mode_steering.cpp

9
APMrover2/mode.cpp

@ -511,7 +511,7 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse @@ -511,7 +511,7 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
set_steering(steering_out * 4500.0f);
}
// calculate steering output to drive towards desired heading
@ -527,7 +527,7 @@ void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max_deg @@ -527,7 +527,7 @@ void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max_deg
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
set_steering(steering_out * 4500.0f);
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
@ -555,6 +555,11 @@ void Mode::calc_stopping_location(Location& stopping_loc) @@ -555,6 +555,11 @@ void Mode::calc_stopping_location(Location& stopping_loc)
stopping_loc.offset(stopping_offset.x, stopping_offset.y);
}
void Mode::set_steering(float steering_value)
{
g2.motors.set_steering(steering_value);
}
Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
{
Mode *ret = nullptr;

1
APMrover2/mode.h

@ -195,6 +195,7 @@ protected: @@ -195,6 +195,7 @@ protected:
// steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
// throttle_out is in the range -100 ~ +100
void get_pilot_input(float &steering_out, float &throttle_out);
void set_steering(float steering_value);
// references to avoid code churn:
class AP_AHRS &ahrs;

2
APMrover2/mode_acro.cpp

@ -43,7 +43,7 @@ void ModeAcro::update() @@ -43,7 +43,7 @@ void ModeAcro::update()
rover.G_Dt);
}
g2.motors.set_steering(steering_out * 4500.0f);
set_steering(steering_out * 4500.0f);
}
bool ModeAcro::requires_velocity() const

2
APMrover2/mode_guided.cpp

@ -80,7 +80,7 @@ void ModeGuided::update() @@ -80,7 +80,7 @@ void ModeGuided::update()
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true, true);
} else {
// we have reached the destination so stay here

2
APMrover2/mode_steering.cpp

@ -28,7 +28,7 @@ void ModeSteering::update() @@ -28,7 +28,7 @@ void ModeSteering::update()
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
set_steering(steering_out * 4500.0f);
} else {
// In steering mode we control lateral acceleration directly.
// For regular steering vehicles we use the maximum lateral acceleration

Loading…
Cancel
Save