j.blanckenburg/rideheight-sensing
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
rideheight-sensing/src/rideheight_sensing.cpp

189 lines
7.3 KiB
C++
Raw Blame History

#include "rideheight_sensing/rideheight_sensing.h"
#include <optional>
#include "vl53l0x_api.h"
#include "vl53l0x_i2c_comms.h"
#include "vl6180x_api.h"
#include "vl6180x_glue.h"
#include <ros/assert.h>
#include <sensor_msgs/Range.h>
namespace rh_sensing {
RideheightSensing::RideheightSensing(ros::NodeHandle nh,
ros::NodeHandle nh_private)
: nh{nh}, nh_private{nh_private},
vl6180x_pub{nh.advertise<sensor_msgs::Range>("vl6180x", 10)},
vl53l0x_pub{nh.advertise<sensor_msgs::Range>("vl53l0x", 10)} {
// init_6180("/dev/i2c-8");
init_53l0("/dev/i2c-8");
}
void RideheightSensing::spin() {
sensor_msgs::Range vl6180x_msg, vl53l0x_msg;
vl6180x_msg.radiation_type = vl53l0x_msg.radiation_type =
sensor_msgs::Range::INFRARED;
vl6180x_msg.field_of_view = vl53l0x_msg.field_of_view = 25.0f / 180 * M_PI;
vl6180x_msg.min_range = 0.005f;
vl6180x_msg.max_range = 0.1f;
vl53l0x_msg.min_range = 0.05f;
vl53l0x_msg.max_range = 1.0f;
ros::Rate loop_rate(20);
std::optional<ros::Time> first_signal_fail = std::nullopt;
while (ros::ok()) {
// VL6180x_RangePollMeasurement(vl6180x_dev, &vl6180x_range);
// if (vl6180x_range.errorStatus == 0) {
// vl6180x_msg.header.stamp = ros::Time::now();
// vl6180x_msg.range = vl6180x_range.range_mm / 1000.0f;
// vl6180x_pub.publish(vl6180x_msg);
// } else {
// ROS_WARN("VL6180X Ranging error: %s",
// VL6180x_RangeGetStatusErrString(vl6180x_range.errorStatus));
// }
VL53L0X_Error status =
VL53L0X_PerformSingleRangingMeasurement(&vl53l0x_dev, &vl53l0x_range);
if (status != VL53L0X_ERROR_NONE) {
char str[512];
ROS_ASSERT(VL53L0X_GetDeviceErrorString(status, str) ==
VL53L0X_ERROR_NONE);
ROS_WARN("VL53L0X Ranging error: %s", str);
} else if (vl53l0x_range.RangeStatus != 0) {
if (!vl53l0x_long_range && vl53l0x_range.RangeStatus == 2) {
// Signal fail
if (first_signal_fail.has_value()) {
if (ros::Time::now() - *first_signal_fail > ros::Duration(1)) {
ROS_WARN("VL53L0X signal has failed for more than 1s, switching to "
"long range mode");
config_53l0(true);
}
} else {
first_signal_fail = ros::Time::now();
}
} else {
char str[512];
ROS_ASSERT(
VL53L0X_GetRangeStatusString(vl53l0x_range.RangeStatus, str) == 0);
ROS_WARN("VL53L0X Ranging status: %s", str);
}
} else {
first_signal_fail = std::nullopt;
vl53l0x_msg.header.stamp = ros::Time::now();
vl53l0x_msg.range = vl53l0x_range.RangeMilliMeter / 1000.0f;
vl53l0x_pub.publish(vl53l0x_msg);
}
ros::spinOnce();
loop_rate.sleep();
}
}
void RideheightSensing::init_6180(const char *i2c_dev_path) {
VL6180x_glue_init(i2c_dev_path);
VL6180x_InitData(vl6180x_dev);
VL6180x_Prepare(vl6180x_dev);
}
void RideheightSensing::init_53l0(const char *i2c_dev_path) {
VL53L0X_i2c_init(i2c_dev_path, &vl53l0x_dev);
// Adapted from Adafruit's VL53L0X library
VL53L0X_Error status = VL53L0X_DataInit(&vl53l0x_dev);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in DataInit: %d", status);
VL53L0X_DeviceInfo_t device_info;
status = VL53L0X_GetDeviceInfo(&vl53l0x_dev, &device_info);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in GetDeviceInfo: %d",
status);
ROS_INFO("[VL53L0X] Device Name: %s, Type: %s, ID: %s", device_info.Name,
device_info.Type, device_info.ProductId);
ROS_INFO("[VL53L0X] Rev: %u.%u", device_info.ProductRevisionMajor,
device_info.ProductRevisionMinor);
status = VL53L0X_StaticInit(&vl53l0x_dev);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in StaticInit: %d",
status);
uint32_t ref_spad_count;
uint8_t is_aperture_spads;
status = VL53L0X_PerformRefSpadManagement(&vl53l0x_dev, &ref_spad_count,
&is_aperture_spads);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in PerformRefSpadManagement: %d", status);
ROS_INFO("Ref spad count: %u, is aperture spads: %u", ref_spad_count,
is_aperture_spads);
uint8_t vhv_settings, phase_cal;
status =
VL53L0X_PerformRefCalibration(&vl53l0x_dev, &vhv_settings, &phase_cal);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in PerformRefCalibration: %d", status);
status =
VL53L0X_SetDeviceMode(&vl53l0x_dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in SetDeviceMode: %d",
status);
// Enable/Disable Sigma and Signal check
status = VL53L0X_SetLimitCheckEnable(
&vl53l0x_dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetLimitCheckEnable: %d", status);
status = VL53L0X_SetLimitCheckEnable(
&vl53l0x_dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetLimitCheckEnable: %d", status);
config_53l0(false);
}
void RideheightSensing::config_53l0(bool long_range) {
this->vl53l0x_long_range = long_range;
if (long_range) {
VL53L0X_Error status = VL53L0X_SetLimitCheckEnable(
&vl53l0x_dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetLimitCheckEnable: %d", status);
status = VL53L0X_SetLimitCheckValue(
&vl53l0x_dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
(FixPoint1616_t)(0.1 * 65536));
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetLimitCheckValue: %d", status);
status = VL53L0X_SetLimitCheckValue(&vl53l0x_dev,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
(FixPoint1616_t)(60 * 65536));
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetLimitCheckValue: %d", status);
status =
VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&vl53l0x_dev, 33000);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetMeasurementTimingBudgetMicroSeconds: %d",
status);
status = VL53L0X_SetVcselPulsePeriod(&vl53l0x_dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetVcselPulsePeriod: %d", status);
status = VL53L0X_SetVcselPulsePeriod(&vl53l0x_dev,
VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetVcselPulsePeriod: %d", status);
} else {
VL53L0X_Error status = VL53L0X_SetLimitCheckEnable(
&vl53l0x_dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1);
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetLimitCheckEnable: %d", status);
status = VL53L0X_SetLimitCheckValue(
&vl53l0x_dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
(FixPoint1616_t)(1.5 * 0.023 * 65536));
ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
"Error in SetLimitCheckEnable: %d", status);
}
}
} // namespace rh_sensing
Reference in New Issue View Git Blame Copy Permalink