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)); }