Merge pull request #859 from sonydevworld/develop

Merge from develop v3.4.3

Author: SPRESENSE

examples/ahrs_pwbimu/Kconfig

@@ -22,4 +22,14 @@ config EXAMPLES_AHRS_PWBIMU_STACKSIZE
 	int "ahrs_pwbimu stack size"
 	default 2048
 
+config EXAMPLES_AHRS_PWBIMU_EXEC_GYROBIAS_ESTIMATION
+	bool "ahrs_pwbimu execute gyro-bias estimation"
+	default n
+
+config EXAMPLES_AHRS_PWBIMU_GYROBIAS_ESTIMATION_TIME
+	int "ahrs_pwbimu gyro-bias estimation time in seconds"
+	default 10
+	range 1 512
+	depends on EXAMPLES_AHRS_PWBIMU_EXEC_GYROBIAS_ESTIMATION
+
 endif

examples/ahrs_pwbimu/ahrs_pwbimu_main.c

@@ -45,6 +45,8 @@
 #include <fcntl.h>
 #include <poll.h>
 #include <errno.h>
+#include <float.h>
+#include <math.h>
 
 #include <nuttx/sensors/cxd5602pwbimu.h>
 #include <MadgwickAHRS.h>
@@ -59,10 +61,19 @@
 #define DEFAULT_GYROSCOPERANGE  (1000)
 #define DEFAULT_ACCELRANGE      (4)
 
+#define RAD2DEG(x) ((x) * 180.0 / M_PI)
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
 
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static uint32_t s_prev_time;
+static uint64_t s_prev_unroll_time;
+
 /** start_sensing()
  * @rate    [in]: 15, 30, 60, 120, 240, 480, 960, 1920
  * @adrange [in]: 2, 4, 8, 16
@@ -118,21 +129,6 @@ static int start_sensing(int rate, int adrange, int gdrange, int nfifos)
   return fd;
 }
 
-static int drop_50msdata(int fd, int samprate, cxd5602pwbimu_data_t *imu)
-{
-  int cnt = samprate / 20; /* data size of 50ms */
-
-  if (cnt == 0) cnt = 1;
-
-  while (cnt)
-    {
-      read(fd, imu, sizeof(imu[0]));
-      cnt--;
-    }
-
-  return 0;
-}
-
 static int read_imudata(int fd, cxd5602pwbimu_data_t *imudata)
 {
   char c;
@@ -181,6 +177,107 @@ static int read_imudata(int fd, cxd5602pwbimu_data_t *imudata)
   return ret;
 }
 
+static int drop_50msdata(int fd, int samprate, cxd5602pwbimu_data_t *imu)
+{
+  int cnt = samprate / 20; /* data size of 50ms */
+
+  if (cnt == 0) cnt = 1;
+
+  while (cnt)
+    {
+      read_imudata(fd, imu);
+      cnt--;
+    }
+
+  return 0;
+}
+
+#ifdef CONFIG_EXAMPLES_AHRS_PWBIMU_EXEC_GYROBIAS_ESTIMATION 
+static void max_min_check(float val, float* min_val, float* max_val)
+{
+  *min_val = val < *min_val ? val : *min_val;
+  *max_val = val > *max_val ? val : *max_val;
+}
+
+static int gyrobias_estimation(int fd, int samprate, int estimation_time,
+                               double* gyrobias, cxd5602pwbimu_data_t *imu)
+{
+  int cnt = estimation_time * samprate; /* data size of estimation time */
+  int cnt_dwn = cnt;
+  int elp_cnt = 0;
+  float min_gx = FLT_MAX, max_gx = -FLT_MAX;
+  float min_gy = FLT_MAX, max_gy = -FLT_MAX;
+  float min_gz = FLT_MAX, max_gz = -FLT_MAX;
+  float min_ax = FLT_MAX, max_ax = -FLT_MAX;
+  float min_ay = FLT_MAX, max_ay = -FLT_MAX;
+  float min_az = FLT_MAX, max_az = -FLT_MAX;
+  double accelave[3] = {0.0};
+  int i = 0;
+  
+  printf("Start gyro-bias estimation.\n");
+
+  while (cnt_dwn)
+    {
+      read_imudata(fd, imu);
+
+      gyrobias[0] += imu->gx;
+      gyrobias[1] += imu->gy;
+      gyrobias[2] += imu->gz;
+
+      accelave[0] += imu->ax;
+      accelave[1] += imu->ay;
+      accelave[2] += imu->az;
+
+      max_min_check(imu->gx, &min_gx, &max_gx);
+      max_min_check(imu->gy, &min_gy, &max_gy);
+      max_min_check(imu->gz, &min_gz, &max_gz);
+
+      max_min_check(imu->ax, &min_ax, &max_ax);
+      max_min_check(imu->ay, &min_ay, &max_ay);
+      max_min_check(imu->az, &min_az, &max_az);
+
+      cnt_dwn--;
+
+      if (!(cnt_dwn % samprate))
+        {
+          printf("Elapsed time : %03d seconds.\n", ++elp_cnt);
+        }
+    }
+
+  printf("Finish gyro-bias estimation.\n");
+
+  for(i = 0; i < 3; i++)
+    {
+      gyrobias[i] /= (double)cnt;
+      accelave[i] /= (double)cnt;
+    }
+
+  printf("gx: ave=%12.3e, min=%12.3e, max=%12.3e\n", gyrobias[0], min_gx, max_gx);
+  printf("gy: ave=%12.3e, min=%12.3e, max=%12.3e\n", gyrobias[1], min_gy, max_gy);
+  printf("gz: ave=%12.3e, min=%12.3e, max=%12.3e\n", gyrobias[2], min_gz, max_gz);
+
+  printf("ax: ave=%12.3e, min=%12.3e, max=%12.3e\n", accelave[0], min_ax, max_ax);
+  printf("ay: ave=%12.3e, min=%12.3e, max=%12.3e\n", accelave[1], min_ay, max_ay);
+  printf("az: ave=%12.3e, min=%12.3e, max=%12.3e\n", accelave[2], min_az, max_az);
+
+  return 0;
+}
+#endif
+
+static uint64_t generate_unroll_timestamp(uint32_t curr_time)
+{
+  s_prev_unroll_time += (uint64_t)(curr_time - s_prev_time);
+  s_prev_time = curr_time;
+  return s_prev_unroll_time;
+}
+
+static void set_initial_posture(int fd, struct ahrs_out_s *inst,
+                                cxd5602pwbimu_data_t *imu)
+{
+  read_imudata(fd, imu);
+  setPostureByAccel(inst, imu->ax, imu->ay, imu->az, 0.f);
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -191,9 +288,13 @@ int main(int argc, FAR char *argv[])
   struct ahrs_out_s ahrs;
   cxd5602pwbimu_data_t imu;
   float e[3];
+  double gyrobias[3] = {0.0};
   unsigned int cnt = 0;
   int print_hex = 0;
 
+  s_prev_time = 0;
+  s_prev_unroll_time = 0;
+
   if (argc == 2 && argv[1][0] == 'h')
     {
       print_hex = 1;
@@ -213,25 +314,43 @@ int main(int argc, FAR char *argv[])
    *       stationary for several seconds, use that value as the BIAS
    *       value, and subtract that value from each gyro sample data.
    */
+#ifdef CONFIG_EXAMPLES_AHRS_PWBIMU_EXEC_GYROBIAS_ESTIMATION 
+  /* This is a example of gyro bias estimation.
+   * This function estimate the gyro bias and the angular velocity
+   * caused by the Earth's rotation together as offset values.
+   * Please maintain a stationary state as much as possible
+   * during the estimation.
+   */
+  gyrobias_estimation(fd, DEFAULT_SAMPLERATE, 
+                          CONFIG_EXAMPLES_AHRS_PWBIMU_GYROBIAS_ESTIMATION_TIME,
+                          gyrobias, &imu);
+#endif
+
+  set_initial_posture(fd, &ahrs, &imu);
 
   while (read_imudata(fd, &imu))
     {
-        MadgwickAHRSupdateIMU(&ahrs, imu.gx, imu.gy, imu.gz,
-                                     imu.ax, imu.ay, imu.az);
+        MadgwickAHRSupdateIMU(&ahrs,
+                              imu.gx - (float)gyrobias[0],
+                              imu.gy - (float)gyrobias[1],
+                              imu.gz - (float)gyrobias[2],
+                              imu.ax, imu.ay, imu.az);
 
         quaternion2euler(ahrs.q, e);
 
         if (++cnt >= 20)  /* Decimate data 1920Hz / 20 = 96Hz */
           {
             if (print_hex)
               {
-                printf("%08x,%08x,%08x\n", *(unsigned int *)&e[0],
-                                           *(unsigned int *)&e[1],
-                                           *(unsigned int *)&e[2]);
+                printf("%08x,%08x,%08x,%08x\n", *(unsigned int *)&ahrs.q[0],
+                                                *(unsigned int *)&ahrs.q[1],
+                                                *(unsigned int *)&ahrs.q[2],
+                                                *(unsigned int *)&ahrs.q[3]);
               }
             else
               {
-                printf("R:%0.1f, P:%0.1f, Y:%0.1f\n", e[0], e[1], e[2]);
+                uint64_t unroll_time = generate_unroll_timestamp(imu.timestamp);
+                printf("T:%0.2f, R:%0.2f, P:%0.2f, Y:%0.2f\n", (float)(unroll_time/19200000.0f), RAD2DEG(e[0]), RAD2DEG(e[1]), RAD2DEG(e[2]));
               }
 
             cnt = 0;

examples/ahrs_pwbimu/host/arrow.html

@@ -22,8 +22,9 @@
 
             const createScene = () => {
                 let scene = new BABYLON.Scene(engine);
+                scene.useRightHandedSystem = true;
                 let camera = new BABYLON.ArcRotateCamera("camera",
-                                                          -Math.PI / 2,
+                                                          Math.PI / 2,
                                                           Math.PI / 2.5,
                                                           5,
                                                           new BABYLON.Vector3(0, 0, 0),
@@ -63,8 +64,26 @@
                 arrow.material = material;
                 arrow.position.y = arrow_cylinder_height;
 
-                let x_axis = arrow.clone("xaxis")
-                x_axis.scaling = new BABYLON.Vector3(0.1, 0.1, 0.1);
+                // local coordinate axes
+                const axisLength = 1.0;
+
+                const localX = BABYLON.MeshBuilder.CreateLines("localX", {
+                    points: [BABYLON.Vector3.Zero(), new BABYLON.Vector3(axisLength, 0, 0)]
+                }, scene);
+                localX.color = new BABYLON.Color3(1, 0, 0);
+                localX.parent = arrow;
+
+                const localY = BABYLON.MeshBuilder.CreateLines("localY", {
+                    points: [BABYLON.Vector3.Zero(), new BABYLON.Vector3(0, axisLength, 0)]
+                }, scene);
+                localY.color = new BABYLON.Color3(0, 1, 0);
+                localY.parent = arrow;
+
+                const localZ = BABYLON.MeshBuilder.CreateLines("localZ", {
+                    points: [BABYLON.Vector3.Zero(), new BABYLON.Vector3(0, 0, axisLength)]
+                }, scene);
+                localZ.color = new BABYLON.Color3(0, 0, 1);
+                localZ.parent = arrow;
 
                 let material2 = new BABYLON.StandardMaterial("material2", scene);
                 material2.diffuseColor = new BABYLON.Color3(1.0, 1.0, 1.0);
@@ -73,18 +92,48 @@
                 let tile = BABYLON.MeshBuilder.CreateTiledPlane("tile", {size: 100}, scene)
                 tile.material = material2;
 
+                // global coordinate axes
+                const origin = new BABYLON.Vector3(-1, -1, 0);
+                showAxis(origin, 1.5, scene);
+
                 return { scene, arrow };
             };
 
+            function showAxis(origin, size, scene) {
+                const axisX = BABYLON.MeshBuilder.CreateLines("axisX", {
+                    points: [
+                        origin,
+                        origin.add(new BABYLON.Vector3(size, 0, 0))
+                    ]
+                }, scene);
+                axisX.color = new BABYLON.Color3(1, 0, 0);
+
+                const axisY = BABYLON.MeshBuilder.CreateLines("axisY", {
+                    points: [
+                        origin,
+                        origin.add(new BABYLON.Vector3(0, size, 0))
+                    ]
+                }, scene);
+                axisY.color = new BABYLON.Color3(0, 1, 0);
+
+                const axisZ = BABYLON.MeshBuilder.CreateLines("axisZ", {
+                    points: [
+                        origin,
+                        origin.add(new BABYLON.Vector3(0, 0, size))
+                    ]
+                }, scene);
+                axisZ.color = new BABYLON.Color3(0, 0, 1);
+            }
+
             const { scene, arrow } = createScene();
 
             const ws = new WebSocket('ws://localhost:8080');
             ws.onmessage = (event) => {
                 const posture = JSON.parse(event.data);
+                const w = posture.q_w, x = posture.q_x, y = posture.q_y, z = posture.q_z;
+                const q = new BABYLON.Quaternion(x, y, z, w);
 
-                arrow.rotation.x = posture.roll  / 180 * Math.PI;
-                arrow.rotation.y = posture.pitch / 180 * Math.PI;
-                arrow.rotation.z = posture.yaw   / 180 * Math.PI;
+                arrow.rotationQuaternion = q;
             };
 
             engine.runRenderLoop(() => {

examples/ahrs_pwbimu/host/wsserver.py

@@ -96,15 +96,17 @@ def convert_json(self, data):
     data = data.rstrip('\n')
     data = data.rstrip('\r')
     rpy = data.split(',')
-    if len(rpy) == 3:
+    if len(rpy) == 4:
       try:
-        roll  = struct.unpack(">f", binascii.unhexlify(rpy[0]))[0]
-        pitch = struct.unpack(">f", binascii.unhexlify(rpy[1]))[0]
-        yaw   = struct.unpack(">f", binascii.unhexlify(rpy[2]))[0]
-        return json.dumps({"roll" : roll, "pitch" : pitch, "yaw" : yaw})
+        q_w  = struct.unpack(">f", binascii.unhexlify(rpy[0]))[0]
+        q_x  = struct.unpack(">f", binascii.unhexlify(rpy[1]))[0]
+        q_y  = struct.unpack(">f", binascii.unhexlify(rpy[2]))[0]
+        q_z  = struct.unpack(">f", binascii.unhexlify(rpy[3]))[0]
+        return json.dumps({"q_w" : q_w, "q_x" : q_x, "q_y" : q_y, "q_z" : q_z})
       except Exception as e:
         print("   Non hexdigit : >" + str(rpy[0]) + \
-              "< >" + str(rpy[1]) + "< >" + str(rpy[2]) + "<")
+              "< >" + str(rpy[1]) + "< >" + str(rpy[2]) +
+              "< >" + str(rpy[3]) + "<")
         return None
     else:
       return None

examples/gyrocompass_pwbimu/.gitignore

@@ -0,0 +1,12 @@
+/Make.dep
+/.depend
+/.built
+/*.asm
+/*.obj
+/*.rel
+/*.lst
+/*.sym
+/*.adb
+/*.lib
+/*.src
+/*.spk