From 3edf532e90f89f9e58059610b6f3310e7ca1cc0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Nov 2015 17:39:22 +0100 Subject: [PATCH] MAVLink receiver: Fix HIL handling --- src/modules/mavlink/mavlink_receiver.cpp | 58 ++++++++++++------------ 1 file changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d0b95b7f89..5f36354191 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1116,7 +1116,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) /* yaw */ rc.values[2] = man.r / 2 + 1500; /* throttle */ - rc.values[3] = man.z / 1 + 1000; + rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f); /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); @@ -1307,18 +1307,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) float dt; if (_hil_last_frame == 0 || - (imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) || - (_hil_last_frame >= imu.time_usec)) { + (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 = (imu.time_usec - _hil_last_frame) / 1e6f; + dt = (timestamp - _hil_last_frame) / 1e6f; } - _hil_last_frame = imu.time_usec; + _hil_last_frame = timestamp; /* airspeed */ { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + struct airspeed_s airspeed = {}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); // XXX need to fix this @@ -1338,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* gyro */ { - struct gyro_report gyro; - memset(&gyro, 0, sizeof(gyro)); + struct gyro_report gyro = {}; gyro.timestamp = timestamp; gyro.x_raw = imu.xgyro * 1000.0f; @@ -1360,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* accelerometer */ { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); + struct accel_report accel = {}; accel.timestamp = timestamp; accel.x_raw = imu.xacc / mg2ms2; @@ -1382,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* magnetometer */ { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); + struct mag_report mag = {}; mag.timestamp = timestamp; mag.x_raw = imu.xmag * 1000.0f; @@ -1404,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* baro */ { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); + struct baro_report baro = {}; baro.timestamp = timestamp; baro.pressure = imu.abs_pressure; @@ -1422,21 +1422,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* sensor combined */ { - struct sensor_combined_s hil_sensors; - memset(&hil_sensors, 0, sizeof(hil_sensors)); + struct sensor_combined_s hil_sensors = {}; hil_sensors.timestamp = timestamp; hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt; + 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_prev_gyro[0]) / 2.0f; - hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f; - hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f; - memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro)); + hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt; + hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt; + hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt; + memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[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; @@ -1447,10 +1446,10 @@ 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] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * 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_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt; + hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt; + hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt; + memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[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 @@ -1493,12 +1492,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* battery status */ { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + struct battery_status_s hil_battery_status = {}; hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; + hil_battery_status.voltage_v = 11.5f; + hil_battery_status.voltage_filtered_v = 11.5f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f;