fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:58:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improve HIL data input

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-13 10:07:05 +02:00
parent e685ef3c95
commit 96d90c0fe6
1 changed files with 15 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+15 -17
View File
@@ -1306,16 +1306,14 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
float dt;
if (_hil_last_frame == 0 || timestamp - _hil_last_frame > 0.1f) {
if (_hil_last_frame == 0 ||
(imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) ||
(_hil_last_frame >= imu.time_usec)) {
dt = 0.01f; /* default to 100 Hz */
} else {
dt = (timestamp - _hil_last_frame) / 1e6f;
}
_hil_last_frame = timestamp;
if (dt < 0.001f) {
dt = 0.001f;
dt = (imu.time_usec - _hil_last_frame) / 1e6f;
}
_hil_last_frame = imu.time_usec;
/* airspeed */
{
@@ -1432,12 +1430,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
hil_sensors.gyro_integral_rad[0] = hil_sensors.gyro_rad_s[0] * dt;
hil_sensors.gyro_integral_rad[1] = hil_sensors.gyro_rad_s[1] * dt;
hil_sensors.gyro_integral_rad[2] = hil_sensors.gyro_rad_s[2] * dt;
hil_sensors.gyro_integral_rad[0] = (hil_sensors.gyro_rad_s[0] * dt + _hil_prev_gyro[0]) / 2.0f;
hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f;
hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f;
memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro));
hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
hil_sensors.gyro_timestamp[0] = timestamp;
@@ -1446,12 +1444,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_integral_m_s[0] = (hil_sensors.accelerometer_m_s2[0] * dt + _hil_prev_accel[0]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[1] = (hil_sensors.accelerometer_m_s2[1] * dt + _hil_prev_accel[1]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[2] = (hil_sensors.accelerometer_m_s2[2] * dt + _hil_prev_accel[2]) / 2.0f;
hil_sensors.accelerometer_m_s2[0] = ((imu.xacc * dt + _hil_prev_accel[0]) / 2.0f) / dt;
hil_sensors.accelerometer_m_s2[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt;
hil_sensors.accelerometer_m_s2[2] = (((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f) / dt - 9.80665f;
hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[2] = ((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f;
memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?