diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index dfbec9fd96..7835654d64 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -206,7 +206,7 @@ void Rover::Log_Write_Steering() time_us : AP_HAL::micros64(), steering_in : channel_steer->get_control_in(), steering_out : g2.motors.get_steering(), - desired_lat_accel : g2.attitude_control.get_desired_lat_accel(), + desired_lat_accel : control_mode->get_desired_lat_accel(), lat_accel : lat_accel, desired_turn_rate : degrees(g2.attitude_control.get_desired_turn_rate()), turn_rate : degrees(ahrs.get_yaw_rate_earth()) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index f4ca00aecd..acc4f1cd16 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -186,6 +186,15 @@ float Mode::crosstrack_error() const return g2.wp_nav.crosstrack_error(); } +// return desired lateral acceleration +float Mode::get_desired_lat_accel() const +{ + if (!is_autopilot_mode()) { + return 0.0f; + } + return g2.wp_nav.get_lat_accel(); +} + // set desired location void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) { diff --git a/APMrover2/mode.h b/APMrover2/mode.h index dda8956ae2..2e6b7a3577 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -92,6 +92,7 @@ public: virtual float wp_bearing() const; virtual float nav_bearing() const; virtual float crosstrack_error() const; + virtual float get_desired_lat_accel() const; // get speed error in m/s, not currently supported float speed_error() const { return 0.0f; } @@ -554,6 +555,13 @@ public: // steering requires velocity but not position bool requires_position() const override { return false; } bool requires_velocity() const override { return true; } + + // return desired lateral acceleration + float get_desired_lat_accel() const override { return _desired_lat_accel; } + +private: + + float _desired_lat_accel; // desired lateral acceleration calculated from pilot steering input }; class ModeInitializing : public Mode diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index a4d4dbf4bb..460a91fad1 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -9,6 +9,7 @@ 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; } @@ -22,6 +23,7 @@ void ModeSteering::update() // pivot turning using turn rate controller // convert pilot steering input to desired turn rate in radians/sec const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate); + _desired_lat_accel = 0.0f; // run steering turn rate controller and throttle controller const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate, @@ -37,15 +39,15 @@ 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 - float desired_lat_accel = max_g_force * (desired_steering / 4500.0f); + _desired_lat_accel = max_g_force * (desired_steering / 4500.0f); // reverse target lateral acceleration if backing up if (reversed) { - desired_lat_accel = -desired_lat_accel; + _desired_lat_accel = -_desired_lat_accel; } // run lateral acceleration to steering controller - calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); + calc_steering_from_lateral_acceleration(_desired_lat_accel, reversed); } // run speed to throttle controller