From 9cebe3b8808e9560849869f707144a4ee943baf7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Dec 2022 12:34:29 +1100 Subject: [PATCH] AP_GPS: added a common velocity_to_speed_course() this saves flash using common code --- libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp | 3 +-- libraries/AP_GPS/AP_GPS_MAV.cpp | 3 +-- libraries/AP_GPS/AP_GPS_MSP.cpp | 3 +-- libraries/AP_GPS/AP_GPS_NMEA.cpp | 7 +++---- libraries/AP_GPS/AP_GPS_SBF.cpp | 5 +---- libraries/AP_GPS/AP_GPS_SBP.cpp | 5 +---- libraries/AP_GPS/AP_GPS_SBP2.cpp | 5 +---- libraries/AP_GPS/AP_GPS_SITL.cpp | 3 +-- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 3 +-- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 3 +-- libraries/AP_GPS/GPS_Backend.cpp | 9 +++++++++ libraries/AP_GPS/GPS_Backend.h | 5 +++++ 12 files changed, 26 insertions(+), 28 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp index 76a3bf7e89..e19e433055 100644 --- a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp +++ b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp @@ -62,8 +62,7 @@ void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_messag state.velocity.y = pkt.ned_vel_east; state.velocity.z = pkt.ned_vel_down; - state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = state.velocity.xy().length(); + velocity_to_speed_course(state); state.have_speed_accuracy = true; state.have_horizontal_accuracy = true; diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index 41341bd52d..c195d96254 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -82,8 +82,7 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) } state.velocity = vel; - state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); - state.ground_speed = vel.xy().length(); + velocity_to_speed_course(state); } if (have_sa) { diff --git a/libraries/AP_GPS/AP_GPS_MSP.cpp b/libraries/AP_GPS/AP_GPS_MSP.cpp index 1229f3a0d8..32c40c5d70 100644 --- a/libraries/AP_GPS/AP_GPS_MSP.cpp +++ b/libraries/AP_GPS/AP_GPS_MSP.cpp @@ -60,8 +60,7 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt) vel.z = pkt.ned_vel_down * 0.01; state.velocity = vel; - state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = state.velocity.xy().length(); + velocity_to_speed_course(state); state.have_speed_accuracy = true; state.have_horizontal_accuracy = true; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 24b457715e..e5ebe04327 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -399,8 +399,7 @@ bool AP_GPS_NMEA::_term_complete() state.have_vertical_velocity = true; _last_vvelocity_ms = now; // we prefer a true 3D velocity when available - state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = state.velocity.xy().length(); + velocity_to_speed_course(state); _last_3D_velocity_ms = now; } else if (_phd.msg_id == 26) { state.horizontal_accuracy = MAX(_phd.fields[0],_phd.fields[1]) * 0.001; @@ -430,8 +429,7 @@ bool AP_GPS_NMEA::_term_complete() state.have_vertical_velocity = true; _last_vvelocity_ms = now; // we prefer a true 3D velocity when available - state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = state.velocity.xy().length(); + velocity_to_speed_course(state); _last_3D_velocity_ms = now; } if (is_equal(3.0f, float(_ksxt.fields[10]))) { @@ -455,6 +453,7 @@ bool AP_GPS_NMEA::_term_complete() state.location.alt = ag.alt*1.0e2; state.undulation = -ag.undulation; state.velocity = ag.vel_NED; + velocity_to_speed_course(state); state.speed_accuracy = ag.vel_stddev.length(); state.horizontal_accuracy = ag.pos_stddev.xy().length(); state.vertical_accuracy = ag.pos_stddev.z; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 334e847a69..4da9e4e418 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -401,10 +401,7 @@ AP_GPS_SBF::process_message(void) state.have_vertical_velocity = true; - float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1]; - state.ground_speed = (float)safe_sqrt(ground_vector_sq); - - state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0]))); + velocity_to_speed_course(state); state.rtk_age_ms = temp.MeanCorrAge * 10; // value is expressed as twice the rms error = int16 * 0.01/2 diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index 94eea80116..c96262a3e2 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -273,10 +273,7 @@ AP_GPS_SBP::_attempt_state_update() state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3); state.have_vertical_velocity = true; - float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1]; - state.ground_speed = safe_sqrt(ground_vector_sq); - - state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0]))); + velocity_to_speed_course(state); // Update position state diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index 8638e671eb..e13490daea 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp @@ -312,10 +312,7 @@ AP_GPS_SBP2::_attempt_state_update() state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3); state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3); - float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1]; - state.ground_speed = safe_sqrt(ground_vector_sq); - - state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0]))); + velocity_to_speed_course(state); state.speed_accuracy = safe_sqrt( powf((float)last_vel_ned.h_accuracy * 1.0e-3f, 2) + diff --git a/libraries/AP_GPS/AP_GPS_SITL.cpp b/libraries/AP_GPS/AP_GPS_SITL.cpp index fda48fb69a..e671ce05f5 100644 --- a/libraries/AP_GPS/AP_GPS_SITL.cpp +++ b/libraries/AP_GPS/AP_GPS_SITL.cpp @@ -100,8 +100,7 @@ bool AP_GPS_SITL::read(void) state.velocity.y = speedE; state.velocity.z = speedD; - state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = state.velocity.xy().length(); + velocity_to_speed_course(state); state.have_speed_accuracy = true; state.have_horizontal_accuracy = true; diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index ea0c5bea0a..47658c7013 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -377,8 +377,7 @@ void AP_GPS_UAVCAN::handle_velocity(const float vx, const float vy, const float if (!uavcan::isNaN(vx)) { const Vector3f vel(vx, vy, vz); interim_state.velocity = vel; - interim_state.ground_speed = vel.xy().length(); - interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); + velocity_to_speed_course(interim_state); // assume we have vertical velocity if we ever get a non-zero Z velocity if (!isnanf(vel.z) && !is_zero(vel.z)) { interim_state.have_vertical_velocity = true; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 45de386f2e..9747603977 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1587,8 +1587,7 @@ AP_GPS_UBLOX::_parse_gps(void) state.velocity.x = _buffer.velned.ned_north * 0.01f; state.velocity.y = _buffer.velned.ned_east * 0.01f; state.velocity.z = _buffer.velned.ned_down * 0.01f; - state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = state.velocity.xy().length(); + velocity_to_speed_course(state); state.have_speed_accuracy = true; state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; #if UBLOX_FAKE_3DLOCK diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 7bf7e2e899..a581550111 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -133,6 +133,15 @@ void AP_GPS_Backend::fill_3d_velocity(void) state.have_vertical_velocity = false; } +/* + fill in 3D velocity for a GPS that doesn't give vertical velocity numbers + */ +void AP_GPS_Backend::velocity_to_speed_course(AP_GPS::GPS_State &s) +{ + s.ground_course = wrap_360(degrees(atan2f(s.velocity.y, s.velocity.x))); + s.ground_speed = s.velocity.xy().length(); +} + void AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len) { diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 019f0c76b3..ce1fac0d62 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -117,6 +117,11 @@ protected: */ void fill_3d_velocity(void); + /* + fill ground course and speed from velocity + */ + void velocity_to_speed_course(AP_GPS::GPS_State &s); + /* fill in time_week_ms and time_week from BCD date and time components assumes MTK19 millisecond form of bcd_time