Browse Source

SITL: AirSim: Store last timestamp only

Also change the datatype of deltat
master
Rajat Singhal 6 years ago committed by Andrew Tridgell
parent
commit
80bbadf9ca
  1. 6
      libraries/SITL/SIM_AirSim.cpp
  2. 3
      libraries/SITL/SIM_AirSim.h

6
libraries/SITL/SIM_AirSim.cpp

@ -262,8 +262,8 @@ void AirSim::recv_fdm() @@ -262,8 +262,8 @@ void AirSim::recv_fdm()
dcm.from_euler(state.pose.roll, state.pose.pitch, state.pose.yaw);
if (last_state.timestamp) {
double deltat = state.timestamp - last_state.timestamp;
if (last_timestamp) {
int deltat = state.timestamp - last_timestamp;
time_now_us += deltat;
if (deltat > 0 && deltat < 100000) {
@ -312,7 +312,7 @@ void AirSim::recv_fdm() @@ -312,7 +312,7 @@ void AirSim::recv_fdm()
velocity_ef.z);
#endif
last_state = state;
last_timestamp = state.timestamp;
}
/*

3
libraries/SITL/SIM_AirSim.h

@ -66,6 +66,7 @@ private: @@ -66,6 +66,7 @@ private:
double average_frame_time;
uint64_t frame_counter;
uint64_t last_frame_count;
uint64_t last_timestamp;
void send_servos(const struct sitl_input &input);
void recv_fdm();
@ -107,7 +108,7 @@ private: @@ -107,7 +108,7 @@ private:
struct {
struct float_array rc_channels;
} rc;
} state, last_state;
} state;
// table to aid parsing of JSON sensor data
struct keytable {

Loading…
Cancel
Save