diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 8983934804..470b8388b3 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -550,14 +550,18 @@ void VDev::showDevices() pthread_mutex_unlock(&devmutex); PX4_INFO("DF Devices:"); - // TODO NOT IMPLEMENTED + std::string dev_path, instance_path; + unsigned int index = 0; + i = 0; - // std::string devname; - // for (unsigned int index=0; i == 0; ++i) { - // if (DevMgr::getNextDeviceName(index, devname) == 0) { - // PX4_INFO(" %s", devname.c_str()); - // } - // } + do { + // Each look increments index and returns -1 if end reached + i = DevMgr::getNextDevicePath(index, dev_path, instance_path); + + if (i == 0) { + PX4_INFO(" %s (%s)", dev_path.c_str(), instance_path.c_str()); + } + } while (i == 0); } void VDev::showTopics() diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 44403c8eb5..45878058a7 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 44403c8eb5c6c26a33d086a200d017c57d658095 +Subproject commit 45878058a7b37a9bba370396526b95cd7c2a3ab9 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index b95ee30639..a135e9dfca 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -467,33 +467,55 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta #else - // TODO NOT IMPLEMENTED + // Handle VDev devices + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; - // std::string devname; - // unsigned int index = 0; - // for(;;) { - // if (DevMgr::getNextDeviceName(index, devname) < 0) { - // break; - // } + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } - // /* skip mavlink */ - // if (!strcmp("/dev/mavlink", devname.c_str())) { - // continue; - // } - // DevHandle h; - // DevMgr::getHandle(devname.c_str(), h); + int sensfd = px4_open(devname, 0); - // if (!h.isValid()) { - // warn("failed opening device %s", devname.c_str()); - // continue; - // } + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } - // int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1); - // DevMgr::releaseHandle(h); + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); - // printf("Disabling %s: %s\n", devname.c_str(), (block_ret == OK) ? "OK" : "ERROR"); - // } + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + + + // Handle DF devices + std::string df_dev_path, df_dev_instance_path; + unsigned int index = 0; + for(;;) { + if (DevMgr::getNextDevicePath(index, df_dev_path, df_dev_instance_path) < 0) { + break; + } + + DevHandle h; + DevMgr::getHandle(df_dev_path.c_str(), h); + + if (!h.isValid()) { + warn("failed opening device %s", df_dev_path.c_str()); + continue; + } + + int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1); + DevMgr::releaseHandle(h); + + printf("Disabling %s: %s\n", df_dev_path.c_str(), (block_ret == OK) ? "OK" : "ERROR"); + } ret = TRANSITION_CHANGED; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 1656a3451f..44b3bcf0e1 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -136,9 +136,13 @@ int Simulator::start(int argc, char *argv[]) _instance->pollForMAVLinkMessages(false); #endif - } else { + } else if (argv[2][1] == 'p') { // Update sensor data _instance->pollForMAVLinkMessages(true); + + } else { + _instance->initializeSensorData(); + _instance->_initialized = true; } } else { @@ -151,9 +155,10 @@ int Simulator::start(int argc, char *argv[]) static void usage() { - PX4_WARN("Usage: simulator {start -[sc] |stop}"); + PX4_WARN("Usage: simulator {start -[spt] |stop}"); PX4_WARN("Simulate raw sensors: simulator start -s"); PX4_WARN("Publish sensors combined: simulator start -p"); + PX4_WARN("Dummy unit test data: simulator start -t"); } __BEGIN_DECLS @@ -167,7 +172,9 @@ extern "C" { int ret = 0; if (argc == 3 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) { + if (strcmp(argv[2], "-s") == 0 || + strcmp(argv[2], "-p") == 0 || + strcmp(argv[2], "-t") == 0) { if (g_sim_task >= 0) { warnx("Simulator already started"); return 0; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index e039e6b65c..1ba06148cb 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -1101,7 +1101,7 @@ start(enum Rotation rotation) goto fail; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); DevMgr::releaseHandle(h); goto fail; @@ -1110,13 +1110,13 @@ start(enum Rotation rotation) DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ - if (!h_mag.isValid()) { - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + if (h_mag.isValid()) { + if (h_mag.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } } else { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } DevMgr::releaseHandle(h); @@ -1182,6 +1182,11 @@ accelsim_main(int argc, char *argv[]) } } + if (argc <= 1) { + accelsim::usage(); + return 1; + } + const char *verb = argv[myoptind]; /* diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 4326314438..5cd9fff61f 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -239,6 +239,13 @@ adcsim_main(int argc, char *argv[]) PX4_ERR("couldn't allocate the ADCSIM driver"); return 1; } + + ret = g_adc->init(); + + if (ret != 0) { + PX4_ERR("ADCSIM init failed (%d)", ret); + return 1; + } } if (argc > 1 && g_adc) { diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index de6433f8e3..578846a58e 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -834,7 +834,7 @@ start_bus(struct barosim_bus_option &bus) return false; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { DevMgr::releaseHandle(h); PX4_ERR("failed setting default poll rate"); return false; @@ -904,14 +904,14 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); /* set the queue depth to 10 */ - if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, (void *)10UL)) { + if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, 10UL)) { PX4_ERR("failed to set queue depth"); DevMgr::releaseHandle(h); return 1; } /* start the sensor polling at 2Hz */ - if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)2UL)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, 2UL)) { PX4_ERR("failed to set 2Hz poll rate"); DevMgr::releaseHandle(h); return 1; @@ -971,7 +971,7 @@ reset() return 1; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); return 1; } @@ -1017,7 +1017,7 @@ calibrate(unsigned altitude) } /* start the sensor polling at max */ - if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MAX)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { PX4_ERR("failed to set poll rate"); return 1; } @@ -1068,7 +1068,7 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; - if (h.ioctl(BAROIOCSMSLPRESSURE, (void *)((unsigned long)(p1))) != OK) { + if (h.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(p1)) != OK) { PX4_WARN("BAROIOCSMSLPRESSURE"); return 1; } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 8901d2bf63..c44b10e879 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -308,7 +308,7 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VirtDevObj("GYROSIM", path_accel, nullptr, 1000), + VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), _accel_reports(nullptr), @@ -390,6 +390,14 @@ GYROSIM::init() struct gyro_report grp = {}; + ret = VirtDevObj::init(); + + if (ret != 0) { + PX4_WARN("Base class init failed"); + ret = 1; + goto out; + } + /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); @@ -1158,7 +1166,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : // Set sample interval to 0 since device is read by parent - VirtDevObj("GYROSIM_gyro", path, "", 0), + VirtDevObj("GYROSIM_gyro", path, GYRO_BASE_DEVICE_PATH, 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1) @@ -1169,7 +1177,8 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : int GYROSIM_gyro::init() { - return start(); + int ret = VirtDevObj::init(); + return ret ? ret : start(); } void @@ -1252,7 +1261,7 @@ start(enum Rotation rotation) goto fail; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { DevMgr::releaseHandle(h); goto fail; } @@ -1320,7 +1329,7 @@ test() } /* reset to manual polling */ - if (h_accel.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MANUAL) < 0) { + if (h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { PX4_ERR("reset to manual polling"); return 1; } @@ -1393,12 +1402,12 @@ reset() } - if (h.ioctl(SENSORIOCRESET, (void *)0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); goto reset_fail; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); goto reset_fail; }