+
+ // Integrate rotations
+ Vect rpy = new Vect(event.values[1], event.values[0], -event.values[2]);
+ rpy.mul((double)snsInterval_ns/1E9);
+ Quat rot = new Quat(rpy.x, rpy.y, rpy.z);
+ this.rotation.mul(rot);
+ displayData(CMD.Data.ROTATION);
+
+ // Calculate 1 second gryo data
+ this.gyr1hz.add(rpy);
+ if (currentTime_ns > time1hz+1E9) {
+ displayData(CMD.Data.GYR1HZ);
+ this.gyr1hz.set(0, 0, 0);
+ time1hz = currentTime_ns;
+ }
+