forked from Archive/PX4-Autopilot
merged master
This commit is contained in:
Lorenz Meier
commit
4840d5fbcb
|
|
@ -200,7 +200,7 @@ MB12XX::MB12XX(int bus, int address) :
|
|||
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
_debug_enabled = false;
|
||||
|
||||
// work_cancel in the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
|
|
@ -762,6 +762,11 @@ test()
|
|||
warnx("time: %lld", report.timestamp);
|
||||
}
|
||||
|
||||
/* reset the sensor polling to default rate */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
|
||||
errx(1, "failed to set default poll rate");
|
||||
}
|
||||
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@
|
|||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/wqueue.h>
|
||||
|
|
@ -83,8 +84,8 @@ static const int ERROR = -1;
|
|||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
#define SF0X_CONVERSION_INTERVAL 84000
|
||||
#define SF0X_TAKE_RANGE_REG 'D'
|
||||
#define SF0X_CONVERSION_INTERVAL 83334
|
||||
#define SF0X_TAKE_RANGE_REG 'd'
|
||||
#define SF02F_MIN_DISTANCE 0.0f
|
||||
#define SF02F_MAX_DISTANCE 40.0f
|
||||
#define SF0X_DEFAULT_PORT "/dev/ttyS2"
|
||||
|
|
@ -92,7 +93,7 @@ static const int ERROR = -1;
|
|||
class SF0X : public device::CDev
|
||||
{
|
||||
public:
|
||||
SF0X(const char* port=SF0X_DEFAULT_PORT);
|
||||
SF0X(const char *port = SF0X_DEFAULT_PORT);
|
||||
virtual ~SF0X();
|
||||
|
||||
virtual int init();
|
||||
|
|
@ -117,6 +118,9 @@ private:
|
|||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
int _fd;
|
||||
char _linebuf[10];
|
||||
unsigned _linebuf_index;
|
||||
hrt_abstime _last_read;
|
||||
|
||||
orb_advert_t _range_finder_topic;
|
||||
|
||||
|
|
@ -178,6 +182,9 @@ SF0X::SF0X(const char *port) :
|
|||
_sensor_ok(false),
|
||||
_measure_ticks(0),
|
||||
_collect_phase(false),
|
||||
_fd(-1),
|
||||
_linebuf_index(0),
|
||||
_last_read(0),
|
||||
_range_finder_topic(-1),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
|
||||
|
|
@ -186,8 +193,45 @@ SF0X::SF0X(const char *port) :
|
|||
/* open fd */
|
||||
_fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
if (_fd < 0) {
|
||||
warnx("FAIL: laser fd");
|
||||
}
|
||||
|
||||
/* tell it to stop auto-triggering */
|
||||
char stop_auto = ' ';
|
||||
(void)::write(_fd, &stop_auto, 1);
|
||||
usleep(100);
|
||||
(void)::write(_fd, &stop_auto, 1);
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_fd, &uart_config);
|
||||
|
||||
/* clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
/* no parity, one stop bit */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB);
|
||||
|
||||
unsigned speed = B9600;
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
warnx("ERR CFG: %d ISPD", termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
warnx("ERR CFG: %d OSPD\n", termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERR baud %d ATTR", termios_state);
|
||||
}
|
||||
|
||||
// disable debug() calls
|
||||
_debug_enabled = false;
|
||||
|
||||
// work_cancel in the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
|
|
@ -208,15 +252,18 @@ int
|
|||
SF0X::init()
|
||||
{
|
||||
int ret = ERROR;
|
||||
unsigned i = 0;
|
||||
|
||||
/* do regular cdev init */
|
||||
if (CDev::init() != OK)
|
||||
if (CDev::init() != OK) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
||||
|
||||
if (_reports == nullptr) {
|
||||
warnx("mem err");
|
||||
goto out;
|
||||
}
|
||||
|
||||
|
|
@ -226,12 +273,35 @@ SF0X::init()
|
|||
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
|
||||
|
||||
if (_range_finder_topic < 0) {
|
||||
debug("failed to create sensor_range_finder object. Did you start uOrb?");
|
||||
warnx("advert err");
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
_sensor_ok = true;
|
||||
/* attempt to get a measurement 5 times */
|
||||
while (ret != OK && i < 5) {
|
||||
|
||||
if (measure()) {
|
||||
ret = ERROR;
|
||||
_sensor_ok = false;
|
||||
}
|
||||
|
||||
usleep(100000);
|
||||
|
||||
if (collect()) {
|
||||
ret = ERROR;
|
||||
_sensor_ok = false;
|
||||
|
||||
} else {
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
_sensor_ok = true;
|
||||
}
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
/* close the fd */
|
||||
::close(_fd);
|
||||
_fd = -1;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -453,7 +523,7 @@ SF0X::measure()
|
|||
|
||||
if (ret != sizeof(cmd)) {
|
||||
perf_count(_comms_errors);
|
||||
log("serial transfer returned %d", ret);
|
||||
log("write fail %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -469,21 +539,64 @@ SF0X::collect()
|
|||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
char buf[16];
|
||||
/* read from the sensor (uart buffer) */
|
||||
ret = ::read(_fd, buf, sizeof(buf));
|
||||
/* clear buffer if last read was too long ago */
|
||||
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
|
||||
|
||||
if (ret < 1) {
|
||||
log("error reading from sensor: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
|
||||
_linebuf_index = 0;
|
||||
}
|
||||
|
||||
printf("ret: %d, val (str): %s\n", ret, buf);
|
||||
char* end;
|
||||
float si_units = strtod(buf,&end);
|
||||
printf("val (float): %8.4f\n", si_units);
|
||||
/* read from the sensor (uart buffer) */
|
||||
ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
|
||||
|
||||
if (ret < 0) {
|
||||
_linebuf[sizeof(_linebuf) - 1] = '\0';
|
||||
debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
_linebuf_index += ret;
|
||||
|
||||
if (_linebuf_index >= sizeof(_linebuf)) {
|
||||
_linebuf_index = 0;
|
||||
}
|
||||
|
||||
_last_read = hrt_absolute_time();
|
||||
|
||||
if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
|
||||
/* incomplete read, reschedule ourselves */
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
char *end;
|
||||
float si_units;
|
||||
bool valid;
|
||||
|
||||
/* enforce line ending */
|
||||
_linebuf[sizeof(_linebuf) - 1] = '\0';
|
||||
|
||||
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
|
||||
si_units = -1.0f;
|
||||
valid = false;
|
||||
|
||||
} else {
|
||||
si_units = strtod(_linebuf, &end);
|
||||
valid = true;
|
||||
}
|
||||
|
||||
debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
|
||||
|
||||
/* done with this chunk, resetting */
|
||||
_linebuf_index = 0;
|
||||
|
||||
struct range_finder_report report;
|
||||
|
||||
|
|
@ -491,7 +604,7 @@ SF0X::collect()
|
|||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.distance = si_units;
|
||||
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
|
||||
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);
|
||||
|
|
@ -519,21 +632,21 @@ SF0X::start()
|
|||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
SUBSYSTEM_TYPE_RANGEFINDER
|
||||
};
|
||||
static orb_advert_t pub = -1;
|
||||
// /* notify about state change */
|
||||
// struct subsystem_info_s info = {
|
||||
// true,
|
||||
// true,
|
||||
// true,
|
||||
// SUBSYSTEM_TYPE_RANGEFINDER
|
||||
// };
|
||||
// static orb_advert_t pub = -1;
|
||||
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
// if (pub > 0) {
|
||||
// orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
}
|
||||
// } else {
|
||||
// pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
// }
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -553,11 +666,29 @@ SF0X::cycle_trampoline(void *arg)
|
|||
void
|
||||
SF0X::cycle()
|
||||
{
|
||||
/* fds initialized? */
|
||||
if (_fd < 0) {
|
||||
/* open fd */
|
||||
_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
}
|
||||
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
int collect_ret = collect();
|
||||
|
||||
if (collect_ret == -EAGAIN) {
|
||||
/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&SF0X::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(1100));
|
||||
return;
|
||||
}
|
||||
|
||||
if (OK != collect_ret) {
|
||||
log("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
|
|
@ -633,7 +764,7 @@ void info();
|
|||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start(const char* port)
|
||||
start(const char *port)
|
||||
{
|
||||
int fd;
|
||||
|
||||
|
|
@ -653,9 +784,10 @@ start(const char* port)
|
|||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
|
||||
fd = open(RANGE_FINDER_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
warnx("device open fail");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
|
|
@ -749,6 +881,11 @@ test()
|
|||
warnx("time: %lld", report.timestamp);
|
||||
}
|
||||
|
||||
/* reset the sensor polling to default rate */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
|
||||
errx(1, "failed to set default poll rate");
|
||||
}
|
||||
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
||||
|
|
@ -802,6 +939,7 @@ sf0x_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "start")) {
|
||||
if (argc > 2) {
|
||||
sf0x::start(argv[2]);
|
||||
|
||||
} else {
|
||||
sf0x::start(SF0X_DEFAULT_PORT);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,6 +58,8 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <drivers/drv_range_finder.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
|
|
@ -791,6 +793,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct vehicle_global_velocity_setpoint_s global_vel_sp;
|
||||
struct battery_status_s battery;
|
||||
struct telemetry_status_s telemetry;
|
||||
struct range_finder_report range_finder;
|
||||
} buf;
|
||||
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
|
|
@ -851,6 +854,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
int global_vel_sp_sub;
|
||||
int battery_sub;
|
||||
int telemetry_sub;
|
||||
int range_finder_sub;
|
||||
} subs;
|
||||
|
||||
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
|
@ -874,6 +878,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
|
||||
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
|
||||
thread_running = true;
|
||||
|
||||
|
|
@ -1227,6 +1232,15 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
LOGBUFFER_WRITE_AND_COUNT(TELE);
|
||||
}
|
||||
|
||||
/* --- BOTTOM DISTANCE --- */
|
||||
if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
|
||||
log_msg.msg_type = LOG_DIST_MSG;
|
||||
log_msg.body.log_DIST.bottom = buf.range_finder.distance;
|
||||
log_msg.body.log_DIST.bottom_rate = 0.0f;
|
||||
log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
|
||||
LOGBUFFER_WRITE_AND_COUNT(DIST);
|
||||
}
|
||||
|
||||
/* signal the other thread new data, but not yet unlock */
|
||||
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
|
||||
/* only request write if several packets can be written at once */
|
||||
|
|
|
|||
Loading…
Reference in New Issue