aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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
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;
}

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
* 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 &center_WP, float radius, int8_t loiter_direction);
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:
// 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