From 953051b49e8dbb416b6aae2653275d623eb166fb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Jun 2013 09:49:15 +1000 Subject: [PATCH] AP_L1_Control: added lateral acceleration interface this will be used by the Rover code for steering --- libraries/AP_L1_Control/AP_L1_Control.cpp | 21 +++++++++++++++------ libraries/AP_L1_Control/AP_L1_Control.h | 13 +++++++------ libraries/AP_Navigation/AP_Navigation.h | 16 ++++++++++------ 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index e9d96ad489..503cc667c2 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -35,7 +35,7 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] PROGMEM = { return the bank angle needed to achieve tracking from the last update_*() operation */ -int32_t AP_L1_Control::nav_roll_cd(void) +int32_t AP_L1_Control::nav_roll_cd(void) const { float ret; ret = degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81 @@ -43,22 +43,31 @@ int32_t AP_L1_Control::nav_roll_cd(void) return ret; } -int32_t AP_L1_Control::nav_bearing_cd(void) +/* + return the lateral acceleration needed to achieve tracking from the last + update_*() operation + */ +float AP_L1_Control::lateral_acceleration(void) const +{ + return _latAccDem; +} + +int32_t AP_L1_Control::nav_bearing_cd(void) const { return wrap_180_cd(RadiansToCentiDegrees(_nav_bearing)); } -int32_t AP_L1_Control::bearing_error_cd(void) +int32_t AP_L1_Control::bearing_error_cd(void) const { return RadiansToCentiDegrees(_bearing_error); } -int32_t AP_L1_Control::target_bearing_cd(void) +int32_t AP_L1_Control::target_bearing_cd(void) const { return _target_bearing_cd; } -float AP_L1_Control::turn_distance(float wp_radius) +float AP_L1_Control::turn_distance(float wp_radius) const { wp_radius *= sq(_ahrs->get_EAS2TAS()); return min(wp_radius, _L1_dist); @@ -69,7 +78,7 @@ bool AP_L1_Control::reached_loiter_target(void) return _WPcircle; } -float AP_L1_Control::crosstrack_error(void) +float AP_L1_Control::crosstrack_error(void) const { return _crosstrack_error; } diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index 4e45d31b37..10ac7418e9 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -31,18 +31,19 @@ public: /* see AP_Navigation.h for the definitions and units of these * functions */ - int32_t nav_roll_cd(void); + int32_t nav_roll_cd(void) const; + float lateral_acceleration(void) const; // return the desired track heading angle(centi-degrees) - int32_t nav_bearing_cd(void); + int32_t nav_bearing_cd(void) const; // return the heading error angle (centi-degrees) +ve to left of track - int32_t bearing_error_cd(void); + int32_t bearing_error_cd(void) const; - float crosstrack_error(void); + float crosstrack_error(void) const; - int32_t target_bearing_cd(void); - float turn_distance(float wp_radius); + int32_t target_bearing_cd(void) const; + float turn_distance(float wp_radius) const; void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP); void update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction); void update_heading_hold(int32_t navigation_heading_cd); diff --git a/libraries/AP_Navigation/AP_Navigation.h b/libraries/AP_Navigation/AP_Navigation.h index 537834623b..9e0703a116 100644 --- a/libraries/AP_Navigation/AP_Navigation.h +++ b/libraries/AP_Navigation/AP_Navigation.h @@ -19,7 +19,11 @@ class AP_Navigation { public: // return the desired roll angle in centi-degrees to move towards // the target waypoint - virtual int32_t nav_roll_cd(void) = 0; + virtual int32_t nav_roll_cd(void) const = 0; + + // return the desired lateral acceleration in m/s/s to move towards + // the target waypoint + virtual float lateral_acceleration(void) const = 0; // note: all centi-degree values returned in AP_Navigation should // be wrapped at -18000 to 18000 in centi-degrees. @@ -28,27 +32,27 @@ public: // using in centi-degrees. This is used to display an arrow on // ground stations showing the effect of the cross-tracking in the // controller - virtual int32_t nav_bearing_cd(void) = 0; + virtual int32_t nav_bearing_cd(void) const = 0; // return the difference between the vehicles current course and // the nav_bearing_cd() in centi-degrees. A positive value means // the vehicle is tracking too far to the left of the correct // bearing. - virtual int32_t bearing_error_cd(void) = 0; + virtual int32_t bearing_error_cd(void) const = 0; // return the target bearing in centi-degrees. This is the bearing // from the vehicles current position to the target waypoint. This // should be calculated in the update_*() functions below. - virtual int32_t target_bearing_cd(void) = 0; + virtual int32_t target_bearing_cd(void) const = 0; // return the crosstrack error in meters. This is the distance in // the X-Y plane that we are off the desired track - virtual float crosstrack_error(void) = 0; + virtual float crosstrack_error(void) const = 0; // return the distance in meters at which a turn should commence // to allow the vehicle to neatly move to the next track in the // mission when approaching a waypoint - virtual float turn_distance(float wp_radius) = 0; + virtual float turn_distance(float wp_radius) const = 0; // update the internal state of the navigation controller, given // the previous and next waypoints. This is the step function for