diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index d5c3bc5aca..0d7a275d2c 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -17,8 +17,6 @@ void Mode::exit() { // call sub-classes exit _exit(); - - _desired_lat_accel = 0.0f; } // these are basically the same checks as in AP_Arming: @@ -286,7 +284,7 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed) { // this method makes use the following internal variables const float yaw_error_cd = _yaw_error_cd; - const float target_lateral_accel_G = _desired_lat_accel; + const float target_lateral_accel_G = attitude_control.get_desired_lat_accel(); const float distance_to_waypoint = _distance_to_destination; // calculate the yaw_error_ratio which is the error (capped at 90degrees) expressed as a ratio (from 0 ~ 1) @@ -321,7 +319,7 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed) } // calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination -// this function update lateral_acceleration and _yaw_error_cd members +// this function updates the _yaw_error_cd value void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed) { // Calculate the required turn of the wheels @@ -329,7 +327,7 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct // positive error = right turn rover.nav_controller->set_reverse(reversed); rover.nav_controller->update_waypoint(origin, destination); - _desired_lat_accel = rover.nav_controller->lateral_acceleration(); + float desired_lat_accel = rover.nav_controller->lateral_acceleration(); if (reversed) { _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor + 18000); } else { @@ -337,31 +335,31 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct } if (rover.use_pivot_steering(_yaw_error_cd)) { if (_yaw_error_cd >= 0.0f) { - _desired_lat_accel = g.turn_max_g * GRAVITY_MSS; + desired_lat_accel = g.turn_max_g * GRAVITY_MSS; } else { - _desired_lat_accel = -g.turn_max_g * GRAVITY_MSS; + desired_lat_accel = -g.turn_max_g * GRAVITY_MSS; } } // call lateral acceleration to steering controller - calc_steering_from_lateral_acceleration(reversed); + calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); } /* calculate steering output given lateral_acceleration */ -void Mode::calc_steering_from_lateral_acceleration(bool reversed) +void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reversed) { // add obstacle avoidance response to lateral acceleration target if (!reversed) { - _desired_lat_accel += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g; + lat_accel += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g; } // constrain to max G force - _desired_lat_accel = constrain_float(_desired_lat_accel, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); + lat_accel = constrain_float(lat_accel, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); // send final steering command to motor library - float steering_out = attitude_control.get_steering_out_lat_accel(_desired_lat_accel, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); + const float steering_out = attitude_control.get_steering_out_lat_accel(lat_accel, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); g2.motors.set_steering(steering_out * 4500.0f); } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 838a6279a2..80ef0a7997 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -111,7 +111,7 @@ protected: void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false); // calculate steering angle given a desired lateral acceleration - void calc_steering_from_lateral_acceleration(bool reversed = false); + void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false); // calculate steering output to drive towards desired heading void calc_steering_to_heading(float desired_heading_cd, bool reversed = false); @@ -152,7 +152,6 @@ protected: Location _destination; // destination Location when in Guided_WP float _distance_to_destination; // distance from vehicle to final destination in meters bool _reached_destination; // true once the vehicle has reached the destination - float _desired_lat_accel; // desired lateral acceleration in m/s/s float _desired_yaw_cd; // desired yaw in centi-degrees float _yaw_error_cd; // error between desired yaw and actual yaw in centi-degrees float _desired_speed; // desired speed in m/s diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 3d7f12f735..c09885c974 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -60,7 +60,6 @@ void ModeAuto::update() } else { // we have reached the destination so stop stop_vehicle(); - _desired_lat_accel = 0.0f; } break; } diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index ee071d31f7..999b1c0bb0 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -7,7 +7,6 @@ bool ModeGuided::_enter() set_desired_speed_to_default(); // when entering guided mode we set the target as the current location. - _desired_lat_accel = 0.0f; set_desired_location(rover.current_loc); // guided mode never travels in reverse diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index 01b5b61b98..7727e87ebf 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -37,6 +37,5 @@ void ModeRTL::update() } else { // we've reached destination so stop stop_vehicle(); - _desired_lat_accel = 0.0f; } } diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index e9ac83c6c9..c277b68e81 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -15,7 +15,6 @@ void ModeSteering::update() // no valid speed so stop g2.motors.set_throttle(0.0f); g2.motors.set_steering(0.0f); - _desired_lat_accel = 0.0f; return; } @@ -36,13 +35,13 @@ void ModeSteering::update() max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS); // convert pilot steering input to desired lateral acceleration - _desired_lat_accel = max_g_force * (desired_steering / 4500.0f); + float desired_lat_accel = max_g_force * (desired_steering / 4500.0f); // reverse target lateral acceleration if backing up bool reversed = false; if (is_negative(target_speed)) { reversed = true; - _desired_lat_accel = -_desired_lat_accel; + desired_lat_accel = -desired_lat_accel; } // mark us as in_reverse when using a negative throttle @@ -53,7 +52,7 @@ void ModeSteering::update() stop_vehicle(); } else { // run lateral acceleration to steering controller - calc_steering_from_lateral_acceleration(false); + calc_steering_from_lateral_acceleration(desired_lat_accel, false); calc_throttle(target_speed, false); } }