diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 26a3538efa..62487f9a69 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1839,7 +1839,7 @@ void AP_GPS::calc_blended_state(void) state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm; // Calculate ground speed and course from blended velocity vector - state[GPS_BLENDED_INSTANCE].ground_speed = norm(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.y); + state[GPS_BLENDED_INSTANCE].ground_speed = state[GPS_BLENDED_INSTANCE].velocity.xy().length(); state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x))); // If the GPS week is the same then use a blended time_week_ms diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp index 928eb1942c..bd9c910c79 100644 --- a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp +++ b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp @@ -68,7 +68,7 @@ void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_messag state.velocity.z = pkt.ned_vel_down; state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = norm(state.velocity.y, state.velocity.x); + state.ground_speed = state.velocity.xy().length(); 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 2e49e81be6..62cc13da76 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -86,7 +86,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 = norm(vel.x, vel.y); + state.ground_speed = vel.xy().length(); } if (have_sa) { diff --git a/libraries/AP_GPS/AP_GPS_MSP.cpp b/libraries/AP_GPS/AP_GPS_MSP.cpp index 16ce23a5de..c3a807fc0c 100644 --- a/libraries/AP_GPS/AP_GPS_MSP.cpp +++ b/libraries/AP_GPS/AP_GPS_MSP.cpp @@ -66,7 +66,7 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt) state.velocity = vel; state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); - state.ground_speed = norm(state.velocity.y, state.velocity.x); + state.ground_speed = state.velocity.xy().length(); 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 e7c4fcdd95..a04771cf5d 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -394,7 +394,7 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb) if (!uavcan::isNaN(cb.msg->ned_velocity[0])) { Vector3f vel(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]); interim_state.velocity = vel; - interim_state.ground_speed = norm(vel.x, vel.y); + interim_state.ground_speed = vel.xy().length(); interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); interim_state.have_vertical_velocity = true; } else { @@ -518,7 +518,7 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) if (!uavcan::isNaN(cb.msg->ned_velocity[0])) { Vector3f vel(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]); interim_state.velocity = vel; - interim_state.ground_speed = norm(vel.x, vel.y); + interim_state.ground_speed = vel.xy().length(); interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); interim_state.have_vertical_velocity = true; } else { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 420c65fdcb..6845696c09 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1499,7 +1499,7 @@ AP_GPS_UBLOX::_parse_gps(void) 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 = norm(state.velocity.y, state.velocity.x); + state.ground_speed = state.velocity.xy().length(); state.have_speed_accuracy = true; state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; #if UBLOX_FAKE_3DLOCK