Fixed PI wrapping code, debugging more sensor code, possible misalignment of mag and acc frames in filter

2 files changed, 16 insertions, 12 deletions

diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
index af7ad7187..c6869a07e 100644
--- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -92,9 +92,9 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
 	accel_values.z = raw->accelerometer_m_s2[2];
 
 	float_vect3 mag_values;
-	mag_values.x = raw->magnetometer_ga[0];
-	mag_values.y = raw->magnetometer_ga[1];
-	mag_values.z = raw->magnetometer_ga[2];
+	mag_values.x = raw->magnetometer_ga[0]*100.0f;
+	mag_values.y = raw->magnetometer_ga[1]*100.0f;
+	mag_values.z = raw->magnetometer_ga[2]*100.0f;
 
 	attitude_blackmagic(&accel_values, &mag_values, &gyro_values);
 
@@ -215,10 +215,14 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 			att.timestamp = sensor_combined_s_local.timestamp;
 			att.roll = euler.x;
 			att.pitch = euler.y;
-			att.yaw = euler.z + M_PI;
+			att.yaw = euler.z - M_PI_F;
 
-			if (att.yaw > 2.0f * ((float)M_PI)) {
-				att.yaw -= 2.0f * ((float)M_PI);
+			if (att.yaw > M_PI_F) {
+				att.yaw -= 2.0f * M_PI_F;
+			}
+
+			if (att.yaw < -M_PI_F) {
+				att.yaw += 2.0f * M_PI_F;
 			}
 
 			att.rollspeed = rates.x;
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 7933e8518..aa006f3a6 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -315,7 +315,7 @@ int sensors_main(int argc, char *argv[])
 	float	buf_barometer[3];
 
 	int16_t	mag_offset[3] = {0, 0, 0};
-	int16_t acc_offset[3] = {0, 0, 0};
+	int16_t acc_offset[3] = {200, 0, 0};
 	int16_t	gyro_offset[3] = {0, 0, 0};
 
 	bool mag_calibration_enabled = false;
@@ -781,10 +781,10 @@ int sensors_main(int argc, char *argv[])
 			if (gyro_updated) {
 				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
 
-				raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor
+				raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor
 				/* assign negated value, except for -SHORT_MAX, as it would wrap there */
 				raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor
-				raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor
+				raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor
 
 				/* scale measurements */
 				// XXX request scaling from driver instead of hardcoding it
@@ -802,7 +802,7 @@ int sensors_main(int argc, char *argv[])
 
 				/* assign negated value, except for -SHORT_MAX, as it would wrap there */
 				raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
-				raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0];  // y on the board is x of the sensor
+				raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32768 : buf_accelerometer[0]; // y on the board is x of the sensor
 				raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32768 : buf_accelerometer[2]; // z of the board is z of the sensor
 
 				// XXX read range from sensor
@@ -820,8 +820,8 @@ int sensors_main(int argc, char *argv[])
 				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
 
 				/* assign negated value, except for -SHORT_MAX, as it would wrap there */
-				raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1]; // x of the board is -y of the sensor
-				raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is x of the sensor
+				raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1];  // x of the board is y of the sensor
+				raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is -x of the sensor
 				raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor
 
 				// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here