void update_mavlink(void){ mavlink_message_t msg; mavlink_status_t status; if(ser.available()){ uint16_t num_bytes = ser.available(); uint8_t buffer[num_bytes]; ser.read(buffer, num_bytes); for(uint16_t i = 0; i < num_bytes; i++){ if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) { handleMessage(&msg, MAVLINK_COMM_0); } } } }
void receive(){ if(!checkConnection()){ return; } // reads: // 3 bytes: // 1 byte to compare with START_BYTE // 1 bytes into _msg->len // 1 byte into _msg->type // len+1 bytes: // len bytes into _msg->data // 1 byte to compare with _msg->checksum size_t amount; while((amount=port.available()) > 0){ if(_pos > 0){ amount = min((unsigned int)amount, _msg->length + 3 - _pos); port.read(&(_msg->data[_pos]), amount); _pos += amount; if(_pos == _msg->length + 3 && port.available()){ uint8_t checksum; port.read(&checksum, 1); _msg->calcChecksum(); if(checksum == _msg->checksum){ parse(_msg); }else{ ROS_WARN_THROTTLE(1, "Serial: Checksum mismatch: %u != %u", checksum, _msg->checksum); } resetMessage(); } }else if(amount >= 3){ uint8_t b; port.read(&b, 1); if(b != START_BYTE){ ROS_WARN_THROTTLE(1, "Serial: Expected start byte, got this instead: %u", b); return; } uint8_t d[2]; port.read(d, 2); uint16_t len = d[0]; if(len > 0){ _msg = new Message(len - 1, d[1]); // TODO catch exception? _pos = 3; } } } }
// Get all bytes available bool getBytes(char * buffer, int& buf_len) { bool res = false; //buf_len = 0; try { while (true) { int res = 0; size_t available = m_serial.available(); if (!available) { break; } else { res = m_serial.read((uint8_t*)buffer + buf_len, available); } if (res <= 0) { break; } buf_len += res; m_last_read_time = Datapoint::get_current_time(); } } catch (...) { if (debug) { std::cout << "exception thrown while wattsup reading" << std::endl; } close(); } if (buf_len > 0) { res = true; } return res; }
int main(int argc, char **argv) { ros::init(argc, argv, "doa_rssi_server"); ros::NodeHandle n2; tf::TransformListener listener; ros::Rate r(10); ros::ServiceServer service = n2.advertiseService("get_doa_rssi", add); ros::Publisher marker_pub_path = n2.advertise<visualization_msgs::Marker>("beacon_point", 10); tf::StampedTransform transformMtoR; tf::StampedTransform transform; string dataString; float doarssi[3]; bool doarssiready = false; //ros::spin();//a blocking call duhh! ROS_INFO("ready to give DOA and RSSI!!"); visualization_msgs::Marker markerDOA,markerIntersections,markerBeacon1,markerBeacon2,markerPath; markerDOA.header.frame_id = "/map"; markerDOA.header.stamp = ros::Time::now(); markerDOA.ns = "markers"; markerDOA.action = visualization_msgs::Marker::ADD; markerDOA.pose.orientation.w = 1.0; markerDOA.id = 0; markerDOA.type = visualization_msgs::Marker::POINTS; //########POINTS markers use x and y scale for width/height respectively markerDOA.scale.x = 1.2; markerDOA.scale.y = 1.2; markerDOA.scale.z = 0.5; markerDOA.color.r = 1.0f; markerDOA.color.b = 1.0f; markerDOA.color.a = 1.0; geometry_msgs::Point marker_point; marker_point.x = 19.0; marker_point.y = 19.0; marker_point.z = 0.0; markerDOA.points.push_back(marker_point); marker_pub_path.publish(markerDOA); while(n2.ok()) { #ifndef SDR try{ listener.lookupTransform("base_link", "beacon", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } phi = atan2f(transform.getOrigin().y(),transform.getOrigin().x()); rssi = 1/(transform.getOrigin().x()*transform.getOrigin().x()+transform.getOrigin().y()*transform.getOrigin().y()); #else if(robotserial.available()) { dataString = robotserial.readline(100,"\n"); cout << "odom received: "<< dataString << endl; char * dataStringArray = new char [dataString.length()+1]; std::strcpy (dataStringArray, dataString.c_str()); //printf("%s\n",dataStringArray); // dataStringArray now contains a c-string copy of str char * token = std::strtok (dataStringArray,","); char *unconverted; int i = 0; if(*token=='t') { while (token!=0) { //std::cout << token << "-" << atoi(token) <<'\n'; doarssi[i] = atof(token); token = std::strtok(NULL,","); i++; } doarssiready = true; } } #endif if(doarssiready==true) { phi = doarssi[1]; rssi = doarssi[2]; doarssiready = false; } ros::spinOnce(); marker_pub_path.publish(markerDOA); } return 0; }