Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: Adding support for rover's operating in reverse

Browse Source 
Mostly invovles sorting out the reverse yaw when travelling backwards.
This commit is contained in:
Grant Morphett 2016-07-14 17:44:52 +10:00 committed by Andrew Tridgell
parent 4e8666cee9
commit b56799f444
2 changed files with 33 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -41,6 +41,28 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = {
//Modified to provide explicit control over capture angle //Modified to provide explicit control over capture angle
/*
Wrap AHRS yaw if in reverse - radians
*/
float AP_L1_Control::get_yaw()
{
if (_reverse) {
return wrap_PI(M_PI + _ahrs.yaw);
}
return _ahrs.yaw;
}
/*
Wrap AHRS yaw sensor if in reverse - centi-degress
*/
float AP_L1_Control::get_yaw_sensor()
{
if (_reverse) {
return wrap_180_cd(18000 + _ahrs.yaw_sensor);
}
return _ahrs.yaw_sensor;
}
/* /*
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
@ -119,7 +141,7 @@ void AP_L1_Control::_prevent_indecision(float &Nu)
const float Nu_limit = 0.9f*M_PI; const float Nu_limit = 0.9f*M_PI;
if (fabsf(Nu) > Nu_limit && if (fabsf(Nu) > Nu_limit &&
fabsf(_last_Nu) > Nu_limit && fabsf(_last_Nu) > Nu_limit &&
labs(wrap_180_cd(_target_bearing_cd - _ahrs.yaw_sensor)) > 12000 && labs(wrap_180_cd(_target_bearing_cd - get_yaw_sensor())) > 12000 &&
Nu * _last_Nu < 0.0f) { Nu * _last_Nu < 0.0f) {
// we are moving away from the target waypoint and pointing // we are moving away from the target waypoint and pointing
// away from the waypoint (not flying backwards). The sign // away from the waypoint (not flying backwards). The sign
@ -166,7 +188,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
// use a small ground speed vector in the right direction, // use a small ground speed vector in the right direction,
// allowing us to use the compass heading at zero GPS velocity // allowing us to use the compass heading at zero GPS velocity
groundSpeed = 0.1f; groundSpeed = 0.1f;
_groundspeed_vector = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)) * groundSpeed; _groundspeed_vector = Vector2f(cosf(get_yaw()), sinf(get_yaw())) * groundSpeed;
} }
// Calculate time varying control parameters // Calculate time varying control parameters
@ -183,7 +205,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
if (AB.length() < 1.0e-6f) { if (AB.length() < 1.0e-6f) {
AB = location_diff(_current_loc, next_WP); AB = location_diff(_current_loc, next_WP);
if (AB.length() < 1.0e-6f) { if (AB.length() < 1.0e-6f) {
AB = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)); AB = Vector2f(cosf(get_yaw()), sinf(get_yaw()));
} }
} }
AB.normalize(); AB.normalize();

8
libraries/AP_L1_Control/AP_L1_Control.h
View File

@ -65,6 +65,10 @@ public:
// this supports the NAVl1_* user settable parameters // this supports the NAVl1_* user settable parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
void set_reverse(bool reverse) {
_reverse = reverse;
}
private: private:
// reference to the AHRS object // reference to the AHRS object
AP_AHRS &_ahrs; AP_AHRS &_ahrs;
@ -109,4 +113,8 @@ private:
float _L1_xtrack_i_gain_prev = 0; float _L1_xtrack_i_gain_prev = 0;
uint32_t _last_update_waypoint_us; uint32_t _last_update_waypoint_us;
bool _data_is_stale = true; bool _data_is_stale = true;
bool _reverse = false;
float get_yaw();
float get_yaw_sensor();
}; };