From 2402f1fbdf981d637e584897cac23c96e798b540 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Oct 2015 14:17:48 +0200 Subject: [PATCH] MAVLink receiver: Add missing integrals for gyro and accel topics --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 17 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4d59226586..19fadb49ed 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -126,6 +126,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), + _hil_last_frame(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, @@ -1301,6 +1302,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); + float dt; + + if (_hil_last_frame == 0 || timestamp - _hil_last_frame > 0.1f) { + dt = 0.01f; /* default to 100 Hz */ + } else { + dt = (timestamp - _hil_last_frame) / 1e6f; + } + _hil_last_frame = timestamp; + /* airspeed */ { struct airspeed_s airspeed; @@ -1419,6 +1429,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; 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_timestamp[0] = timestamp; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; @@ -1427,6 +1440,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) 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_sensors.accelerometer_integral_m_s[1] = hil_sensors.accelerometer_m_s2[1] * dt; + hil_sensors.accelerometer_integral_m_s[2] = hil_sensors.accelerometer_m_s2[2] * dt; hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 hil_sensors.accelerometer_timestamp[0] = timestamp; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3f115e8718..7a513216fb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -196,6 +196,7 @@ private: int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; + uint64_t _hil_last_frame; bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref;