MetaMotionS: Unclear output data format

Hi,

I am busy reverse engineering the C++ SDK for using the metamotionS in Dart.
For now I am rapid prototyping the config and start of the Sensor Fusion in Python.

I am able to get a stream of data on the notification gatt service, and its header indicaties it is indeed Sensor Fusion data.
However the data, when converted to float32, shows for a full rotation something like around 11.00.

This makes no sense to me.

I basically performed the same instructions as is done in the C++ SDK in mbl_mw_sensor_fusion_config and mbl_mw_sensor_fusion_start.
I tried to configure it for NDOF and Euler angles

The output in bytes after a full rotation:
[25, 8, 129, 240, 179, 67, 29, 93, 45, 65, 230, 105, 140, 188, 129, 240, 179, 67]

The first two bytes 25 and 8 is the header and indicates sensor fusion data in euler format, the the data has 4 floats for heading, pitch, roll, and yaw (MblMwEulerAngles).

Therefore the decoded data for heading, pitch, roll, and yaw is:
359.88, 10.84, -0.02, 359.88

What am I missing here, why do I get 10.84, where I would expect a full rotation (360 degrees, or just around 0).
(The values around 359,88 are there because of a slight negative rotation around that axis, not the axis I turned the full 360 degrees)

Thank you in advance!!!

Comments

  • @Doubie

    It may be within the error margin immediately after turning on the sensor fusion. If you are not using a fixture there may be some angle error in the final resting position. Auto calibration of the magnetometer may be kicking in or introducing an offset.

    I would definitely be careful to follow the settings the C++ API uses when configuring sensor fusion. Some of the modes require fairly specific sampling rates from the raw sensor, and that is generally enforced by the API.

    In general as you break down different modules, I would preserve the sequencing of commands as closely as possible. There are many operations throughout the firmware that need to happen in order to behave correctly.

  • Hi @Matt,

    Thanks for your response.
    I reverse engineered all the sequences from the SDK, including the sample rates (those +1 and -1 of the enums as well)

    I'm a bit lost here why it only outputs 10 at a full rotation instead of 360 degrees. I have been fiddling around quite a bit already.

    I kept all the sequences in the mbl_mw_sensor_fusion_config and mbl_mw_sensor_fusion_start.

    As a summary, this is the sequence now (in pseudo python code, for a MetaMotionS):

    commandUUID = "326a9001-85cb-9195-d9dd-464cfbbae75a"
    angleUUID = "326a9006-85cb-9195-d9dd-464cfbbae75a"
    
    MBL_MW_MODULE_SENSOR_FUSION = 25
    MBL_MW_MODULE_ACCELEROMETER = 3
    MBL_MW_MODULE_GYRO = 19
    MBL_MW_MODULE_MAGNETOMETER = 21
    MBL_MW_GYRO_BOSCH_ODR_100Hz = 8
    
    MBL_MW_SENSOR_FUSION_DATA_QUATERNION = 3
    MBL_MW_SENSOR_FUSION_DATA_EULER_ANGLE = 4
    
    SENSOR_FUSION_REGISTER_EULER_ANGLE = 8
    SENSOR_FUSION_MODE = 2
    ACC_DATA_CONFIG = 3
    GYRO_DATA_CONFIG = 3
    MAG_DATA_REPETITION = 4
    MAG_DATA_RATE = 3
    ACC_DATA_INTERRUPT_ENABLE = 2
    GYRO_DATA_INTERRUPT_ENABLE = 2
    MAG_DATA_INTERRUPT_ENABLE = 2
    ACC_POWER_MODE = 1
    GYRO_POWER_MODE = 1
    MAG_POWER_MODE = 1
    FUSION_OUTPUT_ENABLE = 3
    FUSION_ENABLE = 1
    
    MBL_MW_SENSOR_FUSION_MODE_NDOF = 1
    MBL_MW_SENSOR_FUSION_MODE_IMU_PLUS = 2
    MBL_MW_SENSOR_FUSION_ACC_RANGE_4G = 1
    MBL_MW_SENSOR_FUSION_ACC_RANGE_16G = 3
    MBL_MW_SENSOR_FUSION_GYRO_RANGE_2000DPS = 0
    MBL_MW_SENSOR_FUSION_GYRO_RANGE_500DPS = 2
    
      mode = MBL_MW_SENSOR_FUSION_MODE_NDOF
      acc_fusion_range = MBL_MW_SENSOR_FUSION_ACC_RANGE_16G
      gyro_fusion_range = MBL_MW_SENSOR_FUSION_GYRO_RANGE_2000DPS + 1
    
      # // Write the mode:
      # {MBL_MW_MODULE_SENSOR_FUSION, ORDINAL(SensorFusionRegister::MODE), mode, [acc_range, gyro_range]}; // acc_range and gyro range share one byte.
      # SEND_COMMAND
      print("write fusion mode")
      data = bytearray([MBL_MW_MODULE_SENSOR_FUSION, SENSOR_FUSION_MODE, mode, (acc_fusion_range << 4) + gyro_fusion_range])
      self.writeValue(commandUUID, data)
    
      # // First configure and write accelerometer.
      range = acc_fusion_range; # MBL_MW_SENSOR_FUSION_ACC_RANGE_4G
      us = 1; # 0 when odr < 12.5Hz
      bwp = 2; #
      odr = 8 # 100 Hz (MblMwAccBmi270Odr + 1), offsette by one, probably to be compatible with others.
    
          # uint8_t odr:4; //bit 0-4
          # uint8_t bwp:3;
          # uint8_t us:1; // acc_conf.acc_filter_perf
          # uint8_t range:3; //bit 0-3
          # uint8_t:5;
    
      # {MBL_MW_MODULE_ACCELEROMETER, ORDINAL(AccelerometerBmi160Register::DATA_CONFIG), [odr, bwp, us, range]} // Shared last byte
      # SEND_COMMAND
      data = bytearray([MBL_MW_MODULE_ACCELEROMETER, ACC_DATA_CONFIG, (odr << 4) + (bwp << 1) + us , range << 5])
      self.writeValue(commandUUID, data)
    
      # // Then write the gyro
      gyr_range = gyro_fusion_range - 1
      gyr_odr = MBL_MW_GYRO_BOSCH_ODR_100Hz
      gyr_bwp = 2
      # {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::CONFIG), [gyr_odr, gyr_bwp], [00, gyr_range] // Sharing the last byte`
      # SEND_COMMAND
      data = bytearray([MBL_MW_MODULE_GYRO, GYRO_DATA_CONFIG, (gyr_odr << 4) + (gyr_bwp << 2), (gyr_range << 5)])
      self.writeValue(commandUUID, data)
    
      # // Configure magneto
      xy_reps = 9
      z_reps = 15
      odr = 6 #MBL_MW_MAG_BMM150_ODR_25Hz
    
      # uint8_t data_rep_cmd[4]= { MBL_MW_MODULE_MAGNETOMETER, ORDINAL(MagnetometerBmm150Register::DATA_REPETITIONS),
      #             static_cast<uint8_t>((xy_reps - 1) / 2), static_cast<uint8_t>(z_reps - 1) };
      # send_command(board, data_rep_cmd, sizeof(data_rep_cmd));
      data = bytearray([MBL_MW_MODULE_MAGNETOMETER, MAG_DATA_REPETITION, int((xy_reps-1)/2), z_reps-1])
      self.writeValue(commandUUID, data)
    
    
      # uint8_t data_rate_cmd[3]= { MBL_MW_MODULE_MAGNETOMETER, ORDINAL(MagnetometerBmm150Register::DATA_RATE), static_cast<uint8_t>(odr) };
      # send_command(board, data_rate_cmd, sizeof(data_rate_cmd));
      # }
      data = bytearray([MBL_MW_MODULE_MAGNETOMETER, MAG_DATA_RATE, odr])
      self.writeValue(commandUUID, data)
    
      # // Then start all the stuff!
      # //mbl_mw_acc_enable_acceleration_sampling(board);
      # uint8_t command[4]= {MBL_MW_MODULE_ACCELEROMETER, ORDINAL(AccelerometerBmi160Register::DATA_INTERRUPT_ENABLE), 1, 0};
      # SEND_COMMAND;
      data = bytearray([MBL_MW_MODULE_ACCELEROMETER, ACC_DATA_INTERRUPT_ENABLE, 1, 0])
      self.writeValue(commandUUID, data)
    
      # //mbl_mw_gyro_bmi160_enable_rotation_sampling(board);
      # uint8_t command[4]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::DATA_INTERRUPT_ENABLE), 0x1, 0x0};
      # SEND_COMMAND;
      data = bytearray([MBL_MW_MODULE_GYRO, GYRO_DATA_INTERRUPT_ENABLE, 1, 0])
      self.writeValue(commandUUID, data)
    
      # //mbl_mw_mag_bmm150_enable_b_field_sampling(board);
      # uint8_t command[4]= { MBL_MW_MODULE_MAGNETOMETER, ORDINAL(MagnetometerBmm150Register::DATA_INTERRUPT_ENABLE), 1, 0 };
      # SEND_COMMAND;
      data = bytearray([MBL_MW_MODULE_MAGNETOMETER, MAG_DATA_INTERRUPT_ENABLE, 1, 0])
      self.writeValue(commandUUID, data)
    
      # //mbl_mw_acc_start(board);
      # uint8_t command[3]= {MBL_MW_MODULE_ACCELEROMETER, ORDINAL(AccelerometerBmi160Register::POWER_MODE), 1}; //if us ==0 then 1 is 2.
      # SEND_COMMAND;
      data = bytearray([MBL_MW_MODULE_ACCELEROMETER, ACC_POWER_MODE, 1])
      self.writeValue(commandUUID, data)
    
      # //mbl_mw_gyro_bmi160_start(board);
      # uint8_t command[3]= {MBL_MW_MODULE_GYRO, ORDINAL(GyroBmi160Register::POWER_MODE), 1};
      # SEND_COMMAND;
      data = bytearray([MBL_MW_MODULE_GYRO, GYRO_POWER_MODE, 1])
      self.writeValue(commandUUID, data)
    
      # //mbl_mw_mag_bmm150_start(board);
      # uint8_t command[3]= { MBL_MW_MODULE_MAGNETOMETER, ORDINAL(MagnetometerBmm150Register::POWER_MODE), 1 };
      # SEND_COMMAND;
      data = bytearray([MBL_MW_MODULE_MAGNETOMETER, MAG_POWER_MODE, 1])
      self.writeValue(commandUUID, data)
    
    
      # // Lastly enable fusion sensor to output data, and enable module
    
      # // Configure for euler angles:
      # //MBL_MW_SENSOR_FUSION_DATA_EULER_ANGLE = 4
      mask = 1 << MBL_MW_SENSOR_FUSION_DATA_EULER_ANGLE
      # uint8_t enable_cmd[4] = {MBL_MW_MODULE_SENSOR_FUSION, ORDINAL(SensorFusionRegister::OUTPUT_ENABLE),
      #         ((SensorFusionState*) board->module_config.at(MBL_MW_MODULE_SENSOR_FUSION))->enable_mask, 0x0};
      # send_command(board, enable_cmd, sizeof(enable_cmd));
      data = bytearray([MBL_MW_MODULE_SENSOR_FUSION, FUSION_OUTPUT_ENABLE, mask, 0])
      self.writeValue(commandUUID, data)
    
      # uint8_t start_cmd[3] = {MBL_MW_MODULE_SENSOR_FUSION, ORDINAL(SensorFusionRegister::ENABLE), 0x1};
      # send_command(board, start_cmd, sizeof(start_cmd));
      data = bytearray([MBL_MW_MODULE_SENSOR_FUSION, FUSION_ENABLE, 1])
      self.writeValue(commandUUID, data)
    
    
      # uint8_t command[3] = { header.module_id, header.register_id, 1 };
      # SEND_COMMAND_BOARD(owner);
      # ResponseHeader(MBL_MW_MODULE_SENSOR_FUSION, ORDINAL(SensorFusionRegister::EULER_ANGLES)),
      data = bytearray([MBL_MW_MODULE_SENSOR_FUSION, SENSOR_FUSION_REGISTER_EULER_ANGLE, 1])
      self.writeValue(commandUUID, data)
    
  • @Doubie

    Just playing with the sensorfusion a little bit at my desk.

    When sensor fusion is first enabled pitch and roll both start at zero. Shortly afterwards they converge to true pitch and roll values, presumably mostly from the gravity vector but possible magnetometer as well.

    It seems quite possible that the pitch value of around 10 is just the absolute pitch of the surface your sensor is sitting on?

    I would maybe try capturing a short stream so there is more data to consider. The fact that you're getting something that seems close to valid is a very good sign. If you still have doubts, it would be much easier if you could print out all the bytes you are sending the device as hex strings, for me to look at.

Sign In or Register to comment.