diff options
Diffstat (limited to 'src/drivers/hmc5883/hmc5883.cpp')
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 48 |
1 files changed, 39 insertions, 9 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4aef43102..0e9a961ac 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -162,6 +162,7 @@ private: orb_advert_t _mag_topic; orb_advert_t _subsystem_pub; + orb_id_t _mag_orb_id; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -337,6 +338,9 @@ private: */ int check_offset(); + /* this class has pointer data members, do not allow copying it */ + HMC5883(const HMC5883&); + HMC5883 operator=(const HMC5883&); }; /* @@ -347,14 +351,17 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), + _work{}, _measure_ticks(0), _reports(nullptr), + _scale{}, _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), + _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -423,6 +430,20 @@ HMC5883::init() _class_instance = register_class_devname(MAG_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _mag_orb_id = ORB_ID(sensor_mag0); + break; + + case CLASS_DEVICE_SECONDARY: + _mag_orb_id = ORB_ID(sensor_mag1); + break; + + case CLASS_DEVICE_TERTIARY: + _mag_orb_id = ORB_ID(sensor_mag2); + break; + } + ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; @@ -867,7 +888,8 @@ HMC5883::collect() struct { int16_t x, y, z; } report; - int ret = -EIO; + + int ret; uint8_t cmd; uint8_t check_counter; @@ -945,16 +967,16 @@ HMC5883::collect() // apply user specified rotation rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); - if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + if (!(_pub_blocked)) { if (_mag_topic != -1) { /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + orb_publish(_mag_orb_id, _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report); + _mag_topic = orb_advertise(_mag_orb_id, &new_report); if (_mag_topic < 0) - debug("failed to create sensor_mag publication"); + debug("ADVERT FAIL"); } } @@ -1157,7 +1179,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); + warn("failed to set new scale / offsets for mag"); } /* set back to normal mode */ @@ -1341,8 +1363,8 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int; -HMC5883 *g_dev_ext; +HMC5883 *g_dev_int = nullptr; +HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); @@ -1353,6 +1375,9 @@ void usage(); /** * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. */ void start(int bus, enum Rotation rotation) @@ -1378,6 +1403,11 @@ start(int bus, enum Rotation rotation) errx(0, "already started internal"); g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); if (g_dev_int != nullptr && OK != g_dev_int->init()) { + + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + if (bus == PX4_I2C_BUS_ONBOARD) { goto fail; } @@ -1443,7 +1473,7 @@ test(int bus) int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); + err(1, "%s open failed (try 'hmc5883 start')", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); |