int main(int argc, char *argv[])
{
warnx("SF0X test started");
int ret = 0;
const char LINE_MAX = 20;
char _linebuf[LINE_MAX];
_linebuf[0] = '\0';
const char *lines[] = {"0.01\r\n",
"0.02\r\n",
"0.03\r\n",
"0.04\r\n",
"0",
".",
"0",
"5",
"\r",
"\n",
"0",
"3\r",
"\n"
"\r\n",
"0.06",
"\r\n"
};
enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
float dist_m;
char _parserbuf[LINE_MAX];
unsigned _parsebuf_index = 0;
for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
printf("\n%s", _linebuf);
int parse_ret;
for (int i = 0; i < strlen(lines[l]); i++) {
parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
if (parse_ret == 0) {
printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
}
}
printf("%s", lines[l]);
}
warnx("test finished");
return ret;
}
int
SF0X::collect()
{
int ret;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
DEVICE_DEBUG("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (_conversion_interval * 2)) {
return ret;
} else {
return -EAGAIN;
}
} else if (ret == 0) {
return -EAGAIN;
}
_last_read = hrt_absolute_time();
float distance_m = -1.0f;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
valid = true;
}
}
if (!valid) {
return -EAGAIN;
}
DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
SF0X::collect()
{
int ret;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
debug("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
return ret;
} else {
return -EAGAIN;
}
} else if (ret == 0) {
return -EAGAIN;
}
_last_read = hrt_absolute_time();
float si_units;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
valid = true;
}
}
if (!valid) {
return -EAGAIN;
}
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
/* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
