aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: use vector.xy().length() instead of norm(x,y)

Browse Source
This commit is contained in:
Josh Henderson 2021-09-11 06:31:17 -04:00 committed by Andrew Tridgell
parent 38ead58df2
commit 06251335da
6 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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

2
libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp
View File

@ -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;

2
libraries/AP_GPS/AP_GPS_MAV.cpp
View File

@ -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) {

2
libraries/AP_GPS/AP_GPS_MSP.cpp
View File

@ -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;

4
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -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 {

2
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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