aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: added STEER2SRV_MINSPD

Browse Source 
this is the assumed min speed when in STEERING or AUTO mode
This commit is contained in:
Andrew Tridgell 2013-09-30 09:03:52 +10:00
parent 53f66b23f4
commit 0784c01f19
2 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/APM_Control/AP_SteerController.cpp
View File

@ -66,6 +66,15 @@ const AP_Param::GroupInfo AP_SteerController::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("IMAX", 5, AP_SteerController, _imax, 1500),
// @Param: MINSPD
// @DisplayName: Minimum speed
// @Description: This is the minimum assumed ground speed in meters/second for steering. Having a minimum speed prevents osciallations when the vehicle first starts moving. The vehicle can still driver slower than this limit, but the steering calculations will be done based on this minimum speed.
// @Range: 0 5
// @Increment: 0.1
// @Units: m/s
// @User: User
AP_GROUPINFO("MINSPD", 6, AP_SteerController, _minspeed, 0.3f),
AP_GROUPEND
};
@ -84,9 +93,9 @@ int32_t AP_SteerController::get_steering_out(float desired_accel)
_last_t = tnow;
float speed = _ahrs.groundspeed();
if (speed < 1.0e-6) {
// with no speed all we can do is center the steering
return 0;
if (speed < _minspeed) {
// assume a minimum speed. This stops osciallations when first starting to move
speed = _minspeed;
}
// this is a linear approximation of the inverse steering

7
libraries/APM_Control/AP_SteerController.h
View File

@ -22,6 +22,12 @@ public:
*/
int32_t get_steering_out(float desired_accel);
/*
return the steering radius (half diameter). Assumed to be half
the P value.
*/
float get_turn_radius(void) const { return _K_P * 0.5f; }
void reset_I();
static const struct AP_Param::GroupInfo var_info[];
@ -31,6 +37,7 @@ private:
AP_Float _K_P;
AP_Float _K_I;
AP_Float _K_D;
AP_Float _minspeed;
AP_Int16 _imax;
uint32_t _last_t;
float _last_out;