diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 8983934804..0b8044274c 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -552,12 +552,16 @@ void VDev::showDevices() PX4_INFO("DF Devices:"); // TODO NOT IMPLEMENTED - // std::string devname; - // for (unsigned int index=0; i == 0; ++i) { - // if (DevMgr::getNextDeviceName(index, devname) == 0) { - // PX4_INFO(" %s", devname.c_str()); - // } - // } + std::string devname; + unsigned int index = 0; + i = 0; + do { + // Each look increments index and returns -1 if end reached + i = DevMgr::getNextDeviceName(index, devname); + if (i == 0) { + PX4_INFO(" %s", devname.c_str()); + } + } while (i==0); } void VDev::showTopics() diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 44403c8eb5..a09333b9e4 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 44403c8eb5c6c26a33d086a200d017c57d658095 +Subproject commit a09333b9e418b84b51788ec9f349e4d4b9253448 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index b95ee30639..f794972e8a 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 dfdevname; + unsigned int index = 0; + for(;;) { + if (DevMgr::getNextDeviceName(index, dfdevname) < 0) { + break; + } + + DevHandle h; + DevMgr::getHandle(dfdevname.c_str(), h); + + if (!h.isValid()) { + warn("failed opening device %s", dfdevname.c_str()); + continue; + } + + int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1); + DevMgr::releaseHandle(h); + + printf("Disabling %s: %s\n", dfdevname.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..4b34355e56 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -135,10 +135,12 @@ int Simulator::start(int argc, char *argv[]) // Update sensor data _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 +153,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 +170,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 141751fc03..f95e508b21 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; @@ -1111,7 +1111,7 @@ start(enum Rotation rotation) /* don't fail if mag dev cannot be opened */ if (h_mag.isValid()) { - if (h_mag.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h_mag.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } 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 3f3046f3d4..7dc5032eb4 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -1253,7 +1253,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; } @@ -1321,7 +1321,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; } @@ -1394,12 +1394,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; }