void send_heartbeat()
{
double maxdepth = 15;
std::stringstream buf;
buf << "CR0\r"; // load factory settings (won't change baud rate)
buf << "#BJ 100 110 000\r"; // enable only bottom track high res velocity and bottom track
// range
// buf << "#BK2\r"; // send water mass pings when bottom track pings fail
// buf << "#BL7,36,46\r"; // configure near layer and far layer to 12 and 15 feet
buf << "ES0\r"; // 0 salinity
buf << "EX00000\r"; // no transformation
buf << "EZ10000010\r"; // configure sensor sources. Provide manual data for everything except
// speed of sound and temperature
buf << "BX" << std::setw(5) << std::setfill('0') << (int)(maxdepth * 10 + 0.5) << '\r'; // configure max depth
buf << "TT2012/03/04, 05:06:07\r"; // set RTC
buf << "CS\r"; // start pinging
std::string str = buf.str();
try // Write heartbeat to serial port
{
size_t written = 0;
while (written < str.size())
{
written += p.write_some(boost::asio::buffer(str.data() + written, str.size() - written));
}
}
catch (const std::exception &exc)
{
ROS_ERROR_THROTTLE(0.5, "DVL: error on write: %s; dropping heartbeat", exc.what());
}
}
void Controls::joy_callback(const sensor_msgs::Joy::ConstPtr& joy)
{
int leftServoAngle = 0;
int rightServoAngle = 0;
int rightMultiplier = 1;
int leftMultiplier = 1;
int leftPower = 0;
int rightPower = 0;
if(joy->buttons[rightTrigger] > 0){
rightMultiplier = 0;
}
if(joy->buttons[leftTrigger] > 0){
leftMultiplier = 0;
}
/* No Servos, because I said so */
if(joy->buttons[xButton] > 0){
leftServoAngle = 90;
rightServoAngle = 90;
}
leftPower = joy->axes[leftStickY] * (100.0 / 127.0) * leftMultiplier;
rightPower = joy->axes[leftStickY] * (100.0 / 127.0) * rightMultiplier;
leftPower = 1500 + leftPower;
rightPower = 1500 + rightPower;
const int SIZE = 11;
unsigned char packet[SIZE];
packet[0] = '-';
packet[1] = leftServoAngle >> 8;
packet[2] = leftServoAngle;
packet[3] = rightServoAngle >> 8;
packet[4] = rightServoAngle;
packet[5] = 0 >> 8;
packet[6] = 0;
packet[7] = leftPower >> 8;
packet[8] = leftPower;
packet[9] = rightPower >> 8;
packet[10] = rightPower;
s_p.write_some(boost::asio::buffer(&packet, SIZE));
}
void callback(const std_msgs::Float32::ConstPtr& angle)
{
unsigned char d = (char)angle->data;
s_p.write_some(boost::asio::buffer(&d, 1));
}