fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 18:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink receiver: Add missing integrals for gyro and accel topics

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-12 14:17:48 +02:00
parent 09f370940a
commit 2402f1fbdf
2 changed files with 17 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+16
View File
@@ -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;
src/modules/mavlink/mavlink_receiver.h
+1
View File
@@ -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;