uint8_t GPS_MTK3339::parseSentence(){
GPS_DEBUG_READ_PRINTLN("PARSING");
_machineState = GPS_MS_READY;
#ifdef GPS_DEBUG_READ_PRINT_BUFFER
GPS_DEBUG_READ_PRINTLN((char *)Buffer);
#endif
uint8_t sentenceName = getsentenceName();
uint8_t startByte = 0;
uint8_t stopByte = startByte;
uint8_t element = 0;
bool elementParseSuccess = false;
for(uint8_t i = 0; i < sizeof(Buffer); i++){
uint8_t b = Buffer[i];
// ',' or '*'
if(b == GPS_ELEMENT_DELIM || b == GPS_CHKSUM_DELIM){
switch(sentenceName){
case GPS_STATUS_SUCCESS_RMC:
elementParseSuccess = parseRmc(element, startByte, stopByte);
break;
case GPS_STATUS_SUCCESS_GGA:
elementParseSuccess = parseGga(element, startByte, stopByte);
break;
default:
elementParseSuccess = true;
break;
}
if(elementParseSuccess == false){
GPS_DEBUG_READ_PRINTLN("element parse error");
_machineState = GPS_MS_ERROR;
return GPS_ERROR_PARSE_ERR;
}
if(b == GPS_CHKSUM_DELIM){
if(verifyChecksum(i)){
loadValues();
return sentenceName;
}
GPS_DEBUG_READ_PRINTLN("invalid checksum");
_machineState = GPS_MS_ERROR;
return GPS_ERROR_CHECKSUM;
}
startByte = i + 1;
stopByte = startByte;
element++;
}
else{
stopByte++;
}
}
GPS_DEBUG_READ_PRINTLN("unexpected end of parse");
_machineState = GPS_MS_ERROR;
return GPS_ERROR_PARSE_ERR;
}
/*!
* \brief Unpack pressure sensor samples from realtime data.
*
* \return True, if there are no problems, false if there is something wrong with the data.
*/
bool WG06::unpackPressure(unsigned char *pressure_buf)
{
if (!enable_pressure_sensor_)
{
// If pressure sensor is not enabled don't attempt to do anything with pressure data
return true;
}
if (!verifyChecksum(pressure_buf, pressure_size_))
{
++pressure_checksum_error_count_;
if (false /* debugging */)
{
std::stringstream ss;
ss << "Pressure buffer checksum error : " << std::endl;
for (unsigned ii=0; ii<pressure_size_; ++ii)
{
ss << std::uppercase << std::hex << std::setw(2) << std::setfill('0')
<< unsigned(pressure_buf[ii]) << " ";
if ((ii%8) == 7) ss << std::endl;
}
ROS_ERROR_STREAM(ss.str());
std::cerr << ss.str() << std::endl;
}
pressure_checksum_error_ = true;
return false;
}
else
{
WG06Pressure *p( (WG06Pressure *) pressure_buf);
for (int i = 0; i < 22; ++i ) {
pressure_sensors_[0].state_.data_[i] =
((p->l_finger_tip_[i] >> 8) & 0xff) |
((p->l_finger_tip_[i] << 8) & 0xff00);
pressure_sensors_[1].state_.data_[i] =
((p->r_finger_tip_[i] >> 8) & 0xff) |
((p->r_finger_tip_[i] << 8) & 0xff00);
}
if (p->timestamp_ != last_pressure_time_)
{
if (pressure_publisher_ && pressure_publisher_->trylock())
{
pressure_publisher_->msg_.header.stamp = ros::Time::now();
pressure_publisher_->msg_.l_finger_tip.resize(22);
pressure_publisher_->msg_.r_finger_tip.resize(22);
for (int i = 0; i < 22; ++i ) {
pressure_publisher_->msg_.l_finger_tip[i] = pressure_sensors_[0].state_.data_[i];
pressure_publisher_->msg_.r_finger_tip[i] = pressure_sensors_[1].state_.data_[i];
}
pressure_publisher_->unlockAndPublish();
}
}
last_pressure_time_ = p->timestamp_;
}
return true;
}
bool WG06::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
{
bool rv = true;
int status_bytes =
has_accel_and_ft_ ? sizeof(WG06StatusWithAccelAndFT) : // Has FT sensor and accelerometer
accel_publisher_ ? sizeof(WG06StatusWithAccel) :
sizeof(WG0XStatus);
unsigned char *pressure_buf = (this_buffer + command_size_ + status_bytes);
unsigned char* this_status = this_buffer + command_size_;
if (!verifyChecksum(this_status, status_bytes))
{
status_checksum_error_ = true;
rv = false;
goto end;
}
if (!unpackPressure(pressure_buf))
{
rv = false;
//goto end; // all other tasks should not be effected by bad pressure sensor data
}
if (accel_publisher_)
{
WG06StatusWithAccel *status = (WG06StatusWithAccel *)(this_buffer + command_size_);
WG06StatusWithAccel *last_status = (WG06StatusWithAccel *)(prev_buffer + command_size_);
if (!unpackAccel(status, last_status))
{
rv=false;
}
}
if (has_accel_and_ft_ && enable_ft_sensor_)
{
WG06StatusWithAccelAndFT *status = (WG06StatusWithAccelAndFT *)(this_buffer + command_size_);
WG06StatusWithAccelAndFT *last_status = (WG06StatusWithAccelAndFT *)(prev_buffer + command_size_);
if (!unpackFT(status, last_status))
{
rv = false;
}
}
if (!WG0X::unpackState(this_buffer, prev_buffer))
{
rv = false;
}
end:
return rv;
}
void RemoteBlockReader::readNextPacket() {
assert(position >= size);
lastHeader = readPacketHeader();
int dataSize = lastHeader->getDataLen();
int64_t pendingAhead = 0;
if (!lastHeader->sanityCheck(lastSeqNo)) {
THROW(HdfsIOException, "RemoteBlockReader: Packet failed on sanity check for block %s from Datanode %s.",
binfo.toString().c_str(), datanode.formatAddress().c_str());
}
assert(dataSize > 0 || lastHeader->getPacketLen() == sizeof(int32_t));
if (dataSize > 0) {
int chunks = (dataSize + chunkSize - 1) / chunkSize;
int checksumLen = chunks * checksumSize;
size = checksumLen + dataSize;
assert(size == lastHeader->getPacketLen() - static_cast<int>(sizeof(int32_t)));
buffer.resize(size);
in->readFully(&buffer[0], size, readTimeout);
lastSeqNo = lastHeader->getSeqno();
if (lastHeader->getPacketLen() != static_cast<int>(sizeof(int32_t)) + dataSize + checksumLen) {
THROW(HdfsIOException, "Invalid Packet, packetLen is %d, dataSize is %d, checksum size is %d",
lastHeader->getPacketLen(), dataSize, checksumLen);
}
if (verify) {
verifyChecksum(chunks);
}
/*
* skip checksum
*/
position = checksumLen;
/*
* the first packet we get may start at the position before we required
*/
pendingAhead = cursor - lastHeader->getOffsetInBlock();
pendingAhead = pendingAhead > 0 ? pendingAhead : 0;
position += pendingAhead;
}
/*
* we reach the end of the range we required, send status to datanode
* if datanode do not sending data anymore.
*/
if (cursor + dataSize - pendingAhead >= endOffset && readTrailingEmptyPacket()) {
sendStatus();
}
}
bool WG021::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
{
bool rv = true;
pr2_hardware_interface::ProjectorState &state = projector_.state_;
WG021Status *this_status, *prev_status;
this_status = (WG021Status *)(this_buffer + command_size_);
prev_status = (WG021Status *)(prev_buffer + command_size_);
if (!verifyChecksum(this_status, status_size_))
{
status_checksum_error_ = true;
rv = false;
goto end;
}
digital_out_.state_.data_ = this_status->digital_out_;
state.timestamp_us_ = this_status->timestamp_;
state.falling_timestamp_us_ = this_status->output_stop_timestamp_;
state.rising_timestamp_us_ = this_status->output_start_timestamp_;
state.output_ = (this_status->output_status_ & 0x1) == 0x1;
state.falling_timestamp_valid_ = (this_status->output_status_ & 0x8) == 0x8;
state.rising_timestamp_valid_ = (this_status->output_status_ & 0x4) == 0x4;
state.A_ = ((this_status->config0_ >> 4) & 0xf);
state.B_ = ((this_status->config0_ >> 0) & 0xf);
state.I_ = ((this_status->config1_ >> 4) & 0xf);
state.M_ = ((this_status->config1_ >> 0) & 0xf);
state.L1_ = ((this_status->config2_ >> 4) & 0xf);
state.L0_ = ((this_status->config2_ >> 0) & 0xf);
state.pulse_replicator_ = (this_status->general_config_ & 0x1) == 0x1;
state.last_executed_current_ = this_status->programmed_current_ * config_info_.nominal_current_scale_;
state.last_measured_current_ = this_status->measured_current_ * config_info_.nominal_current_scale_;
state.max_current_ = max_current_;
max_board_temperature_ = max(max_board_temperature_, this_status->board_temperature_);
max_bridge_temperature_ = max(max_bridge_temperature_, this_status->bridge_temperature_);
if (!verifyState((WG0XStatus *)(this_buffer + command_size_), (WG0XStatus *)(prev_buffer + command_size_)))
{
rv = false;
}
end:
return rv;
}
uint8_t getData(dht11Data_t* recvData) {
DHTPOUT &= ~DHTPIN;
initGlobals();
// Config Timer
TB0CCTL0 = CCIE; // TRCCR0 interrupt enabled
TB0CCR0 = 54000; // 18ms @ 3MHz
TB0CTL = TBSSEL_2 | ID_2 | MC_1 | TBCLR; // SMCLK/4 = 3MHz, upmode, clear TBR
DHTPDIR |= DHTPIN; // set to output
__bis_SR_register(LPM0_bits + GIE);
recvData->rawData = receivedData;
return verifyChecksum(recvData); // data is only valid if checkSum is OK
}
bool pAnswer::setDatagram(char* _data, int _len)
{
if(!verifyChecksum(_data))
return false;
id = _data[2];
sid = *(ushort*)(_data + 15);
datetime.setDate(sdt.inttodate(get_date(_data)));
datetime.setTime(sdt.inttotime(get_time(_data)));
int size = get_size(_data);
pVariable tvar;
int cur_ptr = 17, len = 0;
for(; size > 0; size -= len) {
tvar.setStr(_data + cur_ptr, &len);
cur_ptr += len;
addVariable(tvar);
}
return true;
}
Error QTar::extract()
{
//ifstream ofstream to seekg()
QArchive::extract();
if(!exists()) return Archive::OpenError;
char buff[Header::RecordSize];
//QFile outFile;
//FILE* f;
size_t bytes_read;
unsigned int filesize;
#if ARCREADER_QT4
if(!open(QIODevice::ReadOnly)) {
error();
#else
if(open(IO_ReadOnly)) {
qDebug("open error");
#endif //ARCREADER_QT4
return Archive::OpenError;
}
Q_D(QArchive);
for (;;) {
#if ARCREADER_QT4
bytes_read = read(buff,Header::RecordSize);
#else
bytes_read = readBlock(buff,Header::RecordSize);
#endif //ARCREADER_QT4
//put them here
emit byteProcessed(d->processedSize+=Header::RecordSize);
d->current_fileName=QFileInfo(buff).fileName();
if (bytes_read < Header::RecordSize) {
fprintf(stderr,"Short read. expected 512, got %d\n", bytes_read);
return Archive::ReadError;
}
if (isEndBuff(buff)) {
#if USE_SLOT
emit byteProcessed(d->processedSize+=Header::RecordSize); //header;
#else
estimate();
progressHandler->Progress(d->current_fileName, d->size, d->processedSize+=Header::RecordSize, d->totalSize, d->speed, d->elapsed, d->left);
#endif
finishMessage();
return End;
}
if (!verifyChecksum(buff)) {
fprintf(stderr, "Checksum failure\n");
return ChecksumError;
}
switch (buff[156]) {
case Header::LinkFlag::kLink : printf(" Ignoring hardlink %s\n", buff); break;
case Header::LinkFlag::kSymbolicLink : printf(" Ignoring symlink %s\n", buff); break; /////////////////////////
case Header::LinkFlag::kCharacter: printf(" Ignoring character device %s\n", buff); break;
case Header::LinkFlag::kBlock: printf(" Ignoring block device %s\n", buff); break;
case Header::LinkFlag::kDirectory:
createDir(QString::fromLocal8Bit(buff), parseOct(buff + 100, 8));
filesize = 0;
break;
case Header::LinkFlag::kFIFO: printf(" Ignoring FIFO %s\n", buff); break;
default:
createFile(QString::fromLocal8Bit(buff), parseOct(buff + 100, 8));
break;
}
++d->numFiles;
filesize = parseOct(buff + 124, 12);
d->size = filesize;
#if USE_SLOT
updateMessage();
#endif
while (filesize > 0) {
checkTryPause();
#if ARCREADER_QT4
bytes_read = read(buff,Header::RecordSize);
#else
bytes_read = readBlock(buff,Header::RecordSize);
#endif //ARCREADER_QT4
if (bytes_read < Header::RecordSize) {
fprintf(stderr,"Short read. Expected 512, got %d\n",bytes_read);
return Archive::ReadError;
}
if (filesize < Header::RecordSize) bytes_read = filesize;
if (d->outFile.isOpen()) {
#if CONFIG_QT4
if(d->outFile.write(buff,bytes_read)!=bytes_read) {
fprintf(stderr, "[%s] %s @%d: Failed to write %s\n",__FILE__,__PRETTY_FUNCTION__,__LINE__,qPrintable(d->outFile.fileName()));
#else
if(d->outFile.writeBlock(buff,bytes_read)!=bytes_read) {
fprintf(stderr, "[%s] %s @%d: Failed to write %s\n",__FILE__,__PRETTY_FUNCTION__,__LINE__,qPrintable(d->outFile.name()));
#endif
d->outFile.close();
}
/*if (fwrite(buff, 1, bytes_read, f)!= bytes_read) {
fprintf(stderr, "Failed write\n");
fclose(f);
f = NULL;
}*/
}
#if USE_SLOT
forceShowMessage(1000);
emit byteProcessed(d->processedSize+=Header::RecordSize);//bytes_read);
#else
estimate();
progressHandler->Progress(d->current_fileName, d->size, d->processedSize+=Header::RecordSize, d->totalSize, d->speed, d->elapsed, d->left);
#endif
filesize -= bytes_read;
}
//emit byteProcessed(processedSize+=size);
if(d->outFile.isOpen()) d->outFile.close();
}
close();
}
Archive::Error QTar::extract(const QString& archive,const QString& dir)
{
setArchive(archive);
setOutDir(dir);
return extract();
}
bool TeleClientUDP::notify(byte event, const char* payload)
{
char buf[48];
CStorageRAM netbuf;
netbuf.init(128);
netbuf.header(devid);
byte len = sprintf(buf, "EV=%X", (unsigned int)event);
netbuf.dispatch(buf, len);
len = sprintf(buf, "TS=%lu", millis());
netbuf.dispatch(buf, len);
len = sprintf(buf, "ID=%s", devid);
netbuf.dispatch(buf, len);
if (vin[0]) {
len = sprintf(buf, "VIN=%s", vin);
netbuf.dispatch(buf, len);
}
if (payload) {
netbuf.dispatch(payload, strlen(payload));
}
netbuf.tailer();
//Serial.println(netbuf.buffer());
for (byte attempts = 0; attempts < 3; attempts++) {
// send notification datagram
//Serial.println(netbuf.buffer());
if (!net.send(netbuf.buffer(), netbuf.length())) {
// error sending data
break;
}
if (event == EVENT_ACK) return true; // no reply for ACK
char *data = 0;
// receive reply
uint32_t t = millis();
do {
if ((data = net.receive())) break;
// no reply yet
delay(100);
} while (millis() - t < DATA_RECEIVING_TIMEOUT);
if (!data) {
//Serial.println("Timeout");
continue;
}
// verify checksum
if (!verifyChecksum(data)) {
Serial.print("Checksum mismatch:");
Serial.println(data);
continue;
}
char pattern[16];
sprintf(pattern, "EV=%u", event);
if (!strstr(data, pattern)) {
Serial.println("Invalid reply");
continue;
}
if (event == EVENT_LOGIN) {
// extract info from server response
char *p = strstr(data, "TM=");
if (p) {
// set local time from server
unsigned long tm = atol(p + 3);
struct timeval tv = { .tv_sec = (time_t)tm, .tv_usec = 0 };
settimeofday(&tv, NULL);
}
p = strstr(data, "SN=");
if (p) {
char *q = strchr(p, ',');
if (q) *q = 0;
}
feedid = hex2uint16(data);
login = true;
} else if (event == EVENT_LOGOUT) {
login = false;
}
// success
return true;
}
void
rel_recvpkt (rel_t *r, packet_t *pkt, size_t n)
{
// printf("rel_recvpkt\n");
uint16_t len = ntohs(pkt->len);
uint32_t ackno = ntohl(pkt->ackno);
int verified = verifyChecksum(r, pkt, n);
if (!verified || (len != n)) { // Drop packets with bad length
// fprintf(stderr, "Packet w/ sequence number %d dropped\n", ntohl(pkt->seqno));
return;
}
if (len == ACK_PACKET_SIZE) { // Received packet is an ack packet
// fprintf(stderr, "Received ack number: %d\n", ackno);
// fprintf(stderr, "%s\n", "======================RECEIVED ACK PACKET=========================");
if (ackno <= r->LAST_PACKET_ACKED + 1) { // Drop duplicate acks
// fprintf(stderr, "Duplicate ack: %d received\n", ackno);
return;
}
if (ackno == r->LAST_ACK_RECVD) {
r->LAST_ACK_COUNT++;
}
else {
r->LAST_ACK_RECVD = ackno;
r->LAST_ACK_COUNT = 1;
}
if (r->slowStart) {
// fprintf(stderr, "window size: %d\n", r->windowSize);
if (r->windowSize * 2 > r->ssThresh) {
// START AIMD
// return;
} else {
r->windowSize = r->windowSize * 2;
}
// fprintf(stderr, "success? %d\n", r->windowSize);
}
else {
if (r->windowSize + 1 == r->ssThresh) {
} else {
r->windowSize = r->windowSize + 1;
}
}
shiftSentPacketList(r, ackno);
r->LAST_PACKET_ACKED = ackno - 1;
rel_read(r);
}
else { // data packet
// fprintf(stderr, "%s\n", "======================RECEIVED DATA PACKET=========================");
uint32_t seqno = ntohl(pkt->seqno);
// fprintf(stderr, "Received Data: %s\n", pkt->data);
// fprintf(stderr, "Received data: %s\n", pkt->data);
// if (seqno > r->NEXT_PACKET_EXPECTED) {
// // Ghetto fix
// return;
// }
if (seqno < r->NEXT_PACKET_EXPECTED) { // duplicate packet
// fprintf(stderr, "Received duplicate packet w/ sequence number: %d\n", seqno);
struct ack_packet *ack = createAckPacket(r, r->NEXT_PACKET_EXPECTED);
conn_sendpkt(r->c, (packet_t *)ack, ACK_PACKET_SIZE);
free(ack);
return;
}
if (seqno - r->NEXT_PACKET_EXPECTED > r->windowSize) { // Packet outside window
return;
}
// fprintf(stderr, "Received sequence number: %d\n", seqno);
int slot = seqno - r->NEXT_PACKET_EXPECTED;
// fprintf(stderr, "RecvPacket slot number: %d\n", slot);
memcpy(r->recvPackets[slot]->packet, pkt, sizeof(packet_t));
r->recvPackets[slot]->sentTime = getCurrentTime();
r->recvPackets[slot]->acked = 1;
rel_output(r);
// if (seqno == r->NEXT_PACKET_EXPECTED) {
// struct ack_packet *ack = createAckPacket(r, r->NEXT_PACKET_EXPECTED + 1);
// conn_sendpkt(r->c, (packet_t *)ack, ACK_PACKET_SIZE);
// conn_output(r->c, pkt->data, len - HEADER_SIZE);
// // rel_output(r);
// r->NEXT_PACKET_EXPECTED++;
// free(ack);
// }
}
}
void
rel_recvpkt (rel_t *r, packet_t *pkt, size_t n)
{
uint16_t len = ntohs(pkt->len);
uint32_t ackno = ntohl(pkt->ackno);
int verified = verifyChecksum(r, pkt, n);
if (!verified || (len != n)) { // Drop packets with bad length
fprintf(stderr, "Packet w/ sequence number %d dropped\n", ntohl(pkt->seqno));
return;
}
if (len == ACK_PACKET_SIZE) { // Received packet is an ack packet
fprintf(stderr, "Received ack number: %d\n", ackno);
if (ackno <= r->LAST_PACKET_ACKED + 1) { // Drop duplicate acks
fprintf(stderr, "Duplicate ack: %d received\n", ackno);
return;
}
//if (ackno == r->LAST_PACKET_SENT + 1) { // REMOVE THIS AFTER WE FIX ACK SENDING!!!!
shiftSentPacketList(r, ackno);
r->LAST_PACKET_ACKED = ackno - 1;
rel_read(r);
//}
}
else { // data packet
fprintf(stderr, "%s\n", "======================RECEIVED DATA PACKET=========================");
uint32_t seqno = ntohl(pkt->seqno);
// fprintf(stderr, "Received data: %s\n", pkt->data);
// if (seqno > r->NEXT_PACKET_EXPECTED) {
// // Ghetto fix
// return;
// }
if (seqno < r->NEXT_PACKET_EXPECTED) { // duplicate packet
fprintf(stderr, "Received duplicate packet w/ sequence number: %d\n", seqno);
struct ack_packet *ack = createAckPacket(r, r->NEXT_PACKET_EXPECTED);
conn_sendpkt(r->c, (packet_t *)ack, ACK_PACKET_SIZE);
free(ack);
return;
}
if (seqno - r->NEXT_PACKET_EXPECTED > r->windowSize) { // Packet outside window
return;
}
fprintf(stderr, "Received sequence number: %d\n", seqno);
int slot = seqno - r->NEXT_PACKET_EXPECTED;
fprintf(stderr, "RecvPacket slot number: %d\n", slot);
memcpy(r->recvPackets[slot]->packet, pkt, sizeof(packet_t));
r->recvPackets[slot]->sentTime = getCurrentTime();
r->recvPackets[slot]->acked = 1;
rel_output(r);
// if (seqno == r->NEXT_PACKET_EXPECTED) {
// struct ack_packet *ack = createAckPacket(r, r->NEXT_PACKET_EXPECTED + 1);
// conn_sendpkt(r->c, (packet_t *)ack, ACK_PACKET_SIZE);
// conn_output(r->c, pkt->data, len - HEADER_SIZE);
// // rel_output(r);
// r->NEXT_PACKET_EXPECTED++;
// free(ack);
// }
}
}
