Browse Source

Merge pull request #3191 from mcharleb/driver_framework_latest

Updated to latest DriverFramework
sbg
Mark Charlebois 9 years ago
parent
commit
9afec98740
  1. 20
      src/drivers/device/vdev.cpp
  2. 2
      src/lib/DriverFramework
  3. 64
      src/modules/commander/state_machine_helper.cpp
  4. 13
      src/modules/simulator/simulator.cpp
  5. 15
      src/platforms/posix/drivers/accelsim/accelsim.cpp
  6. 7
      src/platforms/posix/drivers/adcsim/adcsim.cpp
  7. 12
      src/platforms/posix/drivers/barosim/baro.cpp
  8. 23
      src/platforms/posix/drivers/gyrosim/gyrosim.cpp

20
src/drivers/device/vdev.cpp

@ -550,14 +550,18 @@ void VDev::showDevices() @@ -550,14 +550,18 @@ void VDev::showDevices()
pthread_mutex_unlock(&devmutex);
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 dev_path, instance_path;
unsigned int index = 0;
i = 0;
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()

2
src/lib/DriverFramework

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 44403c8eb5c6c26a33d086a200d017c57d658095
Subproject commit 45878058a7b37a9bba370396526b95cd7c2a3ab9

64
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 @@ -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;
/* skip mavlink */
if (!strcmp("/dev/mavlink", devname)) {
continue;
}
// std::string devname;
// unsigned int index = 0;
// for(;;) {
// if (DevMgr::getNextDeviceName(index, devname) < 0) {
// break;
// }
// /* skip mavlink */
// if (!strcmp("/dev/mavlink", devname.c_str())) {
// continue;
// }
int sensfd = px4_open(devname, 0);
// DevHandle h;
// DevMgr::getHandle(devname.c_str(), h);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
// if (!h.isValid()) {
// warn("failed opening device %s", devname.c_str());
// continue;
// }
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
px4_close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
// int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1);
// DevMgr::releaseHandle(h);
// printf("Disabling %s: %s\n", devname.c_str(), (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");

13
src/modules/simulator/simulator.cpp

@ -136,9 +136,13 @@ int Simulator::start(int argc, char *argv[]) @@ -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[]) @@ -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" { @@ -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;

15
src/platforms/posix/drivers/accelsim/accelsim.cpp

@ -1101,7 +1101,7 @@ start(enum Rotation rotation) @@ -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) @@ -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[]) @@ -1182,6 +1182,11 @@ accelsim_main(int argc, char *argv[])
}
}
if (argc <= 1) {
accelsim::usage();
return 1;
}
const char *verb = argv[myoptind];
/*

7
src/platforms/posix/drivers/adcsim/adcsim.cpp

@ -239,6 +239,13 @@ adcsim_main(int argc, char *argv[]) @@ -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) {

12
src/platforms/posix/drivers/barosim/baro.cpp

@ -834,7 +834,7 @@ start_bus(struct barosim_bus_option &bus) @@ -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() @@ -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() @@ -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) @@ -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) @@ -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;
}

23
src/platforms/posix/drivers/gyrosim/gyrosim.cpp

@ -308,7 +308,7 @@ private: @@ -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() @@ -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() @@ -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) : @@ -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) @@ -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() @@ -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() @@ -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;
}

Loading…
Cancel
Save