diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 1dfbf23ccf..cdc6d258fe 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -18,7 +18,7 @@ void Mode::exit() // call sub-classes exit _exit(); - lateral_acceleration = 0.0f; + _desired_lat_accel = 0.0f; } // these are basically the same checks as in AP_Arming: @@ -285,7 +285,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 = lateral_acceleration; + const float target_lateral_accel_G = _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) @@ -328,7 +328,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); - lateral_acceleration = rover.nav_controller->lateral_acceleration(); + _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 { @@ -336,9 +336,9 @@ 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) { - lateral_acceleration = g.turn_max_g * GRAVITY_MSS; + _desired_lat_accel = g.turn_max_g * GRAVITY_MSS; } else { - lateral_acceleration = -g.turn_max_g * GRAVITY_MSS; + _desired_lat_accel = -g.turn_max_g * GRAVITY_MSS; } } @@ -353,13 +353,13 @@ void Mode::calc_steering_from_lateral_acceleration(bool reversed) { // add obstacle avoidance response to lateral acceleration target if (!reversed) { - lateral_acceleration += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g; + _desired_lat_accel += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g; } // constrain to max G force - lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); + _desired_lat_accel = constrain_float(_desired_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(lateral_acceleration, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); + 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); g2.motors.set_steering(steering_out * 4500.0f); } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 842dbd3ff8..9818efe446 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -95,10 +95,6 @@ public: // rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication) void set_desired_speed_to_default(bool rtl = false); - // Navigation control variables - // The instantaneous desired lateral acceleration in m/s/s - float lateral_acceleration; - protected: // subclasses override this to perform checks before entering the mode @@ -153,6 +149,7 @@ 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 0d2dba32f5..e25339987b 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -60,7 +60,7 @@ void ModeAuto::update() } else { // we have reached the destination so stop stop_vehicle(); - lateral_acceleration = 0.0f; + _desired_lat_accel = 0.0f; } break; } diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index a6a37b2746..0ca2cb596d 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -7,7 +7,7 @@ bool ModeGuided::_enter() set_desired_speed_to_default(); // when entering guided mode we set the target as the current location. - lateral_acceleration = 0.0f; + _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 9644082c06..01b5b61b98 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -37,6 +37,6 @@ void ModeRTL::update() } else { // we've reached destination so stop stop_vehicle(); - lateral_acceleration = 0.0f; + _desired_lat_accel = 0.0f; } } diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index b8b3b07255..e9ac83c6c9 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -15,7 +15,7 @@ void ModeSteering::update() // no valid speed so stop g2.motors.set_throttle(0.0f); g2.motors.set_steering(0.0f); - lateral_acceleration = 0.0f; + _desired_lat_accel = 0.0f; return; } @@ -36,13 +36,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 - lateral_acceleration = max_g_force * (desired_steering / 4500.0f); + _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; - lateral_acceleration = -lateral_acceleration; + _desired_lat_accel = -_desired_lat_accel; } // mark us as in_reverse when using a negative throttle