MetaMotionS: Unclear output data format

Comments

• Matt

  February 2022

  @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.
• Doubie

  February 2022

  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)
  
• Matt

  February 2022

  @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.