SensorBHI260AP_Klio.cpp

Go to the documentation of this file.

 
#include "../../../SensorBuildOpt.h"
#if !SENSORLIB_EXCLUDE_BHI260_KLIO
 
#include "SensorBHI260AP_Klio.hpp"
 
const char *SensorBHI260AP_Klio::errorMessages[9] = {
    "",
    "Invalid parameter",
    "Parameter out of range",
    "Invalid pattern operation",
    "Not implemented",
    "Buffer size",
    "Internal",
    "Undefined",
    "Operation pending"
};
 
SensorBHI260AP_Klio::SensorBHI260AP_Klio(SensorBHI260AP &sensor) :
    sensor(&sensor),
    max_patterns(0),
    max_pattern_size(0),
    similarity_result_buf(nullptr),
    similarity_idx_buf(nullptr),
    learning_callback(nullptr),
    learning_callback_user_data(nullptr),
    recognition_callback(nullptr),
    recognition_callback_user_data(nullptr),
    errorCode(KLIO_DRIVER_ERROR_NONE),
    k_state({0, 0, 0, 0}) {}
 
SensorBHI260AP_Klio::~SensorBHI260AP_Klio()
{
    end();
}
 
void SensorBHI260AP_Klio::end()
{
    if (sensor) {
        sensor->configure(BoschSensorID::KLIO, 0, 0);
        sensor->removeResultEvent(BoschSensorID::KLIO, static_klio_callback);
        sensor->configure(BoschSensorID::KLIO_LOG, 0, 0);
        sensor->removeResultEvent(BoschSensorID::KLIO_LOG, static_klio_log_callback);
    }
 
    if (similarity_result_buf ) {
        free(similarity_result_buf);
        similarity_result_buf = nullptr;
    }
    if (similarity_idx_buf ) {
        free(similarity_idx_buf);
        similarity_idx_buf = nullptr;
    }
}
 
 
bool SensorBHI260AP_Klio::begin()
{
    if (similarity_result_buf && similarity_idx_buf && sensor) {
        return true;
    }
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return false;
    }
    const uint8_t PARAM_BUF_LEN = 252;
    uint8_t param_buf[PARAM_BUF_LEN];
    uint16_t size = sizeof(param_buf);
    int major = -1;
    int minor = -1;
    int version = -1;
    int count = 0;
 
    if (!getParameter(KLIO_PARAM_ALGORITHM_VERSION, param_buf, &size)) {
        SENSORLIB_LOG_E("Unable to get Klio firmware version.");
        return false;
    }
 
    if (size > 0) {
        count = sscanf((char *)param_buf, "%d.%d.%d", &major, &minor, &version);
    }
 
    if (major < 0 || minor < 0 || version < 0 || count != 3) {
        SENSORLIB_LOG_E("Unable to get Klio firmware version.");
        return false;
    }
 
    SENSORLIB_LOG_D("Klio version %d.%d.%d - buf:%s", major, minor, version, param_buf);
 
    /* Get number of supported patterns */
    uint16_t length = sizeof(param_buf);
    if (!getParameter(KLIO_PARAM_RECOGNITION_MAX_PATTERNS, param_buf, &length)) {
        return false;
    }
    max_patterns = *((uint16_t *) param_buf);
    similarity_result_buf = (float *)malloc(sizeof(float) * max_patterns);
    if (!similarity_result_buf) {
        SENSORLIB_LOG_E("Klio result buffer allocate failed!");
        return false;
    }
    similarity_idx_buf = (uint8_t *)malloc(sizeof(uint8_t) * max_patterns);
    if (!similarity_idx_buf) {
        SENSORLIB_LOG_E("Klio index buffer allocate failed!");
        free(similarity_result_buf);
        return false;
    }
    SENSORLIB_LOG_D("Klio process allocate successfully!");
 
    /* Get maximum supported pattern size */
    length = sizeof(param_buf);
    if (!getParameter(KLIO_PARAM_PATTERN_BLOB_SIZE, param_buf, &length)) {
        free(similarity_idx_buf);
        free(similarity_result_buf);
        return false;
    }
    max_pattern_size = *((uint16_t *) param_buf);
    SENSORLIB_LOG_D("Klio Max patterns   :%u", max_patterns);
    SENSORLIB_LOG_D("Klio Max pattern len:%u", max_pattern_size);
 
    uint8_t ignore_insignificant_movement = 1;
    /* Prevent learning with small movements, parameter writes should be done after reset and before sensor enable */
    setParameter(KLIO_PARAM_LEARNING_IGNORE_INSIG_MOVEMENT, &ignore_insignificant_movement,
                 sizeof(ignore_insignificant_movement));
 
 
    sensor->onResultEvent(BoschSensorID::KLIO_LOG, static_klio_log_callback, this);
 
    return  sensor->onResultEvent(BoschSensorID::KLIO, static_klio_callback, this);
}
 
bool SensorBHI260AP_Klio::setState(bool learning_enable, bool learning_reset, bool recognition_enable, bool recognition_reset)
{
    bhy2_klio_sensor_state_t sensor_state = {
        .learning_enabled = learning_enable,
        .learning_reset = learning_reset,
        .recognition_enabled = recognition_enable,
        .recognition_reset = recognition_reset
    };
    return KlioTemplate(bhy2_klio_set_state, &sensor_state);
 
}
 
SensorBHI260AP_Klio::KlioState SensorBHI260AP_Klio::getState()
{
    bhy2_klio_sensor_state_t sensor_state;
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return {};
    }
    bhy2_klio_get_state(&sensor_state, sensor->getDev());
    return sensor_state;
}
 
bool SensorBHI260AP_Klio::setState(KlioState sensor_state)
{
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return false;
    }
    return bhy2_klio_set_state(&sensor_state, sensor->getDev()) == BHY2_OK;
}
 
bool SensorBHI260AP_Klio::learning()
{
    k_state =  getState();
    k_state.learning_enabled = true;
    return setState(k_state);
}
 
bool SensorBHI260AP_Klio::recognition(const uint8_t *pattern_ids, size_t size)
{
    if (!KlioTemplate(bhy2_klio_set_pattern_states, KLIO_PATTERN_STATE_ENABLE, pattern_ids, size)) {
        SENSORLIB_LOG_E("Klio set pattern idx failed!");
        return false;
    }
    k_state =  getState();
    k_state.learning_enabled = false;
    k_state.recognition_enabled = true;
    return setState(k_state);
}
 
bool SensorBHI260AP_Klio::getLearnPattern(uint8_t *pattern, uint16_t  *size)
{
    return KlioTemplate(bhy2_klio_read_pattern, 0, pattern, size);
}
 
uint8_t SensorBHI260AP_Klio::getMaxPatterns()
{
    return max_patterns;
}
 
void SensorBHI260AP_Klio::static_klio_callback(uint8_t sensor_id, const uint8_t *data, uint32_t size, uint64_t *timestamp, void *user_data)
{
    SensorBHI260AP_Klio *self = static_cast<SensorBHI260AP_Klio *>(user_data);
    self->klio_call_local(sensor_id, data, size, timestamp, self);
}
 
void SensorBHI260AP_Klio::klio_call_local(uint8_t sensor_id, const uint8_t *data_ptr, uint32_t size, uint64_t *timestamp, void *user_data)
{
    bhy2_klio_sensor_frame_t data;
    if (size != 11) {
        return;
    }
    memcpy(&data, data_ptr, sizeof(data));
    if (learning_callback) {
        if (k_state.learning_enabled) {
            learning_callback(static_cast<LearningChangeReason>(data.learn.change_reason),
                              data.learn.progress, data.learn.index, learning_callback_user_data);
        }
    }
    if (recognition_callback && data.recognize.index != 0xFF) {
        if (k_state.recognition_enabled) {
            recognition_callback(data.recognize.index, data.recognize.count, recognition_callback_user_data);
        }
    }
}
 
void SensorBHI260AP_Klio::static_klio_log_callback(uint8_t sensor_id, const uint8_t *data, uint32_t size, uint64_t *timestamp, void *user_data)
{
    SensorBHI260AP_Klio *self = static_cast<SensorBHI260AP_Klio *>(user_data);
    self->klio_log_call_local(sensor_id, data, size, timestamp, self);
}
 
void SensorBHI260AP_Klio::klio_log_call_local(uint8_t sensor_id, const uint8_t *data_ptr, uint32_t size, uint64_t *timestamp, void *user_data)
{
    bhy2_klio_log_frame_t data;
    memcpy(&data, data_ptr, sizeof(data));
    SENSORLIB_LOG_D("ax: %.9g, ay: %.9g, az: %.9g, gx: %.9g, gy: %.9g, gz: %.9g\n",
                    data.accel[0],
                    data.accel[1],
                    data.accel[2],
                    data.gyro[0],
                    data.gyro[1],
                    data.gyro[2]);
}
 
bool SensorBHI260AP_Klio::getParameter(KlioParameter id, uint8_t *parameter_data, uint16_t *size)
{
    return KlioTemplate(bhy2_klio_get_parameter, id, parameter_data, size);
}
 
bool SensorBHI260AP_Klio::setParameter(KlioParameter id, const void *parameter_data, uint16_t size)
{
    return KlioTemplate(bhy2_klio_set_parameter, id, parameter_data, size);
}
 
bool SensorBHI260AP_Klio::writePattern(uint8_t idx, const uint8_t  *parameter_data, uint16_t size)
{
    return KlioTemplate(bhy2_klio_write_pattern, idx, parameter_data, size);
}
 
bool SensorBHI260AP_Klio::writeMultiplePatterns(const uint8_t *ids, const uint8_t **parameter_data_array, const uint16_t *sizes, uint8_t count)
{
    for (uint8_t i = 0; i < count; ++i) {
        if (!KlioTemplate(bhy2_klio_write_pattern, ids[i], parameter_data_array[i], sizes[i])) {
            SENSORLIB_LOG_D("Write pattern with ID :%u failed", ids[i]);
            return false;
        }
    }
    return true;
}
 
bool SensorBHI260AP_Klio::setPattern(KlioPatternState operation, const uint8_t *pattern_ids, uint16_t size)
{
    return KlioTemplate(bhy2_klio_set_pattern_states, static_cast<bhy2_klio_pattern_state_t>(operation), pattern_ids, size);
}
 
bool SensorBHI260AP_Klio::enablePattern(const uint8_t *pattern_ids, uint16_t size)
{
    return KlioTemplate(bhy2_klio_set_pattern_states, KLIO_PATTERN_STATE_ENABLE, pattern_ids, size);
}
 
bool SensorBHI260AP_Klio::readPattern(uint8_t idx, uint8_t *buffer, uint16_t *length)
{
    return KlioTemplate(bhy2_klio_read_pattern, idx, buffer, length);
}
 
bool SensorBHI260AP_Klio::enable(float sample_rate, uint32_t report_latency_ms)
{
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return false;
    }
    return sensor->configure(BoschSensorID::KLIO, sample_rate, report_latency_ms);
}
 
void SensorBHI260AP_Klio::disable()
{
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return;
    }
    sensor->configure(BoschSensorID::KLIO, 0, 0);
}
 
bool SensorBHI260AP_Klio::logout(float sample_rate, uint32_t report_latency_ms)
{
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return false;
    }
    return sensor->configure(BoschSensorID::KLIO_LOG, sample_rate, report_latency_ms);
}
 
void SensorBHI260AP_Klio::setLearningCallback(LearningCallback cb, void *user_data)
{
    learning_callback = cb;
    learning_callback_user_data = user_data;
}
 
void SensorBHI260AP_Klio::setRecognitionCallback(RecognitionCallback cb, void *user_data)
{
    recognition_callback = cb;
    recognition_callback_user_data = user_data;
}
 
bool SensorBHI260AP_Klio::checkError()
{
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return false;
    }
    uint32_t klio_status;
    int8_t rslt = bhy2_klio_read_reset_driver_status(&klio_status, sensor->getDev());
    if (rslt != BHY2_OK) {
        errorCode = static_cast<KlioError>(klio_status);
        return true;
    }
    errorCode = static_cast<KlioError>(klio_status);
    return errorCode != KLIO_DRIVER_ERROR_NONE;
}
 
SensorBHI260AP_Klio::KlioError SensorBHI260AP_Klio::getError() const
{
    return errorCode;
}
 
const char *SensorBHI260AP_Klio::errorToString() const
{
    if (errorCode <= 8) {
        return errorMessages[errorCode];
    }
    return "Unknown error code";
}
 
 
template<typename Func, typename... Args>
bool SensorBHI260AP_Klio::KlioTemplate(Func func, Args &&... args)
{
    if (!sensor) {
        SENSORLIB_LOG_E("The sensor handle is null");
        return false;
    }
    int8_t rslt = func(std::forward<Args>(args)..., sensor->getDev());
    if (rslt != BHY2_OK) {
        SENSORLIB_LOG_E("Interface access error, %s", BoschSensorUtils::get_api_error(rslt));
        return false;
    }
    if (checkError()) {
        SENSORLIB_LOG_E("%s", errorToString());
        return false;
    }
    return true;
}
 
 
#endif