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

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.y = pkt.ned_vel_east;
state.velocity.z = pkt.ned_vel_down; state.velocity.z = pkt.ned_vel_down;
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); velocity_to_speed_course(state);
state.ground_speed = state.velocity.xy().length();
state.have_speed_accuracy = true; state.have_speed_accuracy = true;
state.have_horizontal_accuracy = true; state.have_horizontal_accuracy = true;

View File

@ -82,8 +82,7 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
} }
state.velocity = vel; state.velocity = vel;
state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); velocity_to_speed_course(state);
state.ground_speed = vel.xy().length();
} }
if (have_sa) { if (have_sa) {

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; vel.z = pkt.ned_vel_down * 0.01;
state.velocity = vel; state.velocity = vel;
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); velocity_to_speed_course(state);
state.ground_speed = state.velocity.xy().length();
state.have_speed_accuracy = true; state.have_speed_accuracy = true;
state.have_horizontal_accuracy = true; state.have_horizontal_accuracy = true;

View File

@ -399,8 +399,7 @@ bool AP_GPS_NMEA::_term_complete()
state.have_vertical_velocity = true; state.have_vertical_velocity = true;
_last_vvelocity_ms = now; _last_vvelocity_ms = now;
// we prefer a true 3D velocity when available // we prefer a true 3D velocity when available
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); velocity_to_speed_course(state);
state.ground_speed = state.velocity.xy().length();
_last_3D_velocity_ms = now; _last_3D_velocity_ms = now;
} else if (_phd.msg_id == 26) { } else if (_phd.msg_id == 26) {
state.horizontal_accuracy = MAX(_phd.fields[0],_phd.fields[1]) * 0.001; 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; state.have_vertical_velocity = true;
_last_vvelocity_ms = now; _last_vvelocity_ms = now;
// we prefer a true 3D velocity when available // we prefer a true 3D velocity when available
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); velocity_to_speed_course(state);
state.ground_speed = state.velocity.xy().length();
_last_3D_velocity_ms = now; _last_3D_velocity_ms = now;
} }
if (is_equal(3.0f, float(_ksxt.fields[10]))) { 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.location.alt = ag.alt*1.0e2;
state.undulation = -ag.undulation; state.undulation = -ag.undulation;
state.velocity = ag.vel_NED; state.velocity = ag.vel_NED;
velocity_to_speed_course(state);
state.speed_accuracy = ag.vel_stddev.length(); state.speed_accuracy = ag.vel_stddev.length();
state.horizontal_accuracy = ag.pos_stddev.xy().length(); state.horizontal_accuracy = ag.pos_stddev.xy().length();
state.vertical_accuracy = ag.pos_stddev.z; state.vertical_accuracy = ag.pos_stddev.z;

View File

@ -401,10 +401,7 @@ AP_GPS_SBF::process_message(void)
state.have_vertical_velocity = true; state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1]; velocity_to_speed_course(state);
state.ground_speed = (float)safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.rtk_age_ms = temp.MeanCorrAge * 10; state.rtk_age_ms = temp.MeanCorrAge * 10;
// value is expressed as twice the rms error = int16 * 0.01/2 // value is expressed as twice the rms error = int16 * 0.01/2

View File

@ -273,10 +273,7 @@ AP_GPS_SBP::_attempt_state_update()
state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3); state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
state.have_vertical_velocity = true; state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1]; velocity_to_speed_course(state);
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
// Update position state // Update position state

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[1] = (float)(last_vel_ned.e * 1.0e-3);
state.velocity[2] = (float)(last_vel_ned.d * 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]; velocity_to_speed_course(state);
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.speed_accuracy = safe_sqrt( state.speed_accuracy = safe_sqrt(
powf((float)last_vel_ned.h_accuracy * 1.0e-3f, 2) + powf((float)last_vel_ned.h_accuracy * 1.0e-3f, 2) +

View File

@ -100,8 +100,7 @@ bool AP_GPS_SITL::read(void)
state.velocity.y = speedE; state.velocity.y = speedE;
state.velocity.z = speedD; state.velocity.z = speedD;
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); velocity_to_speed_course(state);
state.ground_speed = state.velocity.xy().length();
state.have_speed_accuracy = true; state.have_speed_accuracy = true;
state.have_horizontal_accuracy = true; state.have_horizontal_accuracy = true;

View File

@ -377,8 +377,7 @@ void AP_GPS_UAVCAN::handle_velocity(const float vx, const float vy, const float
if (!uavcan::isNaN(vx)) { if (!uavcan::isNaN(vx)) {
const Vector3f vel(vx, vy, vz); const Vector3f vel(vx, vy, vz);
interim_state.velocity = vel; interim_state.velocity = vel;
interim_state.ground_speed = vel.xy().length(); velocity_to_speed_course(interim_state);
interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
// assume we have vertical velocity if we ever get a non-zero Z velocity // assume we have vertical velocity if we ever get a non-zero Z velocity
if (!isnanf(vel.z) && !is_zero(vel.z)) { if (!isnanf(vel.z) && !is_zero(vel.z)) {
interim_state.have_vertical_velocity = true; interim_state.have_vertical_velocity = true;

View File

@ -1587,8 +1587,7 @@ AP_GPS_UBLOX::_parse_gps(void)
state.velocity.x = _buffer.velned.ned_north * 0.01f; state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f; state.velocity.y = _buffer.velned.ned_east * 0.01f;
state.velocity.z = _buffer.velned.ned_down * 0.01f; state.velocity.z = _buffer.velned.ned_down * 0.01f;
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); velocity_to_speed_course(state);
state.ground_speed = state.velocity.xy().length();
state.have_speed_accuracy = true; state.have_speed_accuracy = true;
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
#if UBLOX_FAKE_3DLOCK #if UBLOX_FAKE_3DLOCK

View File

@ -133,6 +133,15 @@ void AP_GPS_Backend::fill_3d_velocity(void)
state.have_vertical_velocity = false; 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 void
AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len) AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
{ {

View File

@ -117,6 +117,11 @@ protected:
*/ */
void fill_3d_velocity(void); 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 fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time assumes MTK19 millisecond form of bcd_time