/*!
* \brief read from the chip
* \param serial serial port object
* \param timeout_mS communications timeout in milliseconds
* \return true if successful
*/
bool SonarChipRead(
SerialPort &serial,
unsigned timeout_mS)
{
sbuf[0] = I2C_CMD;
sbuf[1] = 0x41;
CurrentTransmissionType = TRANSMISSION_CHIP_READ;
if (serial.In(sbuf, 3, timeout_mS) > 0)
return true;
else
return false;
}
/*!
* \brief periodically ping each sensor
* \param serial serial port object
* \param timeout_mS communications timeout in milliseconds
* \param range_mm array in which to store range data
*/
void Update(
SerialPort &serial,
unsigned timeout_mS,
int *range_mm)
{
ros::Time t = ros::Time::now();
double time_since_last_ping_sec = (t - LastPinged).toSec();
if ((time_since_last_ping_sec*1000 > ping_separation_mS) && (sensor_address == 0)) {
// issue a ping
PingNextSensor(serial, timeout_mS);
LastPinged = t;
}
else {
// read the time of flight or other returned value
const int bytes = 4;
unsigned char comBuffer[bytes];
int returned_bytes = serial.In(comBuffer, bytes, timeout_mS);
if (returned_bytes > 0) {
unsigned char sensor_address = (unsigned char)(0xe0 + (sensor_index * 2));
switch (CurrentTransmissionType)
{
case TRANSMISSION_PING: {
SonarChipWrite(serial, timeout_mS);
break;
}
case TRANSMISSION_CHIP_WRITE: {
SonarChipRead(serial, timeout_mS);
break;
}
case TRANSMISSION_CHIP_READ: {
break;
}
case TRANSMISSION_GAIN: {
SonarPing(serial, sensor_address, timeout_mS);
break;
}
case TRANSMISSION_SET_ADDRESS: {
ROS_INFO("Sensor address set");
SonarChipWrite(serial, timeout_mS);
transmission_complete = true;
break;
}
case TRANSMISSION_READ: {
if (returned_bytes > 2) {
unsigned int n = (unsigned int)(comBuffer[2] << 8);
if (returned_bytes > 3) n |= comBuffer[3];
int echo_uS = (int)n;
if (echo_uS > 0) {
range_mm[sensor_index] = n * 10 / 58;
// publish the range
usbi2c::sonar_params s;
s.index = sensor_index;
s.range_mm = range_mm[sensor_index];
s.stamp = ros::Time::now();
sonar_pub.publish(s);
// publish filtered range
sensor_msgs::Range sonar_height;
sonar_height.header.stamp = ros::Time::now();
sonar_height.header.frame_id = "uav_base_link";
sonar_height.radiation_type = 2;
sonar_height.field_of_view = 0.0;
sonar_height.min_range = -1000.0;
sonar_height.max_range = 1000.0;
sonar_height.range = cAFilter->run(outlierFilter->run(s.range_mm / 1000.0));
sonar_filtered_pub.publish(sonar_height);
//ROS_INFO("Sensor %d Range mm %d", sensor_index, range_mm[sensor_index]);
}
// send gain limit
SonarGainLimit(serial, sensor_address, timeout_mS);
}
break;
}
}
}
}
}
