added parsed state enum

PX4 · Dec 13, 2024 · 34cb900 · 34cb900
1 parent 0b96092
commit 34cb900
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 28 deletions.
diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
@@ -346,72 +346,71 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
 
 		while (restart_flag != true) {
 			switch (_parsed_state) {
-			case 0: {
+			case SF45_PARSED_STATE::START: {
 					if (_linebuf[index] == 0xAA) {
 						// start of frame is valid, continue
 						_sop_valid = true;
 						_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
-						_parsed_state = 1;
+						_parsed_state = SF45_PARSED_STATE::FLG_LOW;
 						break;
 
 					} else {
 						_sop_valid = false;
 						_crc_valid = false;
-						_parsed_state = 0;
+						_parsed_state = SF45_PARSED_STATE::START;
 						restart_flag = true;
 						_calc_crc = 0;
 						PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
 						break;
-					} // end else
-				} // end case 0
+					}
+				}
 
-			case 1: {
+			case SF45_PARSED_STATE::FLG_LOW: {
 					rx_field.flags_lo = _linebuf[index + 1];
 					_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
-					_parsed_state = 2;
+					_parsed_state = SF45_PARSED_STATE::FLG_HIGH;
 					break;
 				}
 
-			case 2: {
+			case SF45_PARSED_STATE::FLG_HIGH: {
 					rx_field.flags_hi = _linebuf[index + 2];
 					rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6);
 					_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi);
 
 					// Check payload length against known max value
 					if (rx_field.data_len > 17) {
-						_parsed_state = 0;
+						_parsed_state = SF45_PARSED_STATE::START;
 						restart_flag = true;
 						_calc_crc = 0;
 						PX4_DEBUG("Payload length error: %d", _sensor_state);
 						break;
 
 					} else {
-						_parsed_state = 3;
+						_parsed_state = SF45_PARSED_STATE::ID;
 						break;
 					}
 				}
 
-			case 3: {
+			case SF45_PARSED_STATE::ID: {
 					rx_field.msg_id = _linebuf[index + 3];
 
 					if (rx_field.msg_id == msg_id) {
 						_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
-						_parsed_state = 4;
+						_parsed_state = SF45_PARSED_STATE::DATA;
 						break;
 					}
 
 					// Ignore message ID's that aren't searched
 					else {
-						_parsed_state = 0;
+						_parsed_state = SF45_PARSED_STATE::START;
 						_calc_crc = 0;
 						restart_flag = true;
 						PX4_DEBUG("Non needed message ID: %d", _sensor_state);
 						break;
 					}
 				}
 
-			// Data
-			case 4: {
+			case SF45_PARSED_STATE::DATA: {
 					// Process commands with  & w/out data bytes
 					if (rx_field.data_len > 1) {
 						for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) {
@@ -420,30 +419,28 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
 							_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
 							_data_bytes_recv = _data_bytes_recv + 1;
 
-						}  // end for
-					} //end if
+						}
+					}
 
 					else {
 
-						_parsed_state = 5;
+						_parsed_state = SF45_PARSED_STATE::CRC_LOW;
 						_data_bytes_recv = 0;
 						_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
 					}
 
-					_parsed_state = 5;
+					_parsed_state = SF45_PARSED_STATE::CRC_LOW;
 					_data_bytes_recv = 0;
 					break;
 				}
 
-			// CRC low byte
-			case 5: {
+			case SF45_PARSED_STATE::CRC_LOW: {
 					rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len];
-					_parsed_state = 6;
+					_parsed_state = SF45_PARSED_STATE::CRC_HIGH;
 					break;
 				}
 
-			// CRC high byte
-			case 6: {
+			case SF45_PARSED_STATE::CRC_HIGH: {
 					rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len];
 					uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
 
@@ -452,24 +449,24 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
 					// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
 					if (recv_crc == _calc_crc) {
 						_crc_valid = true;
-						_parsed_state = 0;
+						_parsed_state = SF45_PARSED_STATE::START;
 						_calc_crc = 0;
 						restart_flag = true;
 						break;
 
 					} else {
 
 						_crc_valid = false;
-						_parsed_state = 0;
+						_parsed_state = SF45_PARSED_STATE::START;
 						_calc_crc = 0;
 						restart_flag = true;
 						perf_count(_comms_errors);
 						PX4_DEBUG("CRC mismatch: %d", _sensor_state);
 						break;
 					}
 				}
-			} // end switch
-		} //end while
+			}
+		}
 
 		index++;
 	}

diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
@@ -62,6 +62,15 @@ enum SF_SERIAL_STATE {
 	STATE_SEND_STREAM = 4,
 };
 
+enum SF45_PARSED_STATE {
+	START = 0,
+	FLG_LOW,
+	FLG_HIGH,
+	ID,
+	DATA,
+	CRC_LOW,
+	CRC_HIGH
+};
 
 enum SensorOrientation {	  // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
 	ROTATION_FORWARD_FACING = 0,  // MAV_SENSOR_ROTATION_NONE