Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: added a common velocity_to_speed_course() 

this saves flash using common code
This commit is contained in:
Andrew Tridgell 2022-12-04 12:34:29 +11:00
parent 78dac16520
commit 9cebe3b880
12 changed files with 26 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp
View File

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

3
libraries/AP_GPS/AP_GPS_MAV.cpp
View File

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

3
libraries/AP_GPS/AP_GPS_MSP.cpp
View File

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

7
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

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

5
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

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

5
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

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

5
libraries/AP_GPS/AP_GPS_SBP2.cpp
View File

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

3
libraries/AP_GPS/AP_GPS_SITL.cpp
View File

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

3
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

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

3
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

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

9
libraries/AP_GPS/GPS_Backend.cpp
View File

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

5
libraries/AP_GPS/GPS_Backend.h
View File

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