Merged with master

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;