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 index 22ae7c820703..5d218c0fa397 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -47,9 +47,8 @@ #define SF45_MAX_PAYLOAD 256 #define SF45_SCALE_FACTOR 0.01f -SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : +SF45LaserSerial::SF45LaserSerial(const char *port) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _px4_rangefinder(0, rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { @@ -69,15 +68,19 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : } _num_retries = 2; - _px4_rangefinder.set_device_id(device_id.devid); - _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER); // populate obstacle map members _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + _obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; _obstacle_map_msg.increment = 5; - _obstacle_map_msg.angle_offset = 2.5; - _obstacle_map_msg.min_distance = UINT16_MAX; + _obstacle_map_msg.min_distance = 20; _obstacle_map_msg.max_distance = 5000; + _obstacle_map_msg.angle_offset = 0; + const uint32_t internal_bins = sizeof(_obstacle_map_msg.distances) / sizeof(_obstacle_map_msg.distances[0]); + + for (uint32_t i = 0 ; i < internal_bins; i++) { + _obstacle_map_msg.distances[i] = UINT16_MAX; + } } @@ -91,16 +94,12 @@ SF45LaserSerial::~SF45LaserSerial() int SF45LaserSerial::init() { - param_get(param_find("SF45_UPDATE_CFG"), &_update_rate); param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); /* SF45/B (50M) */ - _px4_rangefinder.set_min_distance(0.2f); - _px4_rangefinder.set_max_distance(50.0f); _interval = 10000; - start(); return PX4_OK; @@ -161,7 +160,6 @@ int SF45LaserSerial::collect() float distance_m = -1.0f; /* read from the sensor (uart buffer) */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); @@ -214,7 +212,6 @@ int SF45LaserSerial::collect() // Stream data from sensor } else { - ret = ::read(_fd, &readbuf[0], 10); if (ret < 0) { @@ -262,7 +259,7 @@ int SF45LaserSerial::collect() } PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO")); - _px4_rangefinder.update(timestamp_sample, distance_m); + perf_end(_sample_perf); @@ -687,8 +684,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) { switch (rx_field.msg_id) { case SF_DISTANCE_DATA_CM: { - - uint16_t obstacle_dist_cm = 0; const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8); int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8)); int16_t scaled_yaw = 0; @@ -700,7 +695,7 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) } // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle - if (_orient_cfg == 1) { + if (_orient_cfg == ROTATION_DOWNWARD_FACING) { raw_yaw = raw_yaw * -1; } @@ -708,10 +703,10 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; switch (_yaw_cfg) { - case 0: + case ROTATION_FORWARD_FACING: break; - case 1: + case ROTATION_BACKWARD_FACING: if (scaled_yaw > 180) { scaled_yaw = scaled_yaw - 180; @@ -721,11 +716,11 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) break; - case 2: + case ROTATION_RIGHT_FACING: scaled_yaw = scaled_yaw + 90; // rotation facing right break; - case 3: + case ROTATION_LEFT_FACING: scaled_yaw = scaled_yaw - 90; // rotation facing left break; @@ -733,9 +728,9 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) break; } - // Convert to meters for rangefinder update + // Convert to meters for the debug message *distance_m = raw_distance * SF45_SCALE_FACTOR; - obstacle_dist_cm = (uint16_t)raw_distance; + _current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist; uint8_t current_bin = sf45_convert_angle(scaled_yaw); @@ -743,15 +738,19 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) if (current_bin != _previous_bin) { + if (_current_bin_dist > _obstacle_map_msg.max_distance) { + _current_bin_dist = _obstacle_map_msg.max_distance + 1; // As per ObstacleDistance.msg definition + } + // update the current bin to the distance sensor reading // readings in cm - _obstacle_map_msg.distances[current_bin] = obstacle_dist_cm; + _obstacle_map_msg.distances[current_bin] = _current_bin_dist; _obstacle_map_msg.timestamp = hrt_absolute_time(); + _current_bin_dist = UINT16_MAX; + _previous_bin = current_bin; } - _previous_bin = current_bin; - _obstacle_distance_pub.publish(_obstacle_map_msg); break; @@ -768,7 +767,7 @@ uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) uint8_t mapped_sector = 0; float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset); - mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment); + mapped_sector = floor(adjusted_yaw / _obstacle_map_msg.increment); return mapped_sector; } 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 index 89e736719262..188e8fb01cde 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -63,10 +63,19 @@ enum SF_SERIAL_STATE { }; + +enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE + ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 + ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180 + ROTATION_LEFT_FACING = 6, // MAV_SENSOR_ROTATION_YAW_270 + ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90 + ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270 +}; class SF45LaserSerial : public px4::ScheduledWorkItem { public: - SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + SF45LaserSerial(const char *port); ~SF45LaserSerial() override; int init(); @@ -77,11 +86,10 @@ class SF45LaserSerial : public px4::ScheduledWorkItem void sf45_process_replies(float *data); uint8_t sf45_convert_angle(const int16_t yaw); float sf45_wrap_360(float f); -protected: - obstacle_distance_s _obstacle_map_msg{}; - uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ private: + obstacle_distance_s _obstacle_map_msg{}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ void start(); void stop(); @@ -89,15 +97,14 @@ class SF45LaserSerial : public px4::ScheduledWorkItem int measure(); int collect(); bool _crc_valid{false}; - PX4Rangefinder _px4_rangefinder; char _port[20] {}; - int _interval{10000}; + int _interval{10000}; bool _collect_phase{false}; int _fd{-1}; - int _linebuf[256] {}; - unsigned _linebuf_index{0}; - hrt_abstime _last_read{0}; + int _linebuf[256] {}; + unsigned _linebuf_index{0}; + hrt_abstime _last_read{0}; // SF45/B uses a binary protocol to include header,flags // message ID, payload, and checksum @@ -118,14 +125,14 @@ class SF45LaserSerial : public px4::ScheduledWorkItem uint8_t _num_retries{0}; int32_t _yaw_cfg{0}; int32_t _orient_cfg{0}; - int32_t _collision_constraint{0}; uint16_t _previous_bin{0}; + uint16_t _current_bin_dist{UINT16_MAX}; // end of SF45/B data members - unsigned _consecutive_fail_count; + unsigned _consecutive_fail_count; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; }; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp index f05417e053bd..3446ad1a359f 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp @@ -41,7 +41,7 @@ namespace lightware_sf45 SF45LaserSerial *g_dev{nullptr}; -static int start(const char *port, uint8_t rotation) +static int start(const char *port) { if (g_dev != nullptr) { PX4_WARN("already started"); @@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation) } /* create the driver */ - g_dev = new SF45LaserSerial(port, rotation); + g_dev = new SF45LaserSerial(port); if (g_dev == nullptr) { return -1; @@ -102,7 +102,7 @@ static int usage() Serial bus driver for the Lightware SF45/b Laser rangefinder. -Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html +Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html ### Examples @@ -116,7 +116,6 @@ Stop driver PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); - PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false); PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); return PX4_OK; } @@ -125,18 +124,13 @@ Stop driver extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[]) { - uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING; const char *device_path = nullptr; int ch; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - break; - case 'd': device_path = myoptarg; break; @@ -153,7 +147,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[]) } if (!strcmp(argv[myoptind], "start")) { - return lightware_sf45::start(device_path, rotation); + return lightware_sf45::start(device_path); } else if (!strcmp(argv[myoptind], "stop")) { return lightware_sf45::stop(); diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml index 6356411fbe43..4dbdf25c9083 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -1,6 +1,6 @@ module_name: Lightware SF45 Rangefinder (serial) serial_config: - - command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV} + - command: lightware_sf45_serial start -d ${SERIAL_DEV} port_config_param: name: SENS_EN_SF45_CFG group: Sensors @@ -41,11 +41,11 @@ parameters: The SF45 mounted facing upward or downward on the frame type: enum values: - 0: Rotation upward - 1: Rotation downward + 24: Rotation upward + 25: Rotation downward reboot_required: true num_instances: 1 - default: 0 + default: 24 SF45_YAW_CFG: description: @@ -55,9 +55,9 @@ parameters: type: enum values: 0: Rotation forward - 1: Rotation backward 2: Rotation right - 3: Rotation left + 4: Rotation backward + 6: Rotation left reboot_required: true num_instances: 1 default: 0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..d1e67a5cd7c9 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -323,7 +323,9 @@ void LoggedTopics::add_sensor_comparison_topics() void LoggedTopics::add_vision_and_avoidance_topics() { add_topic("collision_constraints"); + add_topic_multi("distance_sensor"); add_topic("obstacle_distance_fused"); + add_topic("obstacle_distance"); add_topic("vehicle_mocap_odometry", 30); add_topic("vehicle_trajectory_waypoint", 200); add_topic("vehicle_trajectory_waypoint_desired", 200);