Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mavlink_receiver: don't publish sensor_combined for MAVLINK_MSG_ID_HIL_SENSOR

Browse Source 
In hil mode, sensors is responsible for publishing this topic.
This commit is contained in:
Beat Küng
2017-03-03 17:41:49 +01:00
committed by Lorenz Meier
parent 8957b473a8
commit 2bbe04c3d6
2 changed files with 0 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
-61
View File
@@ -137,11 +137,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_global_ref_timestamp(0),
_hil_frames(0),
_old_timestamp(0),
_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{},
@@ -1776,24 +1773,6 @@ 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) ||
(timestamp - _hil_last_frame) > (0.1f * 1e6f) ||
(_hil_last_frame >= timestamp)) {
dt = 0.01f; /* default to 100 Hz */
memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro));
_hil_prev_accel[0] = 0.0f;
_hil_prev_accel[1] = 0.0f;
_hil_prev_accel[2] = 9.866f;
} else {
dt = (timestamp - _hil_last_frame) / 1e6f;
}
_hil_last_frame = timestamp;
/* airspeed */
{
struct airspeed_s airspeed = {};
@@ -1896,46 +1875,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
}
}
/* sensor combined */
{
struct sensor_combined_s hil_sensors = {};
hil_sensors.gyro_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]);
hil_sensors.gyro_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]);
hil_sensors.gyro_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]);
_hil_prev_gyro[0] = imu.xgyro;
_hil_prev_gyro[1] = imu.ygyro;
_hil_prev_gyro[2] = imu.zgyro;
hil_sensors.gyro_integral_dt = dt;
hil_sensors.timestamp = timestamp;
hil_sensors.accelerometer_m_s2[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]);
hil_sensors.accelerometer_m_s2[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]);
hil_sensors.accelerometer_m_s2[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]);
_hil_prev_accel[0] = imu.xacc;
_hil_prev_accel[1] = imu.yacc;
_hil_prev_accel[2] = imu.zacc;
hil_sensors.accelerometer_integral_dt = dt;
hil_sensors.accelerometer_timestamp_relative = 0;
hil_sensors.magnetometer_ga[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag;
hil_sensors.magnetometer_timestamp_relative = 0;
hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.baro_timestamp_relative = 0;
/* publish combined sensor topic */
if (_sensors_pub == nullptr) {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
} else {
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
}
}
/* battery status */
{
struct battery_status_s hil_battery_status = {};
src/modules/mavlink/mavlink_receiver.h
-3
View File
@@ -237,11 +237,8 @@ private:
uint64_t _global_ref_timestamp;
int _hil_frames;
uint64_t _old_timestamp;
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;