check array length in PWM callback

LeoRover · May 30, 2019 · cab5ff7 · cab5ff7
1 parent 2cb97b0
commit cab5ff7
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/main.cpp b/main.cpp
@@ -54,12 +54,16 @@ class ServoWrapper
 
 	void pwmCallback(const std_msgs::UInt16MultiArray& msg)
 	{
-		per=msg.data[0];
-		servo.setPeriod(msg.data[0]);
-		servo.setWidth(msg.data[1]);
+		if (msg.data_length >= 2){
+			per=msg.data[0];
+			servo.setPeriod(msg.data[0]);
+			servo.setWidth(msg.data[1]);
 #ifdef DEBUG
-		Serial.printf("[servo%dPWMCallback] period: %d width: %d\r\n", num, msg.data[0], msg.data[1]);
+			Serial.printf("[servo%dPWMCallback] period: %d width: %d\r\n", num, msg.data[0], msg.data[1]);
+		} else {
+			Serial.printf("ERROR: [servo%dPWMCallback] data array should have 2 members\r\n", num);
 #endif
+		}
 	}
 };