Browse Source

ekf2 app: only publish gps in replay if updated

sbg
Roman 9 years ago committed by Lorenz Meier
parent
commit
181a5fd60d
  1. 33
      src/modules/ekf2/ekf2_main.cpp

33
src/modules/ekf2/ekf2_main.cpp

@ -669,19 +669,26 @@ void Ekf2::task_main() @@ -669,19 +669,26 @@ void Ekf2::task_main()
memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], sizeof(replay.accelerometer_integral_m_s));
memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
replay.baro_alt_meter = sensors.baro_alt_meter[0];
replay.time_usec = gps.timestamp_position;
replay.time_usec_vel = gps.timestamp_velocity;
replay.lat = gps.lat;
replay.lon = gps.lon;
replay.alt = gps.alt;
replay.fix_type = gps.fix_type;
replay.eph = gps.eph;
replay.epv = gps.epv;
replay.vel_m_s = gps.vel_m_s;
replay.vel_n_m_s = gps.vel_n_m_s;
replay.vel_e_m_s = gps.vel_e_m_s;
replay.vel_d_m_s = gps.vel_d_m_s;
replay.vel_ned_valid = gps.vel_ned_valid;
// only write gps data if we had a gps update.
if (gps_updated) {
replay.time_usec = gps.timestamp_position;
replay.time_usec_vel = gps.timestamp_velocity;
replay.lat = gps.lat;
replay.lon = gps.lon;
replay.alt = gps.alt;
replay.fix_type = gps.fix_type;
replay.eph = gps.eph;
replay.epv = gps.epv;
replay.vel_m_s = gps.vel_m_s;
replay.vel_n_m_s = gps.vel_n_m_s;
replay.vel_e_m_s = gps.vel_e_m_s;
replay.vel_d_m_s = gps.vel_d_m_s;
replay.vel_ned_valid = gps.vel_ned_valid;
} else {
// this will tell the logging app not to bother logging any gps replay data
replay.time_usec = 0;
}
replay.flow_timestamp = optical_flow.timestamp;
replay.range_to_ground = range_finder.current_distance;

Loading…
Cancel
Save