diff --git a/common/src/main/java/de/tudresden/inf/st/sensorsharing/common/MadgwickAHRS.java b/common/src/main/java/de/tudresden/inf/st/sensorsharing/common/MadgwickAHRS.java
new file mode 100644
index 0000000000000000000000000000000000000000..44eb46c57699b74ca3f6a5e8617d2d0469495a3f
--- /dev/null
+++ b/common/src/main/java/de/tudresden/inf/st/sensorsharing/common/MadgwickAHRS.java
@@ -0,0 +1,254 @@
+package de.tudresden.inf.st.sensorsharing.common;
+
+public class MadgwickAHRS {
+ /// <summary>
+ /// Gets or sets the sample period.
+ /// </summary>
+ public float SamplePeriod;
+
+ /// <summary>
+ /// Gets or sets the algorithm gain beta.
+ /// </summary>
+ public float Beta;
+
+ /// <summary>
+ /// Gets or sets the Quaternion output.
+ /// </summary>
+ public float[] Quaternion;
+
+ float roll;
+ float pitch;
+ float yaw;
+ boolean anglesComputed = false;
+
+ /// <summary>
+ /// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
+ /// </summary>
+ /// <param name="samplePeriod">
+ /// Sample period.
+ /// </param>
+ public MadgwickAHRS(float samplePeriod) {
+ this(samplePeriod, 1f);
+ }
+
+ /// <summary>
+ /// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
+ /// </summary>
+ /// <param name="samplePeriod">
+ /// Sample period.
+ /// </param>
+ /// <param name="beta">
+ /// Algorithm gain beta.
+ /// </param>
+ public MadgwickAHRS(float samplePeriod, float beta) {
+ SamplePeriod = samplePeriod;
+ Beta = beta;
+ Quaternion = new float[]{1f, 0f, 0f, 0f};
+ }
+
+ public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+ float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability
+ float norm;
+ float hx, hy, _2bx, _2bz;
+ float s1, s2, s3, s4;
+ float qDot1, qDot2, qDot3, qDot4;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float _2q1mx;
+ float _2q1my;
+ float _2q1mz;
+ float _2q2mx;
+ float _4bx;
+ float _4bz;
+ float _2q1 = 2f * q1;
+ float _2q2 = 2f * q2;
+ float _2q3 = 2f * q3;
+ float _2q4 = 2f * q4;
+ float _2q1q3 = 2f * q1 * q3;
+ float _2q3q4 = 2f * q3 * q4;
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q1q4 = q1 * q4;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q2q4 = q2 * q4;
+ float q3q3 = q3 * q3;
+ float q3q4 = q3 * q4;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = (float) Math.sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0f) return; // handle NaN
+ norm = 1 / norm; // use reciprocal for division
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Normalise magnetometer measurement
+ norm = (float) Math.sqrt(mx * mx + my * my + mz * mz);
+ if (norm == 0f) return; // handle NaN
+ norm = 1 / norm; // use reciprocal for division
+ mx *= norm;
+ my *= norm;
+ mz *= norm;
+
+ // Reference direction of Earth's magnetic field
+ _2q1mx = 2f * q1 * mx;
+ _2q1my = 2f * q1 * my;
+ _2q1mz = 2f * q1 * mz;
+ _2q2mx = 2f * q2 * mx;
+ hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+ hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+ _2bx = (float) Math.sqrt(hx * hx + hy * hy);
+ _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+ _4bx = 2f * _2bx;
+ _4bz = 2f * _2bz;
+
+ // Gradient decent algorithm corrective step
+ s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ norm = 1f / (float) Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
+ s1 *= norm;
+ s2 *= norm;
+ s3 *= norm;
+ s4 *= norm;
+
+ // Compute rate of change of quaternion
+ qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
+ qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
+ qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
+ qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;
+
+ // Integrate to yield quaternion
+ q1 += qDot1 * SamplePeriod;
+ q2 += qDot2 * SamplePeriod;
+ q3 += qDot3 * SamplePeriod;
+ q4 += qDot4 * SamplePeriod;
+ norm = 1f / (float) Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
+ Quaternion[0] = q1 * norm;
+ Quaternion[1] = q2 * norm;
+ Quaternion[2] = q3 * norm;
+ Quaternion[3] = q4 * norm;
+ anglesComputed = false;
+ }
+
+ public void Update(float gx, float gy, float gz, float ax, float ay, float az) {
+ float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability
+ float norm;
+ float s1, s2, s3, s4;
+ float qDot1, qDot2, qDot3, qDot4;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float _2q1 = 2f * q1;
+ float _2q2 = 2f * q2;
+ float _2q3 = 2f * q3;
+ float _2q4 = 2f * q4;
+ float _4q1 = 4f * q1;
+ float _4q2 = 4f * q2;
+ float _4q3 = 4f * q3;
+ float _8q2 = 8f * q2;
+ float _8q3 = 8f * q3;
+ float q1q1 = q1 * q1;
+ float q2q2 = q2 * q2;
+ float q3q3 = q3 * q3;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = (float) Math.sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0f) return; // handle NaN
+ norm = 1 / norm; // use reciprocal for division
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Gradient decent algorithm corrective step
+ s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
+ s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
+ s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
+ s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay;
+ norm = 1f / (float) Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
+ s1 *= norm;
+ s2 *= norm;
+ s3 *= norm;
+ s4 *= norm;
+
+ // Compute rate of change of quaternion
+ qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
+ qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
+ qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
+ qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;
+
+ // Integrate to yield quaternion
+ q1 += qDot1 * SamplePeriod;
+ q2 += qDot2 * SamplePeriod;
+ q3 += qDot3 * SamplePeriod;
+ q4 += qDot4 * SamplePeriod;
+ norm = 1f / (float) Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
+ Quaternion[0] = q1 * norm;
+ Quaternion[1] = q2 * norm;
+ Quaternion[2] = q3 * norm;
+ Quaternion[3] = q4 * norm;
+ anglesComputed = false;
+ }
+
+ public float getSamplePeriod() {
+ return SamplePeriod;
+ }
+
+ public void setSamplePeriod(float samplePeriod) {
+ SamplePeriod = samplePeriod;
+ }
+
+ public float getBeta() {
+ return Beta;
+ }
+
+ public void setBeta(float beta) {
+ Beta = beta;
+ }
+
+ public float[] getQuaternion() {
+ return Quaternion;
+ }
+
+ public void setQuaternion(float[] quaternion) {
+ Quaternion = quaternion;
+ }
+
+ public void computeAngles()
+ {
+ float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability
+ roll = (float) Math.atan2(q1*q2 + q3*q4, 0.5f - q2*q2 - q3*q3);
+ pitch = (float) Math.asin(-2.0f * (q2*q4 - q1*q3));
+ yaw = (float) Math.atan2(q2*q3 + q1*q4, 0.5f - q3*q3 - q4*q4);
+ anglesComputed = true;
+ }
+
+ public float getRoll() {
+ if (!anglesComputed) computeAngles();
+ return roll * 57.29578f;
+ }
+ public float getPitch() {
+ if (!anglesComputed) computeAngles();
+ return pitch * 57.29578f;
+ }
+ public float getYaw() {
+ if (!anglesComputed) computeAngles();
+ return yaw * 57.29578f + 180.0f;
+ }
+ public float getRollRadians() {
+ if (!anglesComputed) computeAngles();
+ return roll;
+ }
+ public float getPitchRadians() {
+ if (!anglesComputed) computeAngles();
+ return pitch;
+ }
+ public float getYawRadians() {
+ if (!anglesComputed) computeAngles();
+ return yaw;
+ }
+}
diff --git a/mobile/src/main/java/de/tudresden/inf/st/sensorsharing/MainActivity.java b/mobile/src/main/java/de/tudresden/inf/st/sensorsharing/MainActivity.java
index b8d05c559847f6428836dd210d0b00864b8c41d4..5e154a0dbf97bab30a9e2c7ac7b497e415f1b50f 100644
--- a/mobile/src/main/java/de/tudresden/inf/st/sensorsharing/MainActivity.java
+++ b/mobile/src/main/java/de/tudresden/inf/st/sensorsharing/MainActivity.java
@@ -19,6 +19,8 @@ import android.widget.EditText;
import android.widget.TextView;
import android.widget.Toast;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
import com.google.android.gms.wearable.MessageClient;
import com.google.android.gms.wearable.MessageEvent;
import com.google.android.gms.wearable.Wearable;
@@ -34,12 +36,13 @@ import org.eclipse.paho.client.mqttv3.MqttMessage;
import java.nio.ByteBuffer;
import java.text.DateFormat;
-import java.util.Arrays;
import java.util.Date;
+import java.util.HashMap;
import java.util.Locale;
import java.util.Objects;
import java.util.concurrent.ConcurrentHashMap;
+import de.tudresden.inf.st.sensorsharing.common.MadgwickAHRS;
import de.tudresden.inf.st.sensorsharing.common.MqttUtils;
import de.tudresden.inf.st.sensorsharing.common.Utils;
import de.tudresden.inf.st.sensorsharing.settings.AppSettings;
@@ -52,6 +55,7 @@ import static de.tudresden.inf.st.sensorsharing.common.Constants.SUB_KEY_LINEAR_
import static de.tudresden.inf.st.sensorsharing.common.Constants.SUB_KEY_NAME;
import static de.tudresden.inf.st.sensorsharing.common.Constants.SUB_KEY_ROTATION_VECTOR;
import static de.tudresden.inf.st.sensorsharing.settings.AppSettings.SETTINGS_MQTT_SERVER;
+import static java.lang.Math.atan2;
public class MainActivity extends AppCompatActivity implements
SensorEventListener,
@@ -64,6 +68,9 @@ public class MainActivity extends AppCompatActivity implements
private SensorManager mSensorManager;
private Sensor mLight;
private Sensor mRotationVector;
+ private Sensor gyroSensor;
+ private Sensor accelSensor;
+ private Sensor magSensor;
// mqtt management
private static final int MQTT_DEFAULT_QOS = 0;
@@ -74,7 +81,9 @@ public class MainActivity extends AppCompatActivity implements
private final ConcurrentHashMap<String, Boolean> sendMqttUpdates = new ConcurrentHashMap<>();
// wearable management
- /** Delay in milliseconds after which the wearable is assumed to be offline */
+ /**
+ * Delay in milliseconds after which the wearable is assumed to be offline
+ */
public static final int DELAY_WEARABLE_ASSUMED_OFFLINE = 30000;
private Date lastMessageFromWearable = new Date();
private DateFormat dateTimeFormat = DateFormat.getDateTimeInstance(
@@ -82,9 +91,13 @@ public class MainActivity extends AppCompatActivity implements
// sensor values of phone
private float[] mRotationVectorValues;
+ private float[] gyroVectorValues;
+ private float[] accelVectorValues;
+ private float[] magVectorValues;
private float mLightValue;
private String clientId;
private AppSettings settings;
+ MadgwickAHRS filter;
private EditText valueServerURI;
@@ -97,6 +110,7 @@ public class MainActivity extends AppCompatActivity implements
initMqttClientId();
updateMqttTopics("unknown");
+ filter = new MadgwickAHRS(50, 1f);
settings = new AppSettings(this);
initViews();
@@ -108,7 +122,13 @@ public class MainActivity extends AppCompatActivity implements
return;
}
mLight = mSensorManager.getDefaultSensor(Sensor.TYPE_LIGHT);
+
+ gyroSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
mRotationVector = mSensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR);
+ magSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
+ if (mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER) != null){
+ accelSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+ }
if (mRotationVector == null) {
mRotationVector = mSensorManager.getDefaultSensor(Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR);
if (mRotationVector == null) {
@@ -124,6 +144,9 @@ public class MainActivity extends AppCompatActivity implements
// initialize sensor values
mLightValue = -1;
mRotationVectorValues = new float[3];
+ accelVectorValues = new float[3];
+ gyroVectorValues = new float[3];
+ magVectorValues = new float[3];
updateWearStatus(false);
connectMqtt(null);
@@ -170,7 +193,7 @@ public class MainActivity extends AppCompatActivity implements
}
wearTopics = new MqttTopics(subTopic);
// use for phone model for now
- phoneTopics = new MqttTopics(Build.DEVICE);
+ phoneTopics = new MqttTopics("phone");
}
private void setupSendUpdatesCheckbox() {
@@ -178,7 +201,7 @@ public class MainActivity extends AppCompatActivity implements
final CheckBox sendUpdatesRotation = findViewById(R.id.checkBox_send_rotation_ahrs);
sendMqttUpdates.put(phoneTopics.mqtt_topic_brightness, sendUpdates.isChecked());
- sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation, sendUpdatesRotation.isChecked());
+ sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation_ahrs, sendUpdatesRotation.isChecked());
CompoundButton.OnCheckedChangeListener onCheckedChangeListener = new CompoundButton.OnCheckedChangeListener() {
@Override
@@ -188,7 +211,7 @@ public class MainActivity extends AppCompatActivity implements
sendMqttUpdates.put(phoneTopics.mqtt_topic_brightness, isChecked);
break;
case R.id.checkBox_send_rotation_ahrs:
- sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation, sendUpdatesRotation.isChecked());
+ sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation_ahrs, sendUpdatesRotation.isChecked());
break;
}
}
@@ -203,7 +226,7 @@ public class MainActivity extends AppCompatActivity implements
}
private void closeKeyboard() {
- if(this.getCurrentFocus() != null) {
+ if (this.getCurrentFocus() != null) {
InputMethodManager inputManager =
(InputMethodManager) getSystemService(Context.INPUT_METHOD_SERVICE);
Objects.requireNonNull(inputManager).hideSoftInputFromWindow(
@@ -255,19 +278,19 @@ public class MainActivity extends AppCompatActivity implements
try {
IMqttToken token = mqttAndroidClient.connect(options,
null, new IMqttActionListener() {
- @Override
- public void onSuccess(IMqttToken asyncActionToken) {
- Log.i(TAG, "Successfully connected to " + serverURI);
- }
-
- @Override
- public void onFailure(IMqttToken asyncActionToken, Throwable exception) {
- Log.e(TAG, "Connection to " + serverURI + " failed", exception);
- Toast.makeText(MainActivity.this, "Connection to " + serverURI + " failed",
- Toast.LENGTH_SHORT).show();
- options.setAutomaticReconnect(false);
- }
- });
+ @Override
+ public void onSuccess(IMqttToken asyncActionToken) {
+ Log.i(TAG, "Successfully connected to " + serverURI);
+ }
+
+ @Override
+ public void onFailure(IMqttToken asyncActionToken, Throwable exception) {
+ Log.e(TAG, "Connection to " + serverURI + " failed", exception);
+ Toast.makeText(MainActivity.this, "Connection to " + serverURI + " failed",
+ Toast.LENGTH_SHORT).show();
+ options.setAutomaticReconnect(false);
+ }
+ });
} catch (MqttException e) {
Toast.makeText(this, "Connection to " + serverURI + " failed",
@@ -301,6 +324,15 @@ public class MainActivity extends AppCompatActivity implements
textView = findViewById(R.id.value_own_rotation);
textView.setText(Utils.formatArray3(mRotationVectorValues));
break;
+ case Sensor.TYPE_ACCELEROMETER:
+ accelVectorValues = sensorEvent.values;
+ break;
+ case Sensor.TYPE_GYROSCOPE:
+ gyroVectorValues = sensorEvent.values;
+ break;
+ case Sensor.TYPE_MAGNETIC_FIELD:
+ magVectorValues = sensorEvent.values;
+ break;
}
if (++sensorChangedCounter >= 15) {
// only send every 15th change
@@ -320,6 +352,9 @@ public class MainActivity extends AppCompatActivity implements
initMqttClientId();
mSensorManager.registerListener(this, mLight, SensorManager.SENSOR_DELAY_UI);
mSensorManager.registerListener(this, mRotationVector, SensorManager.SENSOR_DELAY_UI);
+ mSensorManager.registerListener(this, accelSensor, SensorManager.SENSOR_DELAY_UI);
+ mSensorManager.registerListener(this, gyroSensor, SensorManager.SENSOR_DELAY_UI);
+ mSensorManager.registerListener(this, magSensor, SensorManager.SENSOR_DELAY_UI);
Wearable.getMessageClient(this).addListener(this);
}
@@ -331,7 +366,7 @@ public class MainActivity extends AppCompatActivity implements
}
private void initMqttClientId() {
- if(clientId == null || clientId.isEmpty()) {
+ if (clientId == null || clientId.isEmpty()) {
clientId = MqttUtils.createClientId();
}
}
@@ -423,8 +458,35 @@ public class MainActivity extends AppCompatActivity implements
* Whether the data is really published depends on the checkboxes.
*/
private void sendUpdateOfSmartphone() {
- sendUpdate(phoneTopics.mqtt_topic_brightness, Float.toString(mLightValue));
- sendUpdate(phoneTopics.mqtt_topic_rotation, Arrays.toString(mRotationVectorValues));
+ ObjectMapper mapper = new ObjectMapper();
+
+ HashMap<String, Float> dataLight = new HashMap<>();
+ dataLight.put("brightness", mLightValue);
+ try {
+ String s = mapper.writeValueAsString(dataLight);
+ sendUpdate(phoneTopics.mqtt_topic_brightness, s);
+ } catch (JsonProcessingException e) {
+ e.printStackTrace();
+ }
+
+ filter.Update(gyroVectorValues[0], gyroVectorValues[1], gyroVectorValues[2],
+ accelVectorValues[0], accelVectorValues[1], accelVectorValues[2],
+ magVectorValues[0], magVectorValues[1], magVectorValues[2]);
+ filter.computeAngles();
+ float yaw = filter.getYaw();
+ float pitch = filter.getPitch();
+ float roll = filter.getRoll();
+ //System.out.println(String.format("%f, %f, %f", yaw, pitch, roll));
+ HashMap<String, Float> data = new HashMap<>();
+ data.put("heading", yaw);
+ data.put("pitch", pitch);
+ data.put("roll", roll);
+ try {
+ String s = mapper.writeValueAsString(data);
+ sendUpdate(phoneTopics.mqtt_topic_rotation_ahrs, s);
+ } catch (JsonProcessingException e) {
+ e.printStackTrace();
+ }
}
private void sendUpdate(String topic, String newValue) {