added parsed state enum

Author: Claudio-Chies

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -44,7 +44,7 @@
 using namespace time_literals;
 
 /* Configuration Constants */
-#define SF45_SCALE_FACTOR 0.01f
+
 
 SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
@@ -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++;
 	}
@@ -678,7 +675,7 @@ void SF45LaserSerial::_publish_obstacle_msg(hrt_abstime now)
 {
 	// This whole logic is in place because CollisionPrevention, just reads in the last published message on the topic, and not every message,
 	// This can result in messages being misssed if the sensor is publishing at a faster rate than the CollisionPrevention module is reading.
-	for (int i = 0; i < BIN_COUNT; i++) {
+	for (uint8_t i = 0; i < BIN_COUNT; i++) {
 		if (now - _data_timestamps[i] > SF45_MEAS_TIMEOUT) {
 			// If the data is older than SF45_MEAS_TIMEOUT, we discard the value (UINT16_MAX means no valid data)
 			_obstacle_distance.distances[i] = UINT16_MAX;

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -50,7 +50,6 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/topics/obstacle_distance.h>
-#include <uORB/topics/distance_sensor.h>
 
 #include "sf45_commands.h"
 
@@ -62,6 +61,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
@@ -91,8 +99,10 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 private:
 	obstacle_distance_s 			_obstacle_distance{};
 	uORB::Publication<obstacle_distance_s>	_obstacle_distance_pub{ORB_ID(obstacle_distance)};	/**< obstacle_distance publication */
-	static constexpr int BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]);
-	static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms};
+	static constexpr uint8_t 	BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(
+				obstacle_distance_s::distances[0]);
+	static constexpr uint64_t 	SF45_MEAS_TIMEOUT{100_ms};
+	static constexpr float 		SF45_SCALE_FACTOR = 0.01f;
 
 	void				start();
 	void				stop();