diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4a2fa6cd73..d787feb62e 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -224,3 +224,15 @@ Vector2f AP_AHRS::groundspeed_vector(void) } return Vector2f(0.0f, 0.0f); } + +/* + get position projected by groundspeed and heading + */ +bool AP_AHRS::get_projected_position(struct Location *loc) +{ + if (!get_position(loc)) { + return false; + } + location_update(loc, degrees(yaw), _gps->ground_speed * 0.01 * _gps->get_lag()); + return true; +} diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index c523370265..2e8c4c5818 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -127,6 +127,10 @@ public: return true; } + // get our projected position, based on our GPS position plus + // heading and ground speed + bool get_projected_position(struct Location *loc); + // return a wind estimation vector, in m/s virtual Vector3f wind_estimate(void) { return Vector3f(0,0,0);