Browse Source

APM_Control: wrap_180_cd no longer solely returns floats

master
Peter Barker 5 years ago committed by Peter Barker
parent
commit
7fbaea7971
  1. 2
      libraries/APM_Control/AR_AttitudeControl.cpp

2
libraries/APM_Control/AR_AttitudeControl.cpp

@ -601,7 +601,7 @@ bool AR_AttitudeControl::get_forward_speed(float &speed) const
if (!_ahrs.get_velocity_NED(velocity)) { if (!_ahrs.get_velocity_NED(velocity)) {
// use less accurate GPS, assuming entire length is along forward/back axis of vehicle // use less accurate GPS, assuming entire length is along forward/back axis of vehicle
if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
if (fabsf(wrap_180_cd(_ahrs.yaw_sensor - AP::gps().ground_course_cd())) <= 9000) { if (abs(wrap_180_cd(_ahrs.yaw_sensor - AP::gps().ground_course_cd())) <= 9000) {
speed = AP::gps().ground_speed(); speed = AP::gps().ground_speed();
} else { } else {
speed = -AP::gps().ground_speed(); speed = -AP::gps().ground_speed();

Loading…
Cancel
Save