diff --git a/src/ld2410.cpp b/src/ld2410.cpp
index fb9c0eb..14d6470 100644
--- a/src/ld2410.cpp
+++ b/src/ld2410.cpp
@@ -84,63 +84,75 @@ bool ld2410::isConnected()
 	{
 		return true;
 	}
-	if(read_frame_() && parse_frame_())	//Try and read a frame if the current reading is too old
+	if(read_frame_())	//Try and read a frame if the current reading is too old
 	{
 		return true;
 	}
 	return false;
 }
 
-
 bool ld2410::read()
 {
 	return read_frame_();
-	return false;
 }
+
 bool ld2410::presenceDetected()
 {
 	return target_type_ != 0;
 }
+
 bool ld2410::stationaryTargetDetected()
 {
-	if(stationary_target_energy_ > 0)
+	if((target_type_ & 0x02) && stationary_target_distance_ > 0 && stationary_target_energy_ > 0)
 	{
 		return true;
 	}
 	return false;
 }
+
 uint16_t ld2410::stationaryTargetDistance()
 {
-	if(stationary_target_energy_ > 0)
+	//if(stationary_target_energy_ > 0)
 	{
 		return stationary_target_distance_;
 	}
-	return 0;
+	//return 0;
 }
+
 uint8_t ld2410::stationaryTargetEnergy()
 {
-	return stationary_target_energy_;
+	//if(stationary_target_distance_ > 0)
+	{
+		return stationary_target_energy_;
+	}
+	//return 0;
 }
 
 bool ld2410::movingTargetDetected()
 {
-	if(moving_target_energy_ > 0)
+	if((target_type_ & 0x01) && moving_target_distance_ > 0 && moving_target_energy_ > 0)
 	{
 		return true;
 	}
 	return false;
 }
+
 uint16_t ld2410::movingTargetDistance()
 {
-	if(moving_target_energy_ > 0)
+	//if(moving_target_energy_ > 0)
 	{
 		return moving_target_distance_;
 	}
-	return 0;
+	//return 0;
 }
+
 uint8_t ld2410::movingTargetEnergy()
 {
-	return moving_target_energy_;
+	//if(moving_target_distance_ > 0)
+	{
+		return moving_target_energy_;
+	}
+	//return 0;
 }
 
 bool ld2410::read_frame_()
@@ -150,73 +162,133 @@ bool ld2410::read_frame_()
 		if(frame_started_ == false)
 		{
 			uint8_t byte_read_ = radar_uart_ -> read();
-			if(byte_read_ == 0xF4 || byte_read_ == 0xFD)
+			if(byte_read_ == 0xF4)
 			{
+				#ifdef LD2410_DEBUG_DATA
+				if(debug_uart_ != nullptr)
+				{
+					debug_uart_->print(F("\nRcvd : 00 "));
+				}
+				#endif
 				radar_data_frame_[radar_data_frame_position_++] = byte_read_;
 				frame_started_ = true;
-				#ifdef LD2410_DEBUG_FRAMES
+				ack_frame_ = false;
+			}
+			else if(byte_read_ == 0xFD)
+			{
+				#ifdef LD2410_DEBUG_COMMANDS
 				if(debug_uart_ != nullptr)
 				{
-					debug_uart_->print(F("\nLD2410 frame started: ."));
+					debug_uart_->print(F("\nRcvd : 00 "));
 				}
 				#endif
+				radar_data_frame_[radar_data_frame_position_++] = byte_read_;
+				frame_started_ = true;
+				ack_frame_ = true;
 			}
 		}
 		else
 		{
 			if(radar_data_frame_position_ < LD2410_MAX_FRAME_LENGTH)
 			{
-				radar_data_frame_[radar_data_frame_position_++] = radar_uart_ -> read();
-				#ifdef LD2410_DEBUG_FRAMES
-				if(debug_uart_ != nullptr)
+				#ifdef LD2410_DEBUG_DATA
+				if(debug_uart_ != nullptr && ack_frame_ == false)
 				{
-					debug_uart_->print('.');
+					if(radar_data_frame_position_ < 0x10)
+					{
+						debug_uart_->print('0');
+					}
+					debug_uart_->print(radar_data_frame_position_, HEX);
+					debug_uart_->print(' ');
 				}
 				#endif
-				if(radar_data_frame_position_ > 7 &&
-					(
-					radar_data_frame_[0]                              == 0xF4 &&	//Data frames
-					radar_data_frame_[1]                              == 0xF3 &&
-					radar_data_frame_[2]                              == 0xF2 &&
-					radar_data_frame_[3]                              == 0xF1 &&
-					radar_data_frame_[radar_data_frame_position_ - 4] == 0xF8 &&
-					radar_data_frame_[radar_data_frame_position_ - 3] == 0xF7 &&
-					radar_data_frame_[radar_data_frame_position_ - 2] == 0xF6 &&
-					radar_data_frame_[radar_data_frame_position_ - 1] == 0xF5
-					) || (
-					radar_data_frame_[0]                              == 0xFD &&	//Command frames
-					radar_data_frame_[1]                              == 0xFC &&
-					radar_data_frame_[2]                              == 0xFB &&
-					radar_data_frame_[3]                              == 0xFA &&
-					radar_data_frame_[radar_data_frame_position_ - 4] == 0x04 &&
-					radar_data_frame_[radar_data_frame_position_ - 3] == 0x03 &&
-					radar_data_frame_[radar_data_frame_position_ - 2] == 0x02 &&
-					radar_data_frame_[radar_data_frame_position_ - 1] == 0x01
-					)
-					)
+				#ifdef LD2410_DEBUG_COMMANDS
+				if(debug_uart_ != nullptr && ack_frame_ == true)
 				{
-					#ifdef LD2410_DEBUG_FRAMES
-					if(debug_uart_ != nullptr)
+					if(radar_data_frame_position_ < 0x10)
 					{
-						debug_uart_->print(F("ended"));
+						debug_uart_->print('0');
 					}
-					#endif
-					if(parse_frame_())
+					debug_uart_->print(radar_data_frame_position_, HEX);
+					debug_uart_->print(' ');
+				}
+				#endif
+				radar_data_frame_[radar_data_frame_position_++] = radar_uart_ -> read();
+				if(radar_data_frame_position_ > 7)	//Can check for start and end
+				{
+					if(	radar_data_frame_[0]                              == 0xF4 &&	//Data frame end state
+						radar_data_frame_[1]                              == 0xF3 &&
+						radar_data_frame_[2]                              == 0xF2 &&
+						radar_data_frame_[3]                              == 0xF1 &&
+						radar_data_frame_[radar_data_frame_position_ - 4] == 0xF8 &&
+						radar_data_frame_[radar_data_frame_position_ - 3] == 0xF7 &&
+						radar_data_frame_[radar_data_frame_position_ - 2] == 0xF6 &&
+						radar_data_frame_[radar_data_frame_position_ - 1] == 0xF5
+					)
 					{
-						frame_started_ = true;
-						radar_data_frame_position_ = 0;
-						return true;
+						if(parse_data_frame_())
+						{
+							#ifdef LD2410_DEBUG_DATA
+							if(debug_uart_ != nullptr)
+							{
+								debug_uart_->print(F("parsed data OK"));
+							}
+							#endif
+							frame_started_ = false;
+							radar_data_frame_position_ = 0;
+							return true;
+						}
+						else
+						{
+							#ifdef LD2410_DEBUG_DATA
+							if(debug_uart_ != nullptr)
+							{
+								debug_uart_->print(F("failed to parse data"));
+							}
+							#endif
+							frame_started_ = false;
+							radar_data_frame_position_ = 0;
+						}
 					}
-					else
+					else if(radar_data_frame_[0]                              == 0xFD &&	//Command frame end state
+							radar_data_frame_[1]                              == 0xFC &&
+							radar_data_frame_[2]                              == 0xFB &&
+							radar_data_frame_[3]                              == 0xFA &&
+							radar_data_frame_[radar_data_frame_position_ - 4] == 0x04 &&
+							radar_data_frame_[radar_data_frame_position_ - 3] == 0x03 &&
+							radar_data_frame_[radar_data_frame_position_ - 2] == 0x02 &&
+							radar_data_frame_[radar_data_frame_position_ - 1] == 0x01
+						)
 					{
-						frame_started_ = false;
-						radar_data_frame_position_ = 0;
+						if(parse_command_frame_())
+						{
+							#ifdef LD2410_DEBUG_COMMANDS
+							if(debug_uart_ != nullptr)
+							{
+								debug_uart_->print(F("parsed command OK"));
+							}
+							#endif
+							frame_started_ = false;
+							radar_data_frame_position_ = 0;
+							return true;
+						}
+						else
+						{
+							#ifdef LD2410_DEBUG_COMMANDS
+							if(debug_uart_ != nullptr)
+							{
+								debug_uart_->print(F("failed to parse command"));
+							}
+							#endif
+							frame_started_ = false;
+							radar_data_frame_position_ = 0;
+						}
 					}
 				}
 			}
 			else
 			{
-				#ifdef LD2410_DEBUG_FRAMES
+				#if defined(LD2410_DEBUG_DATA) || defined(LD2410_DEBUG_COMMANDS)
 				if(debug_uart_ != nullptr)
 				{
 					debug_uart_->print(F("\nLD2410 frame overran"));
@@ -234,466 +306,452 @@ void ld2410::print_frame_()
 {
 	if(debug_uart_ != nullptr)
 	{
-		debug_uart_->print(F("\nFrame: "));
+		if(ack_frame_ == true)
+		{
+			debug_uart_->print(F("\nCmnd : "));
+		}
+		else
+		{
+			debug_uart_->print(F("\nData : "));
+		}
 		for(uint8_t i = 0; i < radar_data_frame_position_ ; i ++)
 		{
+			if(radar_data_frame_[i] < 0x10)
+			{
+				debug_uart_->print('0');
+			}
 			debug_uart_->print(radar_data_frame_[i],HEX);
 			debug_uart_->print(' ');
 		}
 	}
 }
 
-bool ld2410::parse_frame_()
+bool ld2410::parse_data_frame_()
 {
-	if(			radar_data_frame_[0]                              == 0xF4 &&
-				radar_data_frame_[1]                              == 0xF3 &&
-				radar_data_frame_[2]                              == 0xF2 &&
-				radar_data_frame_[3]                              == 0xF1 &&
-				radar_data_frame_[radar_data_frame_position_ - 4] == 0xF8 &&
-				radar_data_frame_[radar_data_frame_position_ - 3] == 0xF7 &&
-				radar_data_frame_[radar_data_frame_position_ - 2] == 0xF6 &&
-				radar_data_frame_[radar_data_frame_position_ - 1] == 0xF5)		//This is a data frame
-	{
-		uint16_t intra_frame_data_length_ = radar_data_frame_[4] + (radar_data_frame_[5] << 8);
-		if(radar_data_frame_position_ == intra_frame_data_length_ + 10)
-		{
-			#ifdef LD2410_DEBUG_FRAMES
+	uint16_t intra_frame_data_length_ = radar_data_frame_[4] + (radar_data_frame_[5] << 8);
+	if(radar_data_frame_position_ == intra_frame_data_length_ + 10)
+	{
+		#ifdef LD2410_DEBUG_DATA
+		if(debug_uart_ != nullptr && ack_frame_ == false)
+		{
+			print_frame_();
+		}
+		#endif
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr && ack_frame_ == true)
+		{
+			print_frame_();
+		}
+		#endif
+		if(radar_data_frame_[6] == 0x01 && radar_data_frame_[7] == 0xAA)	//Engineering mode data
+		{
+			target_type_ = radar_data_frame_[8];
+			#ifdef LD2410_DEBUG_PARSE
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nFrame payload: "));
-				debug_uart_->print(intra_frame_data_length_);
-				debug_uart_->print(F(" bytes"));
+				debug_uart_->print(F("\nEngineering data - "));
+				if(target_type_ == 0x00)
+				{
+					debug_uart_->print(F("no target"));
+				}
+				else if(target_type_ == 0x01)
+				{
+					debug_uart_->print(F("moving target:"));
+				}
+				else if(target_type_ == 0x02)
+				{
+					debug_uart_->print(F("stationary target:"));
+				}
+				else if(target_type_ == 0x03)
+				{
+					debug_uart_->print(F("moving & stationary targets:"));
+				}
 			}
 			#endif
-			if(radar_data_frame_[6] == 0x01 && radar_data_frame_[7] == 0xAA)	//Engineering mode data
+			/*
+			 *
+			 *	To-do support engineering mode
+			 *
+			 */
+		}
+		else if(intra_frame_data_length_ == 13 && radar_data_frame_[6] == 0x02 && radar_data_frame_[7] == 0xAA && radar_data_frame_[17] == 0x55 && radar_data_frame_[18] == 0x00)	//Normal target data
+		{
+			target_type_ = radar_data_frame_[8];
+			//moving_target_distance_ = radar_data_frame_[9] + (radar_data_frame_[10] << 8);
+			stationary_target_distance_ = radar_data_frame_[9] + (radar_data_frame_[10] << 8);
+			stationary_target_energy_ = radar_data_frame_[14];
+			moving_target_energy_ = radar_data_frame_[11];
+			//stationary_target_distance_ = radar_data_frame_[12] + (radar_data_frame_[13] << 8);
+			moving_target_distance_ = radar_data_frame_[15] + (radar_data_frame_[16] << 8);
+			#ifdef LD2410_DEBUG_PARSE
+			if(debug_uart_ != nullptr)
 			{
-				target_type_ = radar_data_frame_[8];
-				#ifdef LD2410_DEBUG_DATA
-				if(debug_uart_ != nullptr)
+				debug_uart_->print(F("\nNormal data - "));
+				if(target_type_ == 0x00)
 				{
-					debug_uart_->print(F("\nEngineering data: "));
-					if(target_type_ == 0x00)
-					{
-						debug_uart_->print(F("no target"));
-					}
-					else if(target_type_ == 0x01)
-					{
-						debug_uart_->print(F("moving target - "));
-					}
-					else if(target_type_ == 0x02)
-					{
-						debug_uart_->print(F("stationary target - "));
-					}
-					else if(target_type_ == 0x03)
-					{
-						debug_uart_->print(F("moving & stationary targets - "));
-					}
+					debug_uart_->print(F("no target"));
 				}
-				#endif
-				/*
-				 *
-				 *	To-do support engineering mode
-				 *
-				 */
-			}
-			else if(intra_frame_data_length_ == 13 && radar_data_frame_[6] == 0x02 && radar_data_frame_[7] == 0xAA && radar_data_frame_[17] == 0x55 && radar_data_frame_[18] == 0x00)	//Normal target data
-			{
-				target_type_ = radar_data_frame_[8];
-				moving_target_distance_ = radar_data_frame_[9] + (radar_data_frame_[10] << 8);
-				moving_target_energy_ = radar_data_frame_[11];
-				stationary_target_distance_ = radar_data_frame_[12] + (radar_data_frame_[13] << 8);
-				stationary_target_energy_ = radar_data_frame_[14];
-				detection_distance_ = radar_data_frame_[15] + (radar_data_frame_[16] << 8);
-				#ifdef LD2410_DEBUG_DATA
-				if(debug_uart_ != nullptr)
+				else if(target_type_ == 0x01)
 				{
-					debug_uart_->print(F("\nNormal data: "));
-					if(target_type_ == 0x00)
-					{
-						debug_uart_->print(F("no target"));
-					}
-					else if(target_type_ == 0x01)
-					{
-						debug_uart_->print(F("moving target - "));
-					}
-					else if(target_type_ == 0x02)
-					{
-						debug_uart_->print(F("stationary target - "));
-					}
-					else if(target_type_ == 0x03)
-					{
-						debug_uart_->print(F("moving & stationary targets - "));
-					}
+					debug_uart_->print(F("moving target:"));
 				}
-				#endif
-				#ifdef LD2410_DEBUG_DATA
-				if(debug_uart_ != nullptr)
+				else if(target_type_ == 0x02)
 				{
-					if(radar_data_frame_[8] & 0x01)
-					{
-						debug_uart_->print(F(" moving at "));
-						debug_uart_->print(moving_target_distance_);
-						debug_uart_->print(F("cm "));
-						debug_uart_->print(F(" power "));
-						debug_uart_->print(moving_target_energy_);
-					}
-					if(radar_data_frame_[8] & 0x02)
-					{
-						debug_uart_->print(F(" stationary at "));
-						debug_uart_->print(stationary_target_distance_);
-						debug_uart_->print(F("cm "));
-						debug_uart_->print(F(" power "));
-						debug_uart_->print(stationary_target_energy_);
-					}
-					if(radar_data_frame_[8] & 0x03)
-					{
-						debug_uart_->print(F(" detection at "));
-						debug_uart_->print(detection_distance_);
-						debug_uart_->print(F("cm"));
-					}
+					debug_uart_->print(F("stationary target:"));
 				}
-				#endif
-				radar_uart_last_packet_ = millis();
-				return true;
-			}
-			else
-			{
-				#ifdef LD2410_DEBUG_FRAMES
-				if(debug_uart_ != nullptr)
+				else if(target_type_ == 0x03)
 				{
-					debug_uart_->print(F("\nUnknown frame type"));
+					debug_uart_->print(F("moving & stationary targets:"));
+				}
+				if(radar_data_frame_[8] & 0x01)
+				{
+					debug_uart_->print(F(" moving at "));
+					debug_uart_->print(moving_target_distance_);
+					debug_uart_->print(F("cm power "));
+					debug_uart_->print(moving_target_energy_);
+				}
+				if(radar_data_frame_[8] & 0x02)
+				{
+					debug_uart_->print(F(" stationary at "));
+					debug_uart_->print(stationary_target_distance_);
+					debug_uart_->print(F("cm power "));
+					debug_uart_->print(stationary_target_energy_);
 				}
-				#endif
-				print_frame_();
 			}
+			#endif
+			radar_uart_last_packet_ = millis();
+			return true;
 		}
 		else
 		{
-			#ifdef LD2410_DEBUG_FRAMES
+			#ifdef LD2410_DEBUG_DATA
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nFrame length unexpected: "));
-				debug_uart_->print(radar_data_frame_position_);
-				debug_uart_->print(F(" not "));
-				debug_uart_->print(intra_frame_data_length_ + 10);
+				debug_uart_->print(F("\nUnknown frame type"));
 			}
 			#endif
+			print_frame_();
 		}
 	}
-	else if(	radar_data_frame_[0]                              == 0xFD &&
-				radar_data_frame_[1]                              == 0xFC &&
-				radar_data_frame_[2]                              == 0xFB &&
-				radar_data_frame_[3]                              == 0xFA &&
-				radar_data_frame_[radar_data_frame_position_ - 4] == 0x04 &&
-				radar_data_frame_[radar_data_frame_position_ - 3] == 0x03 &&
-				radar_data_frame_[radar_data_frame_position_ - 2] == 0x02 &&
-				radar_data_frame_[radar_data_frame_position_ - 1] == 0x01)		//This is an ACK/Command frame
+	else
 	{
-		uint16_t intra_frame_data_length_ = radar_data_frame_[4] + (radar_data_frame_[5] << 8);
-		#ifdef LD2410_DEBUG_ACKS
+		#ifdef LD2410_DEBUG_DATA
 		if(debug_uart_ != nullptr)
 		{
-			print_frame_();
-			debug_uart_->print(F("\nACK frame payload: "));
-			debug_uart_->print(intra_frame_data_length_);
-			debug_uart_->print(F(" bytes"));
+			debug_uart_->print(F("\nFrame length unexpected: "));
+			debug_uart_->print(radar_data_frame_position_);
+			debug_uart_->print(F(" not "));
+			debug_uart_->print(intra_frame_data_length_ + 10);
+		}
+		#endif
+	}
+}
+
+bool ld2410::parse_command_frame_()
+{
+	uint16_t intra_frame_data_length_ = radar_data_frame_[4] + (radar_data_frame_[5] << 8);
+	#ifdef LD2410_DEBUG_COMMANDS
+	if(debug_uart_ != nullptr)
+	{
+		print_frame_();
+		debug_uart_->print(F("\nACK frame payload: "));
+		debug_uart_->print(intra_frame_data_length_);
+		debug_uart_->print(F(" bytes"));
+	}
+	#endif
+	latest_ack_ = radar_data_frame_[6];
+	latest_command_success_ = (radar_data_frame_[8] == 0x00 && radar_data_frame_[9] == 0x00);
+	if(intra_frame_data_length_ == 8 && latest_ack_ == 0xFF)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
+		{
+			debug_uart_->print(F("\nACK for entering configuration mode: "));
 		}
 		#endif
-		latest_ack_ = radar_data_frame_[6];
-		latest_command_success_ = (radar_data_frame_[8] == 0x00 && radar_data_frame_[9] == 0x00);
-		if(intra_frame_data_length_ == 8 && latest_ack_ == 0xFF)
+		if(latest_command_success_)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for entering configuration mode: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
-			{
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("OK"));
-				}
-				#endif
-				return true;
-			}
-			else
+			return true;
+		}
+		else
+		{
+			if(debug_uart_ != nullptr)
 			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
+				debug_uart_->print(F("failed"));
 			}
+			return false;
 		}
-		else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xFE)
+	}
+	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xFE)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			debug_uart_->print(F("\nACK for leaving configuration mode: "));
+		}
+		#endif
+		if(latest_command_success_)
+		{
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for leaving configuration mode: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
-			{
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("OK"));
-				}
-				#endif
-				return true;
-			}
-			else
+			return true;
+		}
+		else
+		{
+			if(debug_uart_ != nullptr)
 			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
+				debug_uart_->print(F("failed"));
 			}
+			return false;
 		}
-		else if(intra_frame_data_length_ == 4 && latest_ack_ == 0x60)
+	}
+	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0x60)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			debug_uart_->print(F("\nACK for setting max values: "));
+		}
+		#endif
+		if(latest_command_success_)
+		{
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for setting max values: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
-			{
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("OK"));
-				}
-				#endif
-				return true;
-			}
-			else
+			return true;
+		}
+		else
+		{
+			if(debug_uart_ != nullptr)
 			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
+				debug_uart_->print(F("failed"));
 			}
+			return false;
+		}
+	}
+	else if(intra_frame_data_length_ == 28 && latest_ack_ == 0x61)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
+		{
+			debug_uart_->print(F("\nACK for current configuration: "));
 		}
-		else if(intra_frame_data_length_ == 28 && latest_ack_ == 0x61)
+		#endif
+		if(latest_command_success_)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for current configuration: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
+			max_gate = radar_data_frame_[11];
+			max_moving_gate = radar_data_frame_[12];
+			max_stationary_gate = radar_data_frame_[13];
+			motion_sensitivity[0] = radar_data_frame_[14];
+			motion_sensitivity[1] = radar_data_frame_[15];
+			motion_sensitivity[2] = radar_data_frame_[16];
+			motion_sensitivity[3] = radar_data_frame_[17];
+			motion_sensitivity[4] = radar_data_frame_[18];
+			motion_sensitivity[5] = radar_data_frame_[19];
+			motion_sensitivity[6] = radar_data_frame_[20];
+			motion_sensitivity[7] = radar_data_frame_[21];
+			motion_sensitivity[8] = radar_data_frame_[22];
+			stationary_sensitivity[0] = radar_data_frame_[23];
+			stationary_sensitivity[1] = radar_data_frame_[24];
+			stationary_sensitivity[2] = radar_data_frame_[25];
+			stationary_sensitivity[3] = radar_data_frame_[26];
+			stationary_sensitivity[4] = radar_data_frame_[27];
+			stationary_sensitivity[5] = radar_data_frame_[28];
+			stationary_sensitivity[6] = radar_data_frame_[29];
+			stationary_sensitivity[7] = radar_data_frame_[30];
+			stationary_sensitivity[8] = radar_data_frame_[31];
+			sensor_idle_time = radar_data_frame_[32];
+			sensor_idle_time += (radar_data_frame_[33] << 8);
+			#ifdef LD2410_DEBUG_COMMANDS
+			if(debug_uart_ != nullptr)
 			{
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
+				debug_uart_->print(F("\nMax gate distance: "));
+				debug_uart_->print(max_gate);
+				debug_uart_->print(F("\nMax motion detecting gate distance: "));
+				debug_uart_->print(max_moving_gate);
+				debug_uart_->print(F("\nMax stationary detecting gate distance: "));
+				debug_uart_->print(max_stationary_gate);
+				debug_uart_->print(F("\nSensitivity per gate"));
+				for(uint8_t i = 0; i < 9; i++)
 				{
-					debug_uart_->print(F("OK"));
+					debug_uart_->print(F("\nGate "));
+					debug_uart_->print(i);
+					debug_uart_->print(F(" ("));
+					debug_uart_->print(i * 0.75);
+					debug_uart_->print('-');
+					debug_uart_->print((i+1) * 0.75);
+					debug_uart_->print(F(" metres) Motion: "));
+					debug_uart_->print(motion_sensitivity[i]);
+					debug_uart_->print(F(" Stationary: "));
+					debug_uart_->print(stationary_sensitivity[i]);
+					
 				}
-				#endif
-				max_gate = radar_data_frame_[11];
-				max_moving_gate = radar_data_frame_[12];
-				max_stationary_gate = radar_data_frame_[13];
-				motion_sensitivity[0] = radar_data_frame_[14];
-				motion_sensitivity[1] = radar_data_frame_[15];
-				motion_sensitivity[2] = radar_data_frame_[16];
-				motion_sensitivity[3] = radar_data_frame_[17];
-				motion_sensitivity[4] = radar_data_frame_[18];
-				motion_sensitivity[5] = radar_data_frame_[19];
-				motion_sensitivity[6] = radar_data_frame_[20];
-				motion_sensitivity[7] = radar_data_frame_[21];
-				motion_sensitivity[8] = radar_data_frame_[22];
-				stationary_sensitivity[0] = radar_data_frame_[23];
-				stationary_sensitivity[1] = radar_data_frame_[24];
-				stationary_sensitivity[2] = radar_data_frame_[25];
-				stationary_sensitivity[3] = radar_data_frame_[26];
-				stationary_sensitivity[4] = radar_data_frame_[27];
-				stationary_sensitivity[5] = radar_data_frame_[28];
-				stationary_sensitivity[6] = radar_data_frame_[29];
-				stationary_sensitivity[7] = radar_data_frame_[30];
-				stationary_sensitivity[8] = radar_data_frame_[31];
-				sensor_idle_time = radar_data_frame_[32];
-				sensor_idle_time += (radar_data_frame_[33] << 8);
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("\nMax gate distance: "));
-					debug_uart_->print(max_gate);
-					debug_uart_->print(F("\nMax motion detecting gate distance: "));
-					debug_uart_->print(max_moving_gate);
-					debug_uart_->print(F("\nMax stationary detecting gate distance: "));
-					debug_uart_->print(max_stationary_gate);
-					debug_uart_->print(F("\nSensitivity per gate"));
-					for(uint8_t i = 0; i < 9; i++)
-					{
-						debug_uart_->print(F("\nGate "));
-						debug_uart_->print(i);
-						debug_uart_->print(F(" ("));
-						debug_uart_->print(i * 0.75);
-						debug_uart_->print('-');
-						debug_uart_->print((i+1) * 0.75);
-						debug_uart_->print(F(" metres) Motion: "));
-						debug_uart_->print(motion_sensitivity[i]);
-						debug_uart_->print(F(" Stationary: "));
-						debug_uart_->print(stationary_sensitivity[i]);
-						
-					}
-					debug_uart_->print(F("\nSensor idle timeout: "));
-					debug_uart_->print(sensor_idle_time);
-					debug_uart_->print('s');
-				}
-				#endif
-				return true;
+				debug_uart_->print(F("\nSensor idle timeout: "));
+				debug_uart_->print(sensor_idle_time);
+				debug_uart_->print('s');
 			}
-			else
+			#endif
+			return true;
+		}
+		else
+		{
+			if(debug_uart_ != nullptr)
 			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
+				debug_uart_->print(F("failed"));
 			}
+			return false;
+		}
+	}
+	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0x64)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
+		{
+			debug_uart_->print(F("\nACK for setting sensitivity values: "));
 		}
-		else if(intra_frame_data_length_ == 4 && latest_ack_ == 0x64)
+		#endif
+		if(latest_command_success_)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for setting sensitivity values: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
-			{
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("OK"));
-				}
-				#endif
-				return true;
-			}
-			else
+			return true;
+		}
+		else
+		{
+			if(debug_uart_ != nullptr)
 			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
+				debug_uart_->print(F("failed"));
 			}
+			return false;
+		}
+	}
+	else if(intra_frame_data_length_ == 12 && latest_ack_ == 0xA0)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
+		{
+			debug_uart_->print(F("\nACK for firmware version: "));
 		}
-		else if(intra_frame_data_length_ == 12 && latest_ack_ == 0xA0)
+		#endif
+		if(latest_command_success_)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			firmware_major_version = radar_data_frame_[13];
+			firmware_minor_version = radar_data_frame_[12];
+			firmware_bugfix_version = radar_data_frame_[14];
+			firmware_bugfix_version += radar_data_frame_[15]<<8;
+			firmware_bugfix_version += radar_data_frame_[16]<<16;
+			firmware_bugfix_version += radar_data_frame_[17]<<24;
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for firmware version: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
-			{
-				firmware_major_version = radar_data_frame_[13];
-				firmware_minor_version = radar_data_frame_[12];
-				firmware_bugfix_version = radar_data_frame_[14];
-				firmware_bugfix_version += radar_data_frame_[15]<<8;
-				firmware_bugfix_version += radar_data_frame_[16]<<16;
-				firmware_bugfix_version += radar_data_frame_[17]<<24;
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("OK"));
-				}
-				#endif
-				return true;
-			}
-			else
+			return true;
+		}
+		else
+		{
+			if(debug_uart_ != nullptr)
 			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
+				debug_uart_->print(F("failed"));
 			}
+			return false;
 		}
-		else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xA2)
+	}
+	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xA2)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
+		{
+			debug_uart_->print(F("\nACK for factory reset: "));
+		}
+		#endif
+		if(latest_command_success_)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for factory reset: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
-			{
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("OK"));
-				}
-				#endif
-				return true;
-			}
-			else
+			return true;
+		}
+		else
+		{
+			if(debug_uart_ != nullptr)
 			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
+				debug_uart_->print(F("failed"));
 			}
+			return false;
 		}
-		else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xA3)
+	}
+	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xA3)
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
+		{
+			debug_uart_->print(F("\nACK for restart: "));
+		}
+		#endif
+		if(latest_command_success_)
 		{
-			#ifdef LD2410_DEBUG_ACKS
+			radar_uart_last_packet_ = millis();
+			#ifdef LD2410_DEBUG_COMMANDS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nACK for restart: "));
+				debug_uart_->print(F("OK"));
 			}
 			#endif
-			if(latest_command_success_)
-			{
-				radar_uart_last_packet_ = millis();
-				#ifdef LD2410_DEBUG_ACKS
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("OK"));
-				}
-				#endif
-				return true;
-			}
-			else
-			{
-				if(debug_uart_ != nullptr)
-				{
-					debug_uart_->print(F("failed"));
-				}
-				return false;
-			}
+			return true;
 		}
 		else
 		{
-			#ifdef LD2410_DEBUG_ACKS
 			if(debug_uart_ != nullptr)
 			{
-				debug_uart_->print(F("\nUnknown ACK"));
+				debug_uart_->print(F("failed"));
 			}
-			#endif
+			return false;
 		}
 	}
-	return false;
+	else
+	{
+		#ifdef LD2410_DEBUG_COMMANDS
+		if(debug_uart_ != nullptr)
+		{
+			debug_uart_->print(F("\nUnknown ACK"));
+		}
+		#endif
+	}
 }
 
 void ld2410::send_command_preamble_()
@@ -713,6 +771,7 @@ void ld2410::send_command_postamble_()
 	radar_uart_->write(char(0x02));
 	radar_uart_->write(char(0x01));
 }
+
 bool ld2410::enter_configuration_mode_()
 {
 	send_command_preamble_();
@@ -727,14 +786,17 @@ bool ld2410::enter_configuration_mode_()
 	radar_uart_last_command_ = millis();
 	while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 	{
-		read_frame_();
-		if(latest_ack_ == 0xFF && latest_command_success_)
+		if(read_frame_())
 		{
-			return true;
+			if(latest_ack_ == 0xFF && latest_command_success_)
+			{
+				return true;
+			}
 		}
 	}
 	return false;
 }
+
 bool ld2410::leave_configuration_mode_()
 {
 	send_command_preamble_();
@@ -747,14 +809,17 @@ bool ld2410::leave_configuration_mode_()
 	radar_uart_last_command_ = millis();
 	while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 	{
-		read_frame_();
-		if(latest_ack_ == 0xFE && latest_command_success_)
+		if(read_frame_())
 		{
-			return true;
+			if(latest_ack_ == 0xFE && latest_command_success_)
+			{
+				return true;
+			}
 		}
 	}
 	return false;
 }
+
 bool ld2410::requestStartEngineeringMode()
 {
 	send_command_preamble_();
@@ -767,14 +832,17 @@ bool ld2410::requestStartEngineeringMode()
 	radar_uart_last_command_ = millis();
 	while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 	{
-		read_frame_();
-		if(latest_ack_ == 0x62 && latest_command_success_)
+		if(read_frame_())
 		{
-			return true;
+			if(latest_ack_ == 0x62 && latest_command_success_)
+			{
+				return true;
+			}
 		}
 	}
 	return false;
 }
+
 bool ld2410::requestEndEngineeringMode()
 {
 	send_command_preamble_();
@@ -787,14 +855,17 @@ bool ld2410::requestEndEngineeringMode()
 	radar_uart_last_command_ = millis();
 	while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 	{
-		read_frame_();
-		if(latest_ack_ == 0x63 && latest_command_success_)
+		if(read_frame_())
 		{
-			return true;
+			if(latest_ack_ == 0x63 && latest_command_success_)
+			{
+				return true;
+			}
 		}
 	}
 	return false;
 }
+
 bool ld2410::requestCurrentConfiguration()
 {
 	if(enter_configuration_mode_())
@@ -810,12 +881,14 @@ bool ld2410::requestCurrentConfiguration()
 		radar_uart_last_command_ = millis();
 		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 		{
-			read_frame_();
-			if(latest_ack_ == 0x61 && latest_command_success_)
+			if(read_frame_())
 			{
-				delay(50);
-				leave_configuration_mode_();
-				return true;
+				if(latest_ack_ == 0x61 && latest_command_success_)
+				{
+					delay(50);
+					leave_configuration_mode_();
+					return true;
+				}
 			}
 		}
 	}
@@ -823,6 +896,7 @@ bool ld2410::requestCurrentConfiguration()
 	leave_configuration_mode_();
 	return false;
 }
+
 bool ld2410::requestFirmwareVersion()
 {
 	if(enter_configuration_mode_())
@@ -851,6 +925,7 @@ bool ld2410::requestFirmwareVersion()
 	leave_configuration_mode_();
 	return false;
 }
+
 bool ld2410::requestRestart()
 {
 	if(enter_configuration_mode_())
@@ -866,12 +941,14 @@ bool ld2410::requestRestart()
 		radar_uart_last_command_ = millis();
 		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 		{
-			read_frame_();
-			if(latest_ack_ == 0xA3 && latest_command_success_)
+			if(read_frame_())
 			{
-				delay(50);
-				leave_configuration_mode_();
-				return true;
+				if(latest_ack_ == 0xA3 && latest_command_success_)
+				{
+					delay(50);
+					leave_configuration_mode_();
+					return true;
+				}
 			}
 		}
 	}
@@ -879,6 +956,7 @@ bool ld2410::requestRestart()
 	leave_configuration_mode_();
 	return false;
 }
+
 bool ld2410::requestFactoryReset()
 {
 	if(enter_configuration_mode_())
@@ -894,12 +972,14 @@ bool ld2410::requestFactoryReset()
 		radar_uart_last_command_ = millis();
 		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 		{
-			read_frame_();
-			if(latest_ack_ == 0xA2 && latest_command_success_)
+			if(read_frame_())
 			{
-				delay(50);
-				leave_configuration_mode_();
-				return true;
+				if(latest_ack_ == 0xA2 && latest_command_success_)
+				{
+					delay(50);
+					leave_configuration_mode_();
+					return true;
+				}
 			}
 		}
 	}
@@ -940,12 +1020,14 @@ bool ld2410::setMaxValues(uint16_t moving, uint16_t stationary, uint16_t inactiv
 		radar_uart_last_command_ = millis();
 		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 		{
-			read_frame_();
-			if(latest_ack_ == 0x60 && latest_command_success_)
+			if(read_frame_())
 			{
-				delay(50);
-				leave_configuration_mode_();
-				return true;
+				if(latest_ack_ == 0x60 && latest_command_success_)
+				{
+					delay(50);
+					leave_configuration_mode_();
+					return true;
+				}
 			}
 		}
 	}
@@ -986,12 +1068,14 @@ bool ld2410::setGateSensitivityThreshold(uint8_t gate, uint8_t moving, uint8_t s
 		radar_uart_last_command_ = millis();
 		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
 		{
-			read_frame_();
-			if(latest_ack_ == 0x64 && latest_command_success_)
+			if(read_frame_())
 			{
-				delay(50);
-				leave_configuration_mode_();
-				return true;
+				if(latest_ack_ == 0x64 && latest_command_success_)
+				{
+					delay(50);
+					leave_configuration_mode_();
+					return true;
+				}
 			}
 		}
 	}
diff --git a/src/ld2410.h b/src/ld2410.h
index ee41b5e..3bd0aac 100644
--- a/src/ld2410.h
+++ b/src/ld2410.h
@@ -15,9 +15,9 @@
 #include <Arduino.h>
 
 #define LD2410_MAX_FRAME_LENGTH 40
-//#define LD2410_DEBUG_FRAMES
-#define LD2410_DEBUG_ACKS
 //#define LD2410_DEBUG_DATA
+#define LD2410_DEBUG_COMMANDS
+//#define LD2410_DEBUG_PARSE
 
 class ld2410	{
 
@@ -65,16 +65,17 @@ class ld2410	{
 		uint8_t radar_data_frame_[LD2410_MAX_FRAME_LENGTH];				//Store the incoming data from the radar, to check it's in a valid format
 		uint8_t radar_data_frame_position_ = 0;							//Where in the frame we are currently writing
 		bool frame_started_ = false;									//Whether a frame is currently being read
+		bool ack_frame_ = false;										//Whether the incoming frame is LIKELY an ACK frame
 		bool waiting_for_ack_ = false;									//Whether a command has just been sent
 		uint8_t target_type_ = 0;
 		uint16_t moving_target_distance_ = 0;
 		uint8_t moving_target_energy_ = 0;
 		uint16_t stationary_target_distance_ = 0;
 		uint8_t stationary_target_energy_ = 0;
-		uint16_t detection_distance_ = 0;
 		
 		bool read_frame_();												//Try to read a frame from the UART
-		bool parse_frame_();											//Is the current frame valid
+		bool parse_data_frame_();										//Is the current data frame valid?
+		bool parse_command_frame_();									//Is the current command frame valid?
 		void print_frame_();											//Print the frame for debugging
 		void send_command_preamble_();									//Commands have the same preamble
 		void send_command_postamble_();									//Commands have the same postamble