C++ metawear callibration

The documentation isn't really precise when it comes to callibration. what am I suppose to look for exactly when callibrating the sensor. Is there different configurations I'm missing. the calibration seems to work differently for the different modes. could that be clarified in the documentation. I'm just assuming but I'm guessing when the state reads out 3 then that part of the sensor is calibrated. does it matter what mode the sensor is in and if so what exactly do I look for when triggering an exit state.

CaptureHandler* handler = static_cast<CaptureHandler*>(context);
    MexUtility::checkNumberOfParameters(engine,MexUtility::ParameterType::INPUT,inputs.size(),2);
    MexUtility::checkType(engine,MexUtility::ParameterType::INPUT,1,inputs[1].getType(),matlab::data::ArrayType::CHAR);


    matlab::data::CharArray address =  inputs[1];
    MetawearWrapper* wrapper =  handler->m_connectionHandler->getDevice(address.toAscii());
    if(wrapper == nullptr)  MexUtility::error(engine, "Unknown Sensor");
    MblMwMetaWearBoard*  board = wrapper->getBoard();

    auto signal = mbl_mw_sensor_fusion_calibration_state_data_signal(board);
    struct Payload{
        std::queue<MblMwCalibrationState*> queue;
        std::mutex lock;
    };
    auto c = new Payload();
    mbl_mw_datasignal_subscribe(signal, c, [](void* context, const MblMwData* data) {
        Payload* results = ( Payload*)context;
        MblMwCalibrationState* casted = (MblMwCalibrationState*) data->value;
        MblMwCalibrationState* temp = new MblMwCalibrationState();
        memcpy(temp,casted, sizeof(MblMwCalibrationState));
        results->lock.lock();
        results->queue.push(temp);
        results->lock.unlock();
    });

    mbl_mw_sensor_fusion_set_mode(board,MBL_MW_SENSOR_FUSION_MODE_NDOF);
    mbl_mw_sensor_fusion_write_config(board);

    mbl_mw_sensor_fusion_enable_data(board, MBL_MW_SENSOR_FUSION_DATA_CORRECTED_ACC);
    mbl_mw_sensor_fusion_enable_data(board, MBL_MW_SENSOR_FUSION_DATA_CORRECTED_GYRO);
    mbl_mw_sensor_fusion_enable_data(board, MBL_MW_SENSOR_FUSION_DATA_CORRECTED_MAG);
    mbl_mw_sensor_fusion_enable_data(board, MBL_MW_SENSOR_FUSION_DATA_EULER_ANGLE);
    mbl_mw_sensor_fusion_start(board);

    bool isCalibrated = false;
    do {
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
        c->lock.lock();
        while (!c->queue.empty()) {
            MblMwCalibrationState *entry = c->queue.front();
            MexUtility::printf(engine, "calibration state: {accelerometer: " + std::to_string(entry->accelrometer) +
                                       +", gyroscope: " + std::to_string(entry->gyroscope) +
                                       +", magnetometer: " + std::to_string(entry->magnetometer) +
                                       +"} \n");
            if (!isCalibrated) {
                isCalibrated = (entry->accelrometer == 3 && entry->gyroscope == 3 && entry->magnetometer == 3);
            }
            free(entry);
            c->queue.pop();
        }
        c->lock.unlock();
        mbl_mw_datasignal_read(signal);
    }
    while(!isCalibrated);
    mbl_mw_datasignal_unsubscribe(signal);
delete(c);

Comments

Sign In or Register to comment.