diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cda6c48abc..14a906bf88 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 = {}; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7e37d14cbb..5486929795 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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;