Diff: akmsensormanager.cpp

Revision:

28:dc4eb14e4d7e

Parent:

27:41aa9fb23a2f

Child:

29:b488d2c89fba

--- a/akmsensormanager.cpp	Sat Mar 11 01:14:15 2017 +0000
+++ b/akmsensormanager.cpp	Sat Mar 11 02:08:32 2017 +0000
@@ -298,15 +298,31 @@
         }
         case Message::CMD_GET_SENSOR_NUM:
         {
-            resMsg.setArgument(0, currentSensorNumber);
+            resMsg.setArgument(0, currentSensorNumber+1);
             throwMessage(&resMsg);
             MSG("#Current Sensor Number reported.\r\n");
             break;
         }
         case Message::CMD_SET_SENSOR_NUM:
         {
-            currentSensorNumber = msg.getArgument(0);
-            MSG("#Current Sensor Number set to %d.\r\n", currentSensorNumber);
+            uint8_t i=0;
+            while(sensor[i] != NULL){
+                i++;    
+            }
+            
+            uint8_t arg = (uint8_t)msg.getArgument(0);
+            
+            if( arg != 0 && msg.getArgNum() == 1 && arg < i+1 ){
+                currentSensorNumber = (uint8_t)msg.getArgument(0)-1;
+                MSG("#Current Sensor Number set to %d.\r\n", currentSensorNumber+1);                
+                resMsg.setArgument(0, 0);
+            }
+            else{
+                currentSensorNumber = 0;
+                resMsg.setArgument(0, 1);
+                MSG("#Error: Current Sensor Number set to %d.\r\n", currentSensorNumber+1);                
+            }
+            throwMessage(&resMsg);
             break;
         }
         case Message::CMD_GET_SENSOR_COUNT: