Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Gazebo fix implicit cast

Browse Source
This commit is contained in:
Pierre Kancir 2016-12-19 11:49:56 +01:00 committed by Andrew Tridgell
parent ee7355a301
commit d612209c19
2 changed files with 20 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/SITL/SIM_Gazebo.cpp
View File

@ -36,7 +36,6 @@ Gazebo::Gazebo(const char *home_str, const char *frame_str) :
// Gazebo keeps sending us packets. Not strictly necessary but // Gazebo keeps sending us packets. Not strictly necessary but
// useful for debugging // useful for debugging
fprintf(stdout, "Starting SITL Gazebo\n"); fprintf(stdout, "Starting SITL Gazebo\n");
} }
/* /*
@ -90,37 +89,36 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
} }
// get imu stuff // get imu stuff
accel_body = Vector3f(pkt.imu_linear_acceleration_xyz[0], accel_body = Vector3f(static_cast<float>(pkt.imu_linear_acceleration_xyz[0]),
pkt.imu_linear_acceleration_xyz[1], static_cast<float>(pkt.imu_linear_acceleration_xyz[1]),
pkt.imu_linear_acceleration_xyz[2]); static_cast<float>(pkt.imu_linear_acceleration_xyz[2]));
gyro = Vector3f(pkt.imu_angular_velocity_rpy[0], gyro = Vector3f(static_cast<float>(pkt.imu_angular_velocity_rpy[0]),
pkt.imu_angular_velocity_rpy[1], static_cast<float>(pkt.imu_angular_velocity_rpy[1]),
pkt.imu_angular_velocity_rpy[2]); static_cast<float>(pkt.imu_angular_velocity_rpy[2]));
// compute dcm from imu orientation // compute dcm from imu orientation
Quaternion quat(pkt.imu_orientation_quat[0], Quaternion quat(static_cast<float>(pkt.imu_orientation_quat[0]),
pkt.imu_orientation_quat[1], static_cast<float>(pkt.imu_orientation_quat[1]),
pkt.imu_orientation_quat[2], static_cast<float>(pkt.imu_orientation_quat[2]),
pkt.imu_orientation_quat[3]); static_cast<float>(pkt.imu_orientation_quat[3]));
quat.rotation_matrix(dcm); quat.rotation_matrix(dcm);
double speedN = pkt.velocity_xyz[0]; velocity_ef = Vector3f(static_cast<float>(pkt.velocity_xyz[0]),
double speedE = pkt.velocity_xyz[1]; static_cast<float>(pkt.velocity_xyz[1]),
double speedD = pkt.velocity_xyz[2]; static_cast<float>(pkt.velocity_xyz[2]));
velocity_ef = Vector3f(speedN, speedE, speedD);
position = Vector3f(pkt.position_xyz[0], position = Vector3f(static_cast<float>(pkt.position_xyz[0]),
pkt.position_xyz[1], static_cast<float>(pkt.position_xyz[1]),
pkt.position_xyz[2]); static_cast<float>(pkt.position_xyz[2]));
// auto-adjust to simulation frame rate // auto-adjust to simulation frame rate
double deltat = pkt.timestamp - last_timestamp; double deltat = pkt.timestamp - last_timestamp;
time_now_us += deltat * 1.0e6; time_now_us += static_cast<uint64_t>(deltat * 1.0e6);
if (deltat < 0.01 && deltat > 0) { if (deltat < 0.01 && deltat > 0) {
adjust_frame_time(1.0/deltat); adjust_frame_time(static_cast<float>(1.0/deltat));
} }
last_timestamp = pkt.timestamp; last_timestamp = pkt.timestamp;
@ -170,4 +168,4 @@ void Gazebo::update(const struct sitl_input &input)
drain_sockets(); drain_sockets();
} }
} // namespace SITL } // namespace SITL

2
libraries/SITL/SIM_Gazebo.h
View File

@ -73,4 +73,4 @@ private:
int _gazebo_port = 9002; int _gazebo_port = 9002;
}; };
} // namespace SITL } // namespace SITL