+ updated bad sensor id error and unified cmd_param

src/nbus_slave_module_unicast.c

@@ -38,12 +38,13 @@ void nbus_slave_unicastToModuleGet(nBus_TypeDef *nbus)
             break;
         }
 
-        nbus->tx_buffer[4] = (uint8_t)(param_value & 0xFF);
-        nbus->tx_buffer[5] = (uint8_t)((param_value >> 8) & 0xFF);
-        nbus->tx_buffer[6] = (uint8_t)((param_value >> 16) & 0xFF);
-        nbus->tx_buffer[7] = (uint8_t)((param_value >> 24) & 0xFF);
+        nbus->tx_buffer[4] = 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 += 4;
+        nbus->tx_length += 5;
     }
     break;
 

src/nbus_slave_sensor_unicast.c

@@ -4,29 +4,33 @@
 
 void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
 {
+    nBus_sensorType_t sensor_type = nbus->interface->getType(nbus->sensorInfo.address);
+
+    if (sensor_type == TYPE_UNKNOWN) // handle bad sensor address
+    {
+        setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
+        return;
+    }
+
     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_buffer[4] = sensor_type;
         nbus->tx_length += 1;
+        break;
     }
-    break;
 
     case CMD_PARAM: {
-        if (nbus->rx_length >= 5)
+        if (nbus->rx_length >= 5) // get concrete param
         {
-            if (nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]) == 0)
+            if (nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]) ==
+                0) // handle non-existing param
             {
                 setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
                 break;
             }
-            nbus->tx_buffer[4] = nbus->rx_buffer[3];
+
+            nbus->tx_buffer[4] = nbus->rx_buffer[3]; // param id
             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);
@@ -34,7 +38,7 @@ void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
             nbus->tx_buffer[8] = (uint8_t)((param_value >> 24) & 0xFF);
             nbus->tx_length += 5;
         }
-        else
+        else // get all params
         {
             nBus_param_t *params = nbus_interface_allParams();
             for (uint8_t i = 0; i < nbus_interface_allParamsCount(); i++)
@@ -42,7 +46,7 @@ void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
                 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[4 + 5 * i] = params[i]; // param id
                     // 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);
@@ -64,7 +68,7 @@ void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
             if (cnt == 0)
             {
                 setErrorResponse(nbus, DEVICE_BUSY);
-                // return;
+                break;
             }
             nbus->tx_length += cnt;
         }
@@ -98,7 +102,14 @@ void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus)
 
 void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus)
 {
-    int32_t param_value;
+    nBus_sensorType_t sensor_type = nbus->interface->getType(nbus->sensorInfo.address);
+
+    if (sensor_type == TYPE_UNKNOWN) // handle bad sensor address
+    {
+        setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
+        return;
+    }
+
     switch (nbus->function_code.function)
     {
     case CMD_PARAM: {
@@ -108,7 +119,7 @@ void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus)
             break;
         }
 
-        param_value =
+        int32_t 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);
@@ -124,15 +135,8 @@ void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus)
 
     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->tx_buffer[4] = nbus->interface->calibrate(nbus->sensorInfo.address, 0, NULL);
+        nbus->tx_length += 1;
         nbus->hw_platform->led_off();
         break;
     }