Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: added lateral acceleration interface

Browse Source 
this will be used by the Rover code for steering
This commit is contained in:
Andrew Tridgell 2013-06-17 09:49:15 +10:00
parent 8b79798e8d
commit 953051b49e
3 changed files with 32 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -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 return the bank angle needed to achieve tracking from the last
update_*() operation update_*() operation
*/ */
int32_t AP_L1_Control::nav_roll_cd(void) int32_t AP_L1_Control::nav_roll_cd(void) const
{ {
float ret; float ret;
ret = degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81 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; 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)); 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); 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; 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()); wp_radius *= sq(_ahrs->get_EAS2TAS());
return min(wp_radius, _L1_dist); return min(wp_radius, _L1_dist);
@ -69,7 +78,7 @@ bool AP_L1_Control::reached_loiter_target(void)
return _WPcircle; return _WPcircle;
} }
float AP_L1_Control::crosstrack_error(void) float AP_L1_Control::crosstrack_error(void) const
{ {
return _crosstrack_error; return _crosstrack_error;
} }

13
libraries/AP_L1_Control/AP_L1_Control.h
View File

@ -31,18 +31,19 @@ public:
/* see AP_Navigation.h for the definitions and units of these /* see AP_Navigation.h for the definitions and units of these
* functions */ * 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) // 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 // 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); int32_t target_bearing_cd(void) const;
float turn_distance(float wp_radius); float turn_distance(float wp_radius) const;
void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP); void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
void update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction); void update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction);
void update_heading_hold(int32_t navigation_heading_cd); void update_heading_hold(int32_t navigation_heading_cd);

16
libraries/AP_Navigation/AP_Navigation.h
View File

@ -19,7 +19,11 @@ class AP_Navigation {
public: public:
// return the desired roll angle in centi-degrees to move towards // return the desired roll angle in centi-degrees to move towards
// the target waypoint // 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 // note: all centi-degree values returned in AP_Navigation should
// be wrapped at -18000 to 18000 in centi-degrees. // 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 // using in centi-degrees. This is used to display an arrow on
// ground stations showing the effect of the cross-tracking in the // ground stations showing the effect of the cross-tracking in the
// controller // 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 // return the difference between the vehicles current course and
// the nav_bearing_cd() in centi-degrees. A positive value means // the nav_bearing_cd() in centi-degrees. A positive value means
// the vehicle is tracking too far to the left of the correct // the vehicle is tracking too far to the left of the correct
// bearing. // 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 // return the target bearing in centi-degrees. This is the bearing
// from the vehicles current position to the target waypoint. This // from the vehicles current position to the target waypoint. This
// should be calculated in the update_*() functions below. // 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 // return the crosstrack error in meters. This is the distance in
// the X-Y plane that we are off the desired track // 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 // return the distance in meters at which a turn should commence
// to allow the vehicle to neatly move to the next track in the // to allow the vehicle to neatly move to the next track in the
// mission when approaching a waypoint // 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 // update the internal state of the navigation controller, given
// the previous and next waypoints. This is the step function for // the previous and next waypoints. This is the step function for