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

MAVLink receiver: Fix HIL msg reception

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-12 15:02:49 +02:00
parent 2402f1fbdf
commit e685ef3c95
2 changed files with 18 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+16 -3
View File
@@ -129,6 +129,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_hil_last_frame(0),
_hil_local_proj_inited(0),
_hil_local_alt0(0.0f),
_hil_prev_gyro{},
_hil_prev_accel{},
_hil_local_proj_ref{},
_offboard_control_mode{},
_att_sp{},
@@ -1311,6 +1313,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
}
_hil_last_frame = timestamp;
if (dt < 0.001f) {
dt = 0.001f;
}
/* airspeed */
{
struct airspeed_s airspeed;
@@ -1432,7 +1438,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
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;
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;
hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -1440,12 +1449,15 @@ 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_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;
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?
hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
hil_sensors.accelerometer_timestamp[0] = timestamp;
hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH;
hil_sensors.adc_voltage_v[0] = 0.0f;
hil_sensors.adc_voltage_v[1] = 0.0f;
@@ -1461,6 +1473,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
hil_sensors.magnetometer_timestamp[0] = timestamp;
hil_sensors.magnetometer_priority[0] = ORB_PRIO_HIGH;
hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -199,6 +199,8 @@ private:
uint64_t _hil_last_frame;
bool _hil_local_proj_inited;
float _hil_local_alt0;
float _hil_prev_gyro[3];
float _hil_prev_accel[3];
struct map_projection_reference_s _hil_local_proj_ref;
struct offboard_control_mode_s _offboard_control_mode;
struct vehicle_attitude_setpoint_s _att_sp;