Browse Source

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

gps-1.3.1
Josh Henderson 3 years ago committed by Andrew Tridgell
parent
commit
06251335da
  1. 2
      libraries/AP_GPS/AP_GPS.cpp
  2. 2
      libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp
  3. 2
      libraries/AP_GPS/AP_GPS_MAV.cpp
  4. 2
      libraries/AP_GPS/AP_GPS_MSP.cpp
  5. 4
      libraries/AP_GPS/AP_GPS_UAVCAN.cpp
  6. 2
      libraries/AP_GPS/AP_GPS_UBLOX.cpp

2
libraries/AP_GPS/AP_GPS.cpp

@ -1839,7 +1839,7 @@ void AP_GPS::calc_blended_state(void) @@ -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

@ -68,7 +68,7 @@ void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_messag @@ -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

@ -86,7 +86,7 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) @@ -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

@ -66,7 +66,7 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt) @@ -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

@ -394,7 +394,7 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb) @@ -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) @@ -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

@ -1499,7 +1499,7 @@ AP_GPS_UBLOX::_parse_gps(void) @@ -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

Loading…
Cancel
Save