diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.c')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.c | 122 |
1 files changed, 2 insertions, 120 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c index e62e4dcc4..a42612f9e 100644 --- a/src/modules/mavlink/mavlink_receiver.c +++ b/src/modules/mavlink/mavlink_receiver.c @@ -308,82 +308,6 @@ handle_message(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); - /* TODO, set ground_press/ temp during calib */ - static const float ground_press = 1013.25f; // mbar - static const float ground_tempC = 21.0f; - static const float ground_alt = 0.0f; - static const float T0 = 273.15f; - static const float R = 287.05f; - static const float g = 9.806f; - - if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { - - mavlink_raw_imu_t imu; - mavlink_msg_raw_imu_decode(msg, &imu); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* sensors general */ - hil_sensors.timestamp = imu.time_usec; - - /* hil gyro */ - static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; - hil_sensors.gyro_raw[0] = imu.xgyro; - hil_sensors.gyro_raw[1] = imu.ygyro; - hil_sensors.gyro_raw[2] = imu.zgyro; - hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad; - hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad; - hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad; - - /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; - static const float mg2ms2 = 9.8f / 1000.0f; - hil_sensors.accelerometer_raw[0] = imu.xacc; - hil_sensors.accelerometer_raw[1] = imu.yacc; - hil_sensors.accelerometer_raw[2] = imu.zacc; - hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc; - hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc; - hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - - /* adc */ - hil_sensors.adc_voltage_v[0] = 0; - hil_sensors.adc_voltage_v[1] = 0; - hil_sensors.adc_voltage_v[2] = 0; - - /* magnetometer */ - float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.magnetometer_raw[0] = imu.xmag; - hil_sensors.magnetometer_raw[1] = imu.ymag; - hil_sensors.magnetometer_raw[2] = imu.zmag; - hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga; - hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga; - hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - - /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil imu at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { mavlink_highres_imu_t imu; @@ -437,13 +361,9 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; - - float tempC = imu.temperature; - float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; - float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure); - - hil_sensors.baro_alt_meter = h; + hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; hil_sensors.gyro_counter = hil_counter; @@ -516,44 +436,6 @@ handle_message(mavlink_message_t *msg) } } - if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) { - - mavlink_raw_pressure_t press; - mavlink_msg_raw_pressure_decode(msg, &press); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* sensors general */ - hil_sensors.timestamp = press.time_usec; - - /* baro */ - - float tempC = press.temperature / 100.0f; - float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; - float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs); - hil_sensors.baro_counter = hil_counter; - hil_sensors.baro_pres_mbar = press.press_abs; - hil_sensors.baro_alt_meter = h; - hil_sensors.baro_temp_celcius = tempC; - - /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil pressure at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { mavlink_hil_state_t hil_state; |