asdf

sugoi/Integrator.cpp

@@ -2,68 +2,11 @@
 #include "Quaternion.h"
 #include "Arduino.h"
 
-Integrator::Integrator(float initial /* = 0 */)
-    : value_(initial), speed_(0), prev_(0)
-{
-}
-
-float Integrator::value() const { return value_; }
-float Integrator::speed() const { return speed_; }
-
-float Integrator::accumulate(float accel, float dt)
-{
-    float v = speed_ + accel * dt / 2;
-    float dx = v * dt;
-    value_ += dx;
-    speed_ += accel * dt;
-    return dx;
-}
-
 INS::INS()
-    : gx_(0), gy_(0), gz_(0)
 {
 }
 
-float INS::posX() const { return posX_.value(); }
-float INS::posY() const { return posY_.value(); }
-float INS::posZ() const { return posZ_.value(); }
-float INS::rotX() const { return rotX_.value(); }
-float INS::rotY() const { return rotY_.value(); }
-float INS::rotZ() const { return rotZ_.value(); }
-
-float INS::posDX() const { return posX_.speed(); }
-float INS::posDY() const { return posY_.speed(); }
-float INS::posDZ() const { return posZ_.speed(); }
-float INS::rotDX() const { return rotX_.speed(); }
-float INS::rotDY() const { return rotY_.speed(); }
-float INS::rotDZ() const { return rotZ_.speed(); }
-
-float INS::grvX() const { return gx_; }
-float INS::grvY() const { return gy_; }
-float INS::grvZ() const { return gz_; }
-
-void INS::setGravity(float ax, float ay, float az)
-{
-    gx_ = ax;
-    gy_ = ay;
-    gz_ = az;
-}
-
 void INS::accumulate(float px, float py, float pz,
     float rx, float ry, float rz, float dt)
 {
-    float rdx = rotX_.accumulate(rx, dt);
-    float rdy = rotY_.accumulate(ry, dt);
-    float rdz = rotZ_.accumulate(rz, dt);
-
-    Quaternion q = Quaternion::from_euler_rotation(rdx, rdy, rdz);
-    Quaternion g(gx_, gy_, gz_);
-    g = q.rotate(g);
-    gx_ = g.b;
-    gy_ = g.c;
-    gz_ = g.d;
-
-    posX_.accumulate(px - gx_, dt);
-    posY_.accumulate(py - gy_, dt);
-    posZ_.accumulate(pz - gz_, dt);
 }

sugoi/Integrator.h

 #ifndef INTEGRATOR_H_
 #define INTEGRATOR_H_
 
-class Integrator
-{
-public:
-    explicit Integrator(float initial = 0);
-
-    float value() const;
-    float speed() const;
-    float accumulate(float accel, float dt);
-
-private:
-    float prev_;
-    float speed_;
-    float value_;
-};
+#include "Quaternion.h"
 
 class INS
 {
 public:
     INS();
 
-    float posX() const;
-    float posY() const;
-    float posZ() const;
-    float rotX() const;
-    float rotY() const;
-    float rotZ() const;
-
-    float posDX() const;
-    float posDY() const;
-    float posDZ() const;
-    float rotDX() const;
-    float rotDY() const;
-    float rotDZ() const;
-
-    float grvX() const;
-    float grvY() const;
-    float grvZ() const;
-
-    void setGravity(float ax, float ay, float az);
-
-    void accumulate(float px, float py, float pz,
-        float rx, float ry, float rz, float dt);
+    void accumulate(float ax, float ay, float azz,
+        float gx, float gy, float gz, float dt);
 
 private:
-    float gx_, gy_, gz_;
-    Integrator posX_, posY_, posZ_;
-    Integrator rotX_, rotY_, rotZ_;
+    Quaternion qrot_;
 };
 
 #endif // INTEGRATOR_H_

sugoi/sugoi.ino

 #include "MPU9250.h"
-#include "Integrator.h"
 
-MPU9250 imu(Wire, 0x68);
-INS ins;
-unsigned long prevTime;
+MPU9250 IMU(Wire,0x68);
+int status;
 
 void setup()
 {
   Serial.begin(115200);
+  while(!Serial) {}
 
-  int status = imu.begin();
+  status = IMU.begin();
   if (status < 0)
   {
     Serial.print("IMU Error: ");
     Serial.println(status);
-    while (1) { }
+    while(1) { }
   }
-
-  imu.readSensor();
-  ins.setGravity(imu.getAccelX_mss(), imu.getAccelY_mss(), imu.getAccelZ_mss());
 }
 
-void loop()
+void writeulong(unsigned long x)
 {
-  unsigned long nowTime = micros();
-  unsigned long deltaTime = nowTime - prevTime;
-  float dt = deltaTime / 1000000.0f;
-
-  imu.readSensor();
-  ins.accumulate(imu.getAccelX_mss(), imu.getAccelY_mss(), imu.getAccelZ_mss(),
-    imu.getGyroX_rads(), imu.getGyroY_rads(), imu.getGyroZ_rads(), dt);
+  Serial.print(x, HEX);
+  Serial.print(" ");
+}
 
-  if (millis() % 200 == 0)
-  {
-    Serial.print("dt : ");
-    Serial.print(deltaTime);
-    Serial.print("\t");
-    Serial.println(dt, 10);
-    Serial.print("ins :\t");
-    Serial.print(ins.posX());
-    Serial.print("\t");
-    Serial.print(ins.posY());
-    Serial.print("\t");
-    Serial.print(ins.posZ());
-    Serial.print("\t");
-    Serial.print(ins.rotX());
-    Serial.print("\t");
-    Serial.print(ins.rotY());
-    Serial.print("\t");
-    Serial.println(ins.rotZ());
-    Serial.print("spd :\t");
-    Serial.print(ins.posDX());
-    Serial.print("\t");
-    Serial.print(ins.posDY());
-    Serial.print("\t");
-    Serial.print(ins.posDZ());
-    Serial.print("\t");
-    Serial.print(ins.rotDX());
-    Serial.print("\t");
-    Serial.print(ins.rotDY());
-    Serial.print("\t");
-    Serial.println(ins.rotDZ());
-    Serial.print("grv :\t");
-    Serial.print(ins.grvX());
-    Serial.print("\t");
-    Serial.print(ins.grvY());
-    Serial.print("\t");
-    Serial.print(ins.grvZ());
-    Serial.print("\t(len: ");
-    Serial.print(sqrt(ins.grvX()*ins.grvX()+ins.grvY()*ins.grvY()+ins.grvZ()*ins.grvZ()));
-    Serial.println(")");
-    Serial.print("imu :\t");
-    Serial.print(imu.getAccelX_mss());
-    Serial.print("\t");
-    Serial.print(imu.getAccelY_mss());
-    Serial.print("\t");
-    Serial.print(imu.getAccelZ_mss());
-    Serial.print("\t");
-    Serial.print(imu.getGyroX_rads());
-    Serial.print("\t");
-    Serial.print(imu.getGyroY_rads());
-    Serial.print("\t");
-    Serial.println(imu.getGyroZ_rads());
-    Serial.print("img :\t");
-    Serial.print(imu.getAccelX_mss() - ins.grvX());
-    Serial.print("\t");
-    Serial.print(imu.getAccelY_mss() - ins.grvY());
-    Serial.print("\t");
-    Serial.println(imu.getAccelZ_mss() - ins.grvZ());
-  }
+void writefloat(float x)
+{
+  byte* ptr = (byte*)&x;
+  Serial.print(ptr[3], HEX);
+  Serial.print(ptr[2], HEX);
+  Serial.print(ptr[1], HEX);
+  Serial.print(ptr[0], HEX);
+  Serial.print(" ");
+}
 
-  prevTime = nowTime;
+void loop()
+{
+  IMU.readSensor();
+  Serial.print("=");
+  writeulong(micros());
+  writefloat(IMU.getGyroX_rads());
+  writefloat(IMU.getGyroY_rads());
+  writefloat(IMU.getGyroZ_rads());
+  Serial.println();
+  delay(20);
 }

wuhyuo/GraphRenderer.xaml.cs

@@ -36,6 +36,8 @@ namespace wuhyuo
             set { SetValue(StrokeProperty, value); }
         }
 
+        public double Time { get; private set; } = 0;
+
         public GraphRenderer()
         {
             InitializeComponent();
@@ -48,9 +50,10 @@ namespace wuhyuo
             polyline.StrokeThickness = 2;
         }
 
-        public void AddValue(double time, double value)
+        public void AddValue(double dt, double value)
         {
-            double x = 50 * time;
+            Time += dt;
+            double x = 50 * Time;
             double y = 10 * value;
 
             polyline.Points.Add(new Point(x, y));

wuhyuo/MainWindow.xaml.cs

@@ -53,7 +53,7 @@ namespace wuhyuo
 
             try
             {
-                SerialPort port = new SerialPort("COM8", 115200);
+                SerialPort port = new SerialPort("COM7", 115200);
                 port.Open();
 
                 while (!_thrdStop)
@@ -84,9 +84,15 @@ namespace wuhyuo
                         if (tokens.Length != 7)
                             continue;
 
-                        uint micros = Convert.ToUInt32(tokens[0], 16);
-                        double[] values = tokens.Skip(1)
-                            .Select(x => (double)uint2float(Convert.ToUInt32(x, 16))).ToArray();
+                        uint micros;
+                        double[] values;
+                        try
+                        {
+                            micros = Convert.ToUInt32(tokens[0], 16);
+                            values = tokens.Skip(1)
+                                .Select(x => (double)uint2float(Convert.ToUInt32(x, 16))).ToArray();
+                        }
+                        catch (FormatException) { continue; }
 
                         if (prevTime is null || prevTime.Value >= micros)
                         {
@@ -95,6 +101,7 @@ namespace wuhyuo
                         }
 
                         double dt = (micros - prevTime.Value) / 1000000.0;
+                        prevTime = micros;
                         System.Diagnostics.Debug.Assert(dt < 1000);
 
                         Dispatcher.InvokeAsync(() =>
@@ -102,9 +109,10 @@ namespace wuhyuo
                             double wx = values[3];
                             double wy = values[4];
                             double wz = values[5];
-                            var w = new Vector3D(wx, wy, wz);
-                            var p = new Quaternion(w, w.Length);
-                            viewRotate.Quaternion = p * viewRotate.Quaternion;
+                            var w = new Quaternion(wx, wy, wz, 0);
+                            var qdot = Scale(viewRotate.Quaternion * w, 0.5);
+                            viewRotate.Quaternion += Scale(qdot, dt);
+                            viewRotate.Quaternion = Normalize(viewRotate.Quaternion);
 
                             accelX.AddValue(dt, values[0]);
                             accelY.AddValue(dt, values[1]);
@@ -132,6 +140,17 @@ namespace wuhyuo
             }
         }
 
+        public static Quaternion Scale(Quaternion lhs, double rhs)
+        {
+            return new Quaternion(lhs.X * rhs, lhs.Y * rhs, lhs.Z * rhs, lhs.W * rhs);
+        }
+
+        public static Quaternion Normalize(Quaternion q)
+        {
+            double l = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
+            return Scale(q, 1 / l);
+        }
+
         private static unsafe float uint2float(uint value)
         {
             return *(float*)(&value);