nbus/src/nbus_slave_sensor_unicast.c

#include "nbus_slave.h"

#if MODULE_SLAVE == 1

void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
{
  switch (nbus->function_code.function)
  {
  case CMD_SENSOR_TYPE: {
    nBus_sensorType_t t = nbus->interface->getType(nbus->sensorInfo.address);
    if (t == TYPE_UNKNOWN)
    {
      setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
      break;
    }
    nbus->tx_buffer[4] = t;
    nbus->tx_length += 1;
  }
  break;

  case CMD_PARAM: {
    if (nbus->rx_length >= 5)
    {
      if (nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]) == 0)
      {
        setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
        break;
      }
      nbus->tx_buffer[4] = nbus->rx_buffer[3];
      int32_t param_value = nbus->interface->getParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]);
      nbus->tx_buffer[5] = (uint8_t)(param_value & 0xFF);
      nbus->tx_buffer[6] = (uint8_t)((param_value >> 8) & 0xFF);
      nbus->tx_buffer[7] = (uint8_t)((param_value >> 16) & 0xFF);
      nbus->tx_buffer[8] = (uint8_t)((param_value >> 24) & 0xFF);
      nbus->tx_length += 5;
    }
    else
    {
      nBus_param_t *params = nbus_interface_allParams();
      for (uint8_t i = 0; i < nbus_interface_allParamsCount(); i++)
      {
        if (nbus->interface->hasParam(nbus->sensorInfo.address, params[i]))
        {
          int32_t param_value = nbus->interface->getParam(nbus->sensorInfo.address, params[i]);
          nbus->tx_buffer[4 + 5 * i] = params[i];
          // nbus->tx_buffer[5+2*i] =
          // nbus->interface->getParam(nbus->sensorInfo.address, params[i]);
          nbus->tx_buffer[5 + 5 * i] = (uint8_t)(param_value & 0xFF);
          nbus->tx_buffer[6 + 5 * i] = (uint8_t)((param_value >> 8) & 0xFF);
          nbus->tx_buffer[7 + 5 * i] = (uint8_t)((param_value >> 16) & 0xFF);
          nbus->tx_buffer[8 + 5 * i] = (uint8_t)((param_value >> 24) & 0xFF);

          nbus->tx_length += 5;
        }
      }
    }
  }
  break;

  case CMD_DATA: {
    if (nbus->measure_active == MEASURE_RUNNING)
    {
      uint8_t cnt = nbus->interface->getData(nbus->sensorInfo.address, &nbus->tx_buffer[4]);
      if (cnt == 0)
      {
        setErrorResponse(nbus, DEVICE_BUSY);
        // return;
      }
      nbus->tx_length += cnt;
    }
    else
    {
      setErrorResponse(nbus, DEVICE_NOT_READY);
    }
  }
  break;
    
  case CMD_INFO: {
    nBus_sensorFormat_t format = nbus->interface->getFormat(nbus->sensorInfo.address);
    // const uint8_t *data = (const uint8_t*)&format; doesnt work, missaligned data        
    uint16_t data = format.sign << 15 | format.variable_length << 14 | format.q_m << 7 | format.q_n;
    nbus->tx_buffer[4] = data >> 8;
    nbus->tx_buffer[5] = data & 0xFF;
    nbus->tx_length += 2;
  }
  break;

  default: {
    setErrorResponse(nbus, ILLEGAL_FUNCTION);
  }
  }
}

void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus)
{
  int32_t param_value;
  switch (nbus->function_code.function)
  {
  case CMD_PARAM: {
    if (!nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]))
    {
      setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
      break;
    }

    param_value =
        nbus->rx_buffer[4] | nbus->rx_buffer[5] << 8 | nbus->rx_buffer[6] << 16 << nbus->rx_buffer[7] << 23;
    nBus_param_t p =
        nbus->interface->setParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3], param_value);
    if (p == PARAM_NONE)
    {
      setErrorResponse(nbus, ILLEGAL_DATA_VALUE);
      break;
    }
    nbus->tx_buffer[4] = OK_CODE;
    nbus->tx_length += 1;
  }
  break;

  case CMD_CALIBRATE: {
    nbus->hw_platform->led_on();
    if (1 == nbus->interface->calibrate(nbus->sensorInfo.address, 0, NULL))
    {
      nbus->tx_buffer[4] = 1;
      nbus->tx_length += 1;
    }
    else
    {
      setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
    }
    nbus->hw_platform->led_off();
    break;
  }

  case CMD_DATA: {
    nbus->tx_buffer[4] = nbus->interface->setData(&nbus->rx_buffer[3]);
    if (nbus->tx_buffer[4] != OK_CODE)
    {
      nbus->function_code.error = 1;
    }
    nbus->tx_length += 1;
  }
  break;

  case CMD_STORE: {
    nBus_param_t *all_params = nbus_interface_allParams();
    for (uint32_t i = 0; i < nbus_interface_allParamsCount(); i++)
    {
      if (nbus->interface->hasParam(nbus->sensorInfo.address, all_params[i]))
      {
        sensor_store_param(nbus, nbus->sensorInfo.address, all_params[i]);
        // param_value = nbus->interface->getParam(nbus->sensorInfo.address,
        // all_params[i]);
        // nbus->memoryInterface->storeParam(nbus->sensorInfo.address,
        // all_params[i], param_value);
      }
    }

    nbus->tx_buffer[4] = 1;
    nbus->tx_length += 1;
  }#include "nbus_slave.h"

#if MODULE_SLAVE == 1

void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
{
    switch (nbus->function_code.function)
    {
    case CMD_SENSOR_TYPE: {
        nBus_sensorType_t t = nbus->interface->getType(nbus->sensorInfo.address);
        if (t == TYPE_UNKNOWN)
        {
            setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
            break;
        }
        nbus->tx_buffer[4] = t;
        nbus->tx_length += 1;
    }
    break;

    case CMD_PARAM: {
        if (nbus->rx_length >= 5)
        {
            if (nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]) == 0)
            {
                setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
                break;
            }
            nbus->tx_buffer[4] = nbus->rx_buffer[3];
            int32_t param_value = nbus->interface->getParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]);
            nbus->tx_buffer[5] = (uint8_t)(param_value & 0xFF);
            nbus->tx_buffer[6] = (uint8_t)((param_value >> 8) & 0xFF);
            nbus->tx_buffer[7] = (uint8_t)((param_value >> 16) & 0xFF);
            nbus->tx_buffer[8] = (uint8_t)((param_value >> 24) & 0xFF);
            nbus->tx_length += 5;
        }
        else
        {
            nBus_param_t *params = nbus_interface_allParams();
            for (uint8_t i = 0; i < nbus_interface_allParamsCount(); i++)
            {
                if (nbus->interface->hasParam(nbus->sensorInfo.address, params[i]))
                {
                    int32_t param_value = nbus->interface->getParam(nbus->sensorInfo.address, params[i]);
                    nbus->tx_buffer[4 + 5 * i] = params[i];
                    // nbus->tx_buffer[5+2*i] =
                    // nbus->interface->getParam(nbus->sensorInfo.address, params[i]);
                    nbus->tx_buffer[5 + 5 * i] = (uint8_t)(param_value & 0xFF);
                    nbus->tx_buffer[6 + 5 * i] = (uint8_t)((param_value >> 8) & 0xFF);
                    nbus->tx_buffer[7 + 5 * i] = (uint8_t)((param_value >> 16) & 0xFF);
                    nbus->tx_buffer[8 + 5 * i] = (uint8_t)((param_value >> 24) & 0xFF);

                    nbus->tx_length += 5;
                }
            }
        }
    }
    break;

    case CMD_DATA: {
        if (nbus->measure_active == MEASURE_RUNNING)
        {
            uint8_t cnt = nbus->interface->getData(nbus->sensorInfo.address, &nbus->tx_buffer[4]);
            if (cnt == 0)
            {
                setErrorResponse(nbus, DEVICE_BUSY);
                // return;
            }
            nbus->tx_length += cnt;
        }
        else
        {
            setErrorResponse(nbus, DEVICE_NOT_READY);
        }
    }
    break;

    case CMD_INFO: {
        nBus_sensorFormat_t format = nbus->interface->getFormat(nbus->sensorInfo.address);
        // const uint8_t *data = (const uint8_t*)&format; doesnt work, missaligned data
        uint16_t data = format.sign << 15 | format.variable_length << 14 | format.q_m << 7 | format.q_n;
        nbus->tx_buffer[4] = data >> 8;
        nbus->tx_buffer[5] = data & 0xFF;
        nbus->tx_length += 2;
    }
    break;

    default: {
        setErrorResponse(nbus, ILLEGAL_FUNCTION);
    }
    }
}

void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus)
{
    int32_t param_value;
    switch (nbus->function_code.function)
    {
    case CMD_PARAM: {
        if (!nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]))
        {
            setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
            break;
        }

        param_value =
            nbus->rx_buffer[4] | nbus->rx_buffer[5] << 8 | nbus->rx_buffer[6] << 16 << nbus->rx_buffer[7] << 23;
        nBus_param_t p =
            nbus->interface->setParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3], param_value);
        if (p == PARAM_NONE)
        {
            setErrorResponse(nbus, ILLEGAL_DATA_VALUE);
            break;
        }
        nbus->tx_buffer[4] = OK_CODE;
        nbus->tx_length += 1;
    }
    break;

    case CMD_CALIBRATE: {
        nbus->hw_platform->led_on();
        if (1 == nbus->interface->calibrate(nbus->sensorInfo.address, 0, NULL))
        {
            nbus->tx_buffer[4] = 1;
            nbus->tx_length += 1;
        }
        else
        {
            setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
        }
        nbus->hw_platform->led_off();
        break;
    }

    case CMD_DATA: {
        nbus->tx_buffer[4] = nbus->interface->setData(&nbus->rx_buffer[3]);
        if (nbus->tx_buffer[4] != OK_CODE)
        {
            nbus->function_code.error = 1;
        }
        nbus->tx_length += 1;
    }
    break;

    case CMD_STORE: {
        nBus_param_t *all_params = nbus_interface_allParams();
        for (uint32_t i = 0; i < nbus_interface_allParamsCount(); i++)
        {
            if (nbus->interface->hasParam(nbus->sensorInfo.address, all_params[i]))
            {
                sensor_store_param(nbus, nbus->sensorInfo.address, all_params[i]);
                // param_value = nbus->interface->getParam(nbus->sensorInfo.address,
                // all_params[i]);
                // nbus->memoryInterface->storeParam(nbus->sensorInfo.address,
                // all_params[i], param_value);
            }
        }

        nbus->tx_buffer[4] = 1;
        nbus->tx_length += 1;
    }
    break;

    default: {
        setErrorResponse(nbus, ILLEGAL_FUNCTION);
    }
    }
}

void sensor_store_param(nBus_TypeDef *nbus, uint8_t sensor_index, uint8_t param_name)
{
    uint32_t param_value = nbus->interface->getParam(sensor_index, param_name);
    nbus->memoryInterface->storeParam(sensor_index, param_name, param_value);
}

#endif

  break;

  default: {
    setErrorResponse(nbus, ILLEGAL_FUNCTION);
  }
  }
}

void sensor_store_param(nBus_TypeDef *nbus, uint8_t sensor_index, uint8_t param_name)
{
  uint32_t param_value = nbus->interface->getParam(sensor_index, param_name);
  nbus->memoryInterface->storeParam(sensor_index, param_name, param_value);
}

#endif