bool Ethernet::sendto(const ifi_info& dev, const char* eth_to_mac, const char* eth_from_mac, uint16_t protocol, const char* data, size_t len)
{
if (-1 == m_sockfd)
{
return false;
}
if (data == NULL)
{
return false;
}
struct sockaddr_ll remote;
bzero(&remote, sizeof(remote));
remote.sll_ifindex = dev.ifi_index;
//other fields not used when send data
// remote.sll_family = htons(AF_PACKET);
// remote.sll_protocol = htons(ETH_P_ARP);
// remote.sll_hatype = htons(ARPHRD_ETHER);
// remote.sll_halen = ETH_ALEN;
// remote.sll_pkttype = PACKET_OTHERHOST;
// memcpy(remote.sll_addr, arp->arp_sha, ETH_ALEN);
uint32_t ethernet_size = sizeof(struct ethhdr) + len;
char* buffer = new char[ethernet_size];
bzero(buffer, ethernet_size);
memcpy(buffer+sizeof(struct ethhdr), data, len);
SocketToolkit toolkit;
struct ethhdr *ethhdr = (struct ethhdr *) buffer;
// memset(ethhdr->h_dest, 0xFF, sizeof(ethhdr->h_dest));
if(!toolkit.toMac(eth_to_mac, ethhdr->h_dest))
{
LOG_ERROR(QObject::tr("The format is not supported: %1").arg(eth_to_mac));
return false;
}
if(!toolkit.toMac(eth_from_mac, ethhdr->h_source))
{
LOG_ERROR(QObject::tr("The format is not supported: %1").arg(eth_from_mac));
return false;
}
ethhdr->h_proto = htons(protocol);
if(protocol == ETH_P_IP) {
addChecksum(buffer+sizeof(struct ethhdr), len);
}
int ret = ::sendto(m_sockfd, buffer, ethernet_size, 0, (struct sockaddr*) &remote,
sizeof(remote));
if (-1 == ret)
{
LOG_ERROR(npg_errno);
delete[] buffer;
return false;
}
delete[] buffer;
return true;
}
char* WPLSentence::get(char str[], size_t buflen) const {
if (str == NULL || buflen < MAX_SENTENCE_LENGTH)
return NULL;
addHead(str);
strcat(str, ",");
char* p = strend(str);
pointToString(waypoint, p);
strcat(p, ",");
strcat(p, name);
return addChecksum(str);
}
int Bisight::sendReceive(char *data, char *reply) {
//check serial port
if (fd < 0)
return 0;
//add checksum
char data2[32];
addChecksum(data, data2);
//autocalculate length
int length = strlen(data2);
// ugly error handling, if write fails then shut down unit
//send command
if (write(fd, data2, length) < length) {
fprintf(stderr,"Error writing to Pan Tilt Unit, disabling\n");
Disconnect();
return 0;
}
//wait for reply
int len = read (fd, buffer, BISIGHT_BUFFER_LEN );
//ROS_INFO("Message received: %s, length: %d", buffer, len);
if (len < 1) {
fprintf(stderr,"Error getting pan-tilt pos\n");
return 0;
}
//check if error was returned
if (buffer[0] == 'E')
fprintf(stderr,"Error response received: %c%c \n", buffer[0], buffer[1]);
buffer[len] = '\0';
//return reply
sprintf(reply, "%s", buffer);
//ROS_INFO("write 3: done");
return 1;
}
char* RSASentence::get(char str[], size_t buflen) const {
if (str == NULL || buflen < MAX_SENTENCE_LENGTH)
return NULL;
addHead(str);
strcat(str, ",");
char* p = strend(str);
ftoa(starboardRudderAngle, p, 1);
strcat(str, ",A,");
p = strend(str);
ftoa(portRudderAngle, p, 1);
strcat(str, ",A");
return addChecksum(str);
}
