enable reading sensor data from simulator module for SITL

src/platforms/posix/drivers/accelsim/accelsim.cpp

@@ -54,6 +54,8 @@
 #include <unistd.h>
 #include <px4_getopt.h>
 
+#include <simulator/simulator.h>
+
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
@@ -86,6 +88,8 @@ static const int ERROR = -1;
 
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
+#define ACC_READ 				(0<<6)
+#define MAG_READ 				(1<<6)
 
 extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); }
 
@@ -512,6 +516,24 @@ ACCELSIM::reset()
 int
 ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
+	uint8_t cmd = send[0];
+
+	if (cmd & DIR_READ) {
+		// Get data from the simulator
+		Simulator *sim = Simulator::getInstance();
+		if (sim == NULL)
+			return ENODEV;
+
+		// FIXME - not sure what interrupt status should be
+		recv[1] = 0;
+		// skip cmd and status bytes
+		if (cmd & ACC_READ) {
+			sim->getRawAccelReport(&recv[2], len-2);
+		} else if (cmd & MAG_READ) {
+			sim->getMagReport(&recv[2], len-2);
+		}
+	}
+
 	return PX4_OK;
 }
 
@@ -986,9 +1008,10 @@ ACCELSIM::measure()
 	struct {
 		uint8_t		cmd;
 		uint8_t		status;
-		int16_t		x;
-		int16_t		y;
-		int16_t		z;
+		float temperature;
+		float		x;
+		float		y;
+		float		z;
 	} raw_accel_report;
 #pragma pack(pop)
 
@@ -999,8 +1022,11 @@ ACCELSIM::measure()
 
 	/* fetch data from the sensor */
 	memset(&raw_accel_report, 0, sizeof(raw_accel_report));
-	raw_accel_report.cmd = DIR_READ;
-	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+	raw_accel_report.cmd = DIR_READ | ACC_READ;
+
+	if(OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) {
+		return;
+	}
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -1029,54 +1055,58 @@ ACCELSIM::measure()
 	// whether it has had failures
         accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
 
-	accel_report.x_raw = raw_accel_report.x;
-	accel_report.y_raw = raw_accel_report.y;
-	accel_report.z_raw = raw_accel_report.z;
+	accel_report.x_raw = (int16_t)(raw_accel_report.x/_accel_range_scale);
+	accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale);
+	accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale);
 
-	float xraw_f = raw_accel_report.x;
-	float yraw_f = raw_accel_report.y;
-	float zraw_f = raw_accel_report.z;
+	// float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale);
+	// float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale);
+	// float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale);
 
-	// apply user specified rotation
-	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
+	// // apply user specified rotation
+	// rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
 
-	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	// float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	// float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	// float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 
 	/*
 	  we have logs where the accelerometers get stuck at a fixed
 	  large value. We want to detect this and mark the sensor as
 	  being faulty
 	 */
-	if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
-	    fabsf(_last_accel[1] - y_in_new) < 0.001f &&
-	    fabsf(_last_accel[2] - z_in_new) < 0.001f &&
-	    fabsf(x_in_new) > 20 &&
-	    fabsf(y_in_new) > 20 &&
-	    fabsf(z_in_new) > 20) {
-		_constant_accel_count += 1;
-	} else {
-		_constant_accel_count = 0;
-	}
-	if (_constant_accel_count > 100) {
-		// we've had 100 constant accel readings with large
-		// values. The sensor is almost certainly dead. We
-		// will raise the error_count so that the top level
-		// flight code will know to avoid this sensor, but
-		// we'll still give the data so that it can be logged
-		// and viewed
-		perf_count(_bad_values);
-		_constant_accel_count = 0;
-	}
-
-	_last_accel[0] = x_in_new;
-	_last_accel[1] = y_in_new;
-	_last_accel[2] = z_in_new;
-
-	accel_report.x = _accel_filter_x.apply(x_in_new);
-	accel_report.y = _accel_filter_y.apply(y_in_new);
-	accel_report.z = _accel_filter_z.apply(z_in_new);
+	// if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
+	//     fabsf(_last_accel[1] - y_in_new) < 0.001f &&
+	//     fabsf(_last_accel[2] - z_in_new) < 0.001f &&
+	//     fabsf(x_in_new) > 20 &&
+	//     fabsf(y_in_new) > 20 &&
+	//     fabsf(z_in_new) > 20) {
+	// 	_constant_accel_count += 1;
+	// } else {
+	// 	_constant_accel_count = 0;
+	// }
+	// if (_constant_accel_count > 100) {
+	// 	// we've had 100 constant accel readings with large
+	// 	// values. The sensor is almost certainly dead. We
+	// 	// will raise the error_count so that the top level
+	// 	// flight code will know to avoid this sensor, but
+	// 	// we'll still give the data so that it can be logged
+	// 	// and viewed
+	// 	perf_count(_bad_values);
+	// 	_constant_accel_count = 0;
+	// }
+
+	// _last_accel[0] = x_in_new;
+	// _last_accel[1] = y_in_new;
+	// _last_accel[2] = z_in_new;
+
+	// accel_report.x = _accel_filter_x.apply(x_in_new);
+	// accel_report.y = _accel_filter_y.apply(y_in_new);
+	// accel_report.z = _accel_filter_z.apply(z_in_new);
+
+	accel_report.x = raw_accel_report.x;
+	accel_report.y = raw_accel_report.y;
+	accel_report.z = raw_accel_report.z;
 
 	accel_report.scaling = _accel_range_scale;
 	accel_report.range_m_s2 = _accel_range_m_s2;
@@ -1109,11 +1139,11 @@ ACCELSIM::mag_measure()
 #pragma pack(push, 1)
 	struct {
 		uint8_t		cmd;
-		int16_t		temperature;
 		uint8_t		status;
-		int16_t		x;
-		int16_t		y;
-		int16_t		z;
+		float		temperature;
+		float		x;
+		float		y;
+		float		z;
 	} raw_mag_report;
 #pragma pack(pop)
 
@@ -1125,8 +1155,11 @@ ACCELSIM::mag_measure()
 
 	/* fetch data from the sensor */
 	memset(&raw_mag_report, 0, sizeof(raw_mag_report));
-	raw_mag_report.cmd = DIR_READ;
-	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+	raw_mag_report.cmd = DIR_READ | MAG_READ;
+
+	if(OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) {
+		return;
+	}
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -1146,29 +1179,33 @@ ACCELSIM::mag_measure()
 
 	mag_report.timestamp = hrt_absolute_time();
 
-	mag_report.x_raw = raw_mag_report.x;
-	mag_report.y_raw = raw_mag_report.y;
-	mag_report.z_raw = raw_mag_report.z;
+	mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale);
+	mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale);
+	mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale);
+
+	float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale);
+	float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale);
+	float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale);
 
-	float xraw_f = mag_report.x_raw;
-	float yraw_f = mag_report.y_raw;
-	float zraw_f = mag_report.z_raw;
 
 	/* apply user specified rotation */
 	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
 
-	mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
-	mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
-	mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
-	mag_report.scaling = _mag_range_scale;
-	mag_report.range_ga = (float)_mag_range_ga;
-	mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
+	// mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	// mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	// mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+	// mag_report.scaling = _mag_range_scale;
+	// mag_report.range_ga = (float)_mag_range_ga;
+	// mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
 
 	/* remember the temperature. The datasheet isn't clear, but it
 	 * seems to be a signed offset from 25 degrees C in units of 0.125C
 	 */
-	_last_temperature = 25 + (raw_mag_report.temperature * 0.125f);
+	_last_temperature = raw_mag_report.temperature;
 	mag_report.temperature = _last_temperature;
+	mag_report.x = raw_mag_report.x;
+	mag_report.y = raw_mag_report.y;
+	mag_report.z = raw_mag_report.z;
 
 	_mag_reports->force(&mag_report);
 

src/platforms/posix/drivers/barosim/baro.cpp

@@ -275,6 +275,13 @@ BAROSIM::init()
 	_measure_phase = 0;
 	_reports->flush();
 
+	_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
+				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+
+	if (_baro_topic == (orb_advert_t)(-1)) {
+			PX4_ERR("failed to create sensor_baro publication");
+	}
+
 	/* this do..while is goto without goto */
 	do {
 		/* do temperature first */
@@ -312,12 +319,6 @@ BAROSIM::init()
 
 		ret = OK;
 
-		_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
-				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
-
-		if (_baro_topic == nullptr) {
-			PX4_ERR("failed to create sensor_baro publication");
-		}
 		//PX4_WARN("sensor_baro publication %ld", _baro_topic);
 
 	} while (0);
@@ -622,7 +623,14 @@ int
 BAROSIM::collect()
 {
 	int ret;
-	uint32_t raw;
+
+#pragma pack(push, 1)
+	struct {
+		float		pressure;
+		float		altitude;
+		float		temperature;
+	} baro_report;
+#pragma pack(pop)
 
 	perf_begin(_sample_perf);
 
@@ -632,7 +640,7 @@ BAROSIM::collect()
         report.error_count = perf_event_count(_comms_errors);
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
-	ret = _interface->dev_read(0, (void *)&raw, 0);
+	ret = _interface->dev_read(0, (void *)&baro_report, 0);
 	if (ret < 0) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
@@ -641,84 +649,15 @@ BAROSIM::collect()
 
 	/* handle a measurement */
 	if (_measure_phase == 0) {
-
-		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
-
-		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
-
-		/* base sensor scale/offset values */
-		_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
-		_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
-
-		/* temperature compensation */
-		if (_TEMP < 2000) {
-
-			int32_t T2 = POW2(dT) >> 31;
-
-			int64_t f = POW2((int64_t)_TEMP - 2000);
-			int64_t OFF2 = 5 * f >> 1;
-			int64_t SENS2 = 5 * f >> 2;
-
-			if (_TEMP < -1500) {
-				int64_t f2 = POW2(_TEMP + 1500);
-				OFF2 += 7 * f2;
-				SENS2 += 11 * f2 >> 1;
-			}
-
-			_TEMP -= T2;
-			_OFF  -= OFF2;
-			_SENS -= SENS2;
-		}
-
+		report.pressure = baro_report.pressure;
+		report.altitude = baro_report.altitude;
+		report.temperature = baro_report.temperature;
+		report.timestamp = hrt_absolute_time();
 	} else {
-
-		/* pressure calculation, result in Pa */
-		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
-		_P = P * 0.01f;
-		_T = _TEMP * 0.01f;
-
-		/* generate a new report */
-		report.temperature = _TEMP / 100.0f;
-		report.pressure = P / 100.0f;		/* convert to millibar */
-
-		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
-
-		/*
-		 * PERFORMANCE HINT:
-		 *
-		 * The single precision calculation is 50 microseconds faster than the double
-		 * precision variant. It is however not obvious if double precision is required.
-		 * Pending more inspection and tests, we'll leave the double precision variant active.
-		 *
-		 * Measurements:
-		 * 	double precision: barosim_read: 992 events, 258641us elapsed, min 202us max 305us
-		 *	single precision: barosim_read: 963 events, 208066us elapsed, min 202us max 241us
-		 */
-
-		/* tropospheric properties (0-11km) for standard atmosphere */
-		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
-		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
-		const double g  = 9.80665;	/* gravity constant in m/s/s */
-		const double R  = 287.05;	/* ideal gas constant in J/kg/K */
-
-		/* current pressure at MSL in kPa */
-		double p1 = _msl_pressure / 1000.0;
-
-		/* measured pressure in kPa */
-		double p = P / 1000.0;
-
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+		report.pressure = baro_report.pressure;
+		report.altitude = baro_report.altitude;
+		report.temperature = baro_report.temperature;
+		report.timestamp = hrt_absolute_time();
 
 		/* publish it */
 		if (!(_pub_blocked)) {

src/platforms/posix/drivers/barosim/baro_sim.cpp

@@ -118,22 +118,26 @@ BARO_SIM::init()
 int
 BARO_SIM::dev_read(unsigned offset, void *data, unsigned count)
 {
+	/*
 	union _cvt {
 		uint8_t	b[4];
 		uint32_t w;
 	} *cvt = (_cvt *)data;
-	uint8_t buf[3];
+	*/
 
 	/* read the most recent measurement */
 	uint8_t cmd = 0;
-	int ret = transfer(&cmd, 1, &buf[0], 3);
+	int ret = transfer(&cmd, 1, static_cast<uint8_t *>(data), 3);
+
+	/*
 	if (ret == PX4_OK) {
-		/* fetch the raw value */
+		// fetch the raw value
 		cvt->b[0] = buf[2];
 		cvt->b[1] = buf[1];
 		cvt->b[2] = buf[0];
 		cvt->b[3] = 0;
 	}
+	*/
 
 	return ret;
 }

src/platforms/posix/drivers/gyrosim/gyrosim.cpp

@@ -379,13 +379,13 @@ private:
 	struct MPUReport {
 		uint8_t		cmd;
 		uint8_t		status;
-		uint8_t		accel_x[2];
-		uint8_t		accel_y[2];
-		uint8_t		accel_z[2];
-		uint8_t		temp[2];
-		uint8_t		gyro_x[2];
-		uint8_t		gyro_y[2];
-		uint8_t		gyro_z[2];
+		float		accel_x;
+		float		accel_y;
+		float		accel_z;
+		float		temp;
+		float		gyro_x;
+		float		gyro_y;
+		float		gyro_z;
 	};
 #pragma pack(pop)
 
@@ -1194,15 +1194,6 @@ void
 GYROSIM::measure()
 {
 	struct MPUReport mpu_report;
-	struct Report {
-		int16_t		accel_x;
-		int16_t		accel_y;
-		int16_t		accel_z;
-		int16_t		temp;
-		int16_t		gyro_x;
-		int16_t		gyro_y;
-		int16_t		gyro_z;
-	} report;
 
 	/* start measuring */
 	perf_begin(_sample_perf);
@@ -1214,69 +1205,10 @@ GYROSIM::measure()
 
         // sensor transfer at high clock speed
         //set_frequency(GYROSIM_HIGH_BUS_SPEED);
-
-	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
-		return;
-
-	/*
-	 * Convert from big to little endian
-	 */
-
-	report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
-	report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
-	report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
-
-	report.temp = int16_t_from_bytes(mpu_report.temp);
-
-	report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
-	report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
-	report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
-
-	if (report.accel_x == 0 &&
-	    report.accel_y == 0 &&
-	    report.accel_z == 0 &&
-	    report.temp == 0 &&
-	    report.gyro_x == 0 &&
-	    report.gyro_y == 0 &&
-	    report.gyro_z == 0) {
-		// all zero data - probably a VDev bus error
-		perf_count(_bad_transfers);
-		perf_end(_sample_perf);
-                // note that we don't call reset() here as a reset()
-                // costs 20ms with interrupts disabled. That means if
-                // the mpu6k does go bad it would cause a FMU failure,
-                // regardless of whether another sensor is available,
+	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) {
 		return;
 	}
 
-	perf_count(_good_transfers);
-
-	if (_register_wait != 0) {
-		// we are waiting for some good transfers before using
-		// the sensor again. We still increment
-		// _good_transfers, but don't return any data yet
-		_register_wait--;
-		return;
-	}
-
-
-	/*
-	 * Swap axes and negate y
-	 */
-	int16_t accel_xt = report.accel_y;
-	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
-
-	int16_t gyro_xt = report.gyro_y;
-	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
-
-	/*
-	 * Apply the swap
-	 */
-	report.accel_x = accel_xt;
-	report.accel_y = accel_yt;
-	report.gyro_x = gyro_xt;
-	report.gyro_y = gyro_yt;
-
 	/*
 	 * Report buffers.
 	 */
@@ -1286,7 +1218,11 @@ GYROSIM::measure()
 	/*
 	 * Adjust and scale results to m/s^2.
 	 */
-	grb.timestamp = arb.timestamp = hrt_absolute_time();
+	grb.timestamp = hrt_absolute_time();
+	arb.timestamp = grb.timestamp;
+
+	// this sleep is needed because the timing of the drivers is not yet working
+	usleep(1000);
 
 	// report the error count as the sum of the number of bad
 	// transfers and bad register reads. This allows the higher
@@ -1312,13 +1248,13 @@ GYROSIM::measure()
 
 	/* NOTE: Axes have been swapped to match the board a few lines above. */
 
-	arb.x_raw = report.accel_x;
-	arb.y_raw = report.accel_y;
-	arb.z_raw = report.accel_z;
-
-	float xraw_f = report.accel_x;
-	float yraw_f = report.accel_y;
-	float zraw_f = report.accel_z;
+	arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale);
+	arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale);
+	arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale);
+/*
+	float xraw_f = (int16_t)(mpu_report.accel_x / _accel_range_scale);
+	float yraw_f = (int16_t)(mpu_report.accel_y / _accel_range_scale);
+	float zraw_f = (int16_t)(mpu_report.accel_z / _accel_range_scale);
 
 	// apply user specified rotation
 	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
@@ -1330,22 +1266,26 @@ GYROSIM::measure()
 	arb.x = _accel_filter_x.apply(x_in_new);
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
-
+*/
 	arb.scaling = _accel_range_scale;
 	arb.range_m_s2 = _accel_range_m_s2;
 
-	_last_temperature = (report.temp) / 361.0f + 35.0f;
+	_last_temperature = mpu_report.temp;
 
-	arb.temperature_raw = report.temp;
+	arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f);
 	arb.temperature = _last_temperature;
 
-	grb.x_raw = report.gyro_x;
-	grb.y_raw = report.gyro_y;
-	grb.z_raw = report.gyro_z;
+	arb.x = mpu_report.accel_x;
+	arb.y = mpu_report.accel_y;
+	arb.z = mpu_report.accel_z;
 
-	xraw_f = report.gyro_x;
-	yraw_f = report.gyro_y;
-	zraw_f = report.gyro_z;
+	grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
+	grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
+	grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
+/*
+	xraw_f = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
+	yraw_f = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
+	zraw_f = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
 
 	// apply user specified rotation
 	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
@@ -1357,20 +1297,24 @@ GYROSIM::measure()
 	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
-
+*/
 	grb.scaling = _gyro_range_scale;
 	grb.range_rad_s = _gyro_range_rad_s;
 
-	grb.temperature_raw = report.temp;
+	grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f);
 	grb.temperature = _last_temperature;
 
+	grb.x = mpu_report.gyro_x;
+	grb.y = mpu_report.gyro_y;
+	grb.z = mpu_report.gyro_z;
+
 	_accel_reports->force(&arb);
 	_gyro_reports->force(&grb);
 
+
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
-
 	if (!(_pub_blocked)) {
 		/* log the time of this report */
 		perf_begin(_controller_latency_perf);