Browse Source

SITL_AirSim: moved NED position initialization

out of always false if -statement
c415-sdk
dmitry 4 years ago committed by Andrew Tridgell
parent
commit
94d2f4ea0e
  1. 3
      libraries/SITL/SIM_AirSim.cpp

3
libraries/SITL/SIM_AirSim.cpp

@ -307,6 +307,8 @@ void AirSim::recv_fdm(const sitl_input& input)
location.lng = state.gps.lon * 1.0e7; location.lng = state.gps.lon * 1.0e7;
location.alt = state.gps.alt * 100.0f; location.alt = state.gps.alt * 100.0f;
position = home.get_distance_NED(location);
dcm.from_euler(state.pose.roll, state.pose.pitch, state.pose.yaw); dcm.from_euler(state.pose.roll, state.pose.pitch, state.pose.yaw);
if (last_timestamp) { if (last_timestamp) {
@ -358,7 +360,6 @@ void AirSim::recv_fdm(const sitl_input& input)
degrees(gyro.z)); degrees(gyro.z));
Vector3f velocity_bf = dcm.transposed() * velocity_ef; Vector3f velocity_bf = dcm.transposed() * velocity_ef;
position = home.get_distance_NED(location);
// @LoggerMessage: ASM2 // @LoggerMessage: ASM2
// @Description: More AirSim simulation data // @Description: More AirSim simulation data

Loading…
Cancel
Save