diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b24ec8167a..509dc45ea0 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -139,6 +139,11 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** + * Stop the automatic measurement state machine. + */ + void stop(); + /** * Diagnostics - print some basic information about the driver. */ @@ -188,11 +193,6 @@ private: */ void start(); - /** - * Stop the automatic measurement state machine. - */ - void stop(); - /** * Reset the device */ @@ -1459,6 +1459,7 @@ struct hmc5883_bus_option { #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) void start(enum HMC5883_BUS busid, enum Rotation rotation); +int stop(); bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation); struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid); void test(enum HMC5883_BUS busid); @@ -1541,6 +1542,23 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) } } +int +stop() +{ + bool stopped = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (bus_options[i].dev != nullptr) { + bus_options[i].dev->stop(); + delete bus_options[i].dev; + bus_options[i].dev = nullptr; + stopped = true; + } + } + + return !stopped; +} + /** * find a bus structure for a busid */ @@ -1843,6 +1861,13 @@ hmc5883_main(int argc, char *argv[]) exit(0); } + /* + * Stop the driver + */ + if (!strcmp(verb, "stop")) { + return hmc5883::stop(); + } + /* * Test the driver/device. */