// Receive URG distance data
int urg_addRecvData(const char buffer[], long data[], int* filled) {
static int remain_byte = 0;
static char remain_data[3];
const int data_byte = 3;
const char* pre_p = buffer;
const char* p = pre_p;
if (remain_byte > 0) {
memmove(&remain_data[remain_byte], buffer, data_byte - remain_byte);
data[*filled] = urg_decode(remain_data, data_byte);
++(*filled);
pre_p = &buffer[data_byte - remain_byte];
p = pre_p;
remain_byte = 0;
}
do {
++p;
if ((p - pre_p) >= static_cast<int>(data_byte)) {
data[*filled] = urg_decode(pre_p, data_byte);
++(*filled);
pre_p = p;
}
} while (*p != '\0');
remain_byte = p - pre_p;
memmove(remain_data, pre_p, remain_byte);
return 0;
}
// Receive URG data
int urg_receiveData(urg_state_t* state, long data[], size_t max_size) {
int filled = 0;
int i=0;
// Fill the positions upto AngleFirst or min by 19 (non-measurement range)
for ( i = state->AngleFirst -1; i >= 0; --i) {
data[filled++] = 19;
}
char message_type = 'M';
char buffer[LineLength];
int line_length;
for ( i = 0; (line_length = urg_readLine(buffer)) >= 0; ++i) {
// Verify the checksum
if ((i >= 6) && (line_length == 0)) {
// End of data receive
for (size_t i = filled; i < max_size; ++i) {
// Fill the position upto data end by 19 (non-measurement range)
data[filled++] = 19;
}
return filled;
}
else if (i == 0) {
// Judge the message (Command) by AngleFirst letter of receive data
if ((buffer[0] != 'M') && (buffer[0] != 'G')) {
return -1;
}
message_type = buffer[0];
} else if (! strncmp(buffer, "99b", 3)) {
// Detect "99b" and assume [time-stamp] and [data] to follow
i = 4;
} else if ((i == 1) && (message_type == 'G')) {
i = 4;
} else if (i == 4) {
// "99b" Fixed
if (strncmp(buffer, "99b", 3)) {
return -1;
}
} else if (i == 5) {
state->last_timestamp = urg_decode(buffer, 4);
} else if (i >= 6) {
// Received Data
if (line_length > (64 + 1)) {
line_length = (64 + 1);
}
buffer[line_length -1] = '\0';
int ret = urg_addRecvData(buffer, data, &filled);
if (ret < 0) {
return ret;
}
}
}
return -1;
}
void getScan(double *rangeOutput)
{
int act;
unsigned char car;
unsigned char buf[2000];
unsigned char aux[2732];
int res[682];
act = 0;
while((readURG(&car, sizeof(car)))==1)
{
buf[act] = car;
act++;
if ((act>1) && ((buf[act-1] == '\r') || (buf[act-1] == '\n')) &&
((buf[act-2] == '\r') || (buf[act-2] == '\n')))
{
if (act>1000)
{
buf[act] = '\0';
memset(res, 0, sizeof(res));
urg_decode(buf,res);
memset(aux, 0, sizeof(aux));
int j = 0;
for (int i = 0; i < 682; ++i) {
biconvert.val = res[i];
aux[j++] = biconvert.bytes[0];
aux[j++] = biconvert.bytes[1];
aux[j++] = biconvert.bytes[2];
aux[j++] = biconvert.bytes[3];
}
biconvert.val = 0;
aux[j++] = biconvert.bytes[0];
aux[j++] = biconvert.bytes[1];
aux[j++] = biconvert.bytes[2];
aux[j++] = biconvert.bytes[3];
act=0;
memset(buf, 0, sizeof(buf));
break;
}
}
}
startSingleScan();
bool negative = false;
for (int i = 0; i < 682; ++i)
{
if (res[i] < 0)
{
negative = true;
break;
}
else
{
rangeOutput[i] = res[i];
}
}
// Si hay algun negativo son datos invaldiso: devolvemos todo 0
if (negative)
{
for (int i = 0; i < 682; ++i) rangeOutput[i] = 0;
}
}
static void mdlOutputs(SimStruct *S, int_T tid) {
uint16_T *y = ssGetOutputPortSignal(S,0);
int act;
char car;
int negative;
//printf("asignacion: %d\n",n = readURG(&car, sizeof(car)));
//printf("lectura de laser: \n");
act = 0;
while((readURG(&car, sizeof(car)))==1) {
buf[act] = car;
act++;
// printf("%c",car);
if ((act>1) && ((buf[act-1] == '\r') || (buf[act-1] == '\n')) && ((buf[act-2] == '\r') || (buf[act-2] == '\n'))) {
if (act>1000) {
buf[act] = '\0';
memset(res, 0, sizeof(res));
urg_decode(buf,res);
memset(aux, 0, sizeof(aux));
j=0;
for (i=0; i<682; i++) {
biconvert.val = res[i];
aux[j++] = biconvert.bytes[0];
aux[j++] = biconvert.bytes[1];
aux[j++] = biconvert.bytes[2];
aux[j++] = biconvert.bytes[3];
}
biconvert.val = time;
aux[j++] = biconvert.bytes[0];
aux[j++] = biconvert.bytes[1];
aux[j++] = biconvert.bytes[2];
aux[j++] = biconvert.bytes[3];
act=0;
memset(buf, 0, sizeof(buf));
break;
}
}
}
urg_start_single_scan();
negative = 0;
for (i=0; i<682; i++)
{
if (res[i] < 0)
{
negative = 1;
break;
}
else
{
y[i] = res[i];
}
}
// Si hay algun negativo son datos invaldiso: devolvemos todo 0
if (negative == 1)
{
for (i=0; i<682; i++) y[i] = 0;
}
}
