Browse Source

Fixed last few compile errors, ready for testing

sbg
Lorenz Meier 12 years ago
parent
commit
7fe2aa2797
  1. 2
      src/drivers/airspeed/airspeed.cpp
  2. 4
      src/drivers/airspeed/airspeed.h
  3. 1
      src/drivers/ets_airspeed/ets_airspeed.cpp
  4. 203
      src/drivers/meas_airspeed/meas_airspeed.cpp

2
src/drivers/airspeed/airspeed.cpp

@ -134,7 +134,7 @@ Airspeed::init()
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
if (_airspeed_pub < 0) if (_airspeed_pub < 0)
debug("failed to create airspeed sensor object. Did you start uOrb?"); warnx("failed to create airspeed sensor object. Did you start uOrb?");
ret = OK; ret = OK;
/* sensor is ok, but we don't really know if it is within range */ /* sensor is ok, but we don't really know if it is within range */

4
src/drivers/airspeed/airspeed.h

@ -86,7 +86,7 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE. # error This requires CONFIG_SCHED_WORKQUEUE.
#endif #endif
class Airspeed : public device::I2C class __EXPORT Airspeed : public device::I2C
{ {
public: public:
Airspeed(int bus, int address, unsigned conversion_interval); Airspeed(int bus, int address, unsigned conversion_interval);
@ -100,7 +100,7 @@ public:
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
*/ */
void print_info(); virtual void print_info();
protected: protected:
virtual int probe(); virtual int probe();

1
src/drivers/ets_airspeed/ets_airspeed.cpp

@ -93,7 +93,6 @@ class ETSAirspeed : public Airspeed
{ {
public: public:
ETSAirspeed(int bus, int address = I2C_ADDRESS); ETSAirspeed(int bus, int address = I2C_ADDRESS);
virtual ~ETSAirspeed();
protected: protected:

203
src/drivers/meas_airspeed/meas_airspeed.cpp

@ -37,6 +37,15 @@
* @author Simon Wilks * @author Simon Wilks
* *
* Driver for the MEAS Spec series connected via I2C. * Driver for the MEAS Spec series connected via I2C.
*
* Supported sensors:
*
* - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf)
* - untested: MS5525DSO (http://www.meas-spec.com/downloads/MS5525DSO.pdf)
*
* Interface application notes:
*
* - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/application-notes.aspx#)
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -79,8 +88,10 @@
/* Default I2C bus */ /* Default I2C bus */
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION #define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
/* I2C bus address */ /* I2C bus address is 1010001x */
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ #define I2C_ADDRESS_MS4525DO 0x51 /* 7-bit address. */
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
#define I2C_ADDRESS_MS5525DSO 0x77 /* 7-bit address, addr. pin pulled low */
/* Register address */ /* Register address */
#define READ_CMD 0x07 /* Read the data */ #define READ_CMD 0x07 /* Read the data */
@ -97,8 +108,7 @@
class MEASAirspeed : public Airspeed class MEASAirspeed : public Airspeed
{ {
public: public:
MEASAirspeed(int bus, int address = I2C_ADDRESS); MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
virtual ~MEASAirspeed();
protected: protected:
@ -126,122 +136,122 @@ MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
int int
MEASAirspeed::measure() MEASAirspeed::measure()
{ {
int ret; // int ret;
/* // /*
* Send the command to begin a measurement. // * Send the command to begin a measurement.
*/ // */
uint8_t cmd = READ_CMD; // uint8_t cmd = READ_CMD;
ret = transfer(&cmd, 1, nullptr, 0); // ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret) { // if (OK != ret) {
perf_count(_comms_errors); // perf_count(_comms_errors);
log("i2c::transfer returned %d", ret); // log("i2c::transfer returned %d", ret);
return ret; // return ret;
} // }
ret = OK; // ret = OK;
return ret; // return ret;
} }
int int
MEASAirspeed::collect() MEASAirspeed::collect()
{ {
int ret = -EIO; // int ret = -EIO;
/* read from the sensor */ // /* read from the sensor */
uint8_t val[2] = {0, 0}; // uint8_t val[2] = {0, 0};
perf_begin(_sample_perf); // perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2); // ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) { // if (ret < 0) {
log("error reading from sensor: %d", ret); // log("error reading from sensor: %d", ret);
return ret; // return ret;
} // }
uint16_t diff_pres_pa = val[1] << 8 | val[0]; // uint16_t diff_pres_pa = val[1] << 8 | val[0];
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { // if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0; // diff_pres_pa = 0;
} else { // } else {
diff_pres_pa -= _diff_pres_offset; // diff_pres_pa -= _diff_pres_offset;
} // }
// XXX we may want to smooth out the readings to remove noise. // // XXX we may want to smooth out the readings to remove noise.
_reports[_next_report].timestamp = hrt_absolute_time(); // _reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].differential_pressure_pa = diff_pres_pa; // _reports[_next_report].differential_pressure_pa = diff_pres_pa;
// Track maximum differential pressure measured (so we can work out top speed). // // Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { // if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_pres_pa; // _reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
} // }
/* announce the airspeed if needed, just publish else */ // /* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); // orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
/* post a report to the ring - note, not locked */ // /* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports); // INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */ // /* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) { // if (_next_report == _oldest_report) {
perf_count(_buffer_overflows); // perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports); // INCREMENT(_oldest_report, _num_reports);
} // }
/* notify anyone waiting for data */ // /* notify anyone waiting for data */
poll_notify(POLLIN); // poll_notify(POLLIN);
ret = OK; // ret = OK;
perf_end(_sample_perf); // perf_end(_sample_perf);
return ret; // return ret;
} }
void void
MEASAirspeed::cycle() MEASAirspeed::cycle()
{ {
/* collection phase? */ // /* collection phase? */
if (_collect_phase) { // if (_collect_phase) {
/* perform collection */ // /* perform collection */
if (OK != collect()) { // if (OK != collect()) {
log("collection error"); // log("collection error");
/* restart the measurement state machine */ // /* restart the measurement state machine */
start(); // start();
return; // return;
} // }
/* next phase is measurement */ // /* next phase is measurement */
_collect_phase = false; // _collect_phase = false;
/* // /*
* Is there a collect->measure gap? // * Is there a collect->measure gap?
*/ // */
if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { // if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */ // /* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK, // work_queue(HPWORK,
&_work, // &_work,
(worker_t)&Airspeed::cycle_trampoline, // (worker_t)&Airspeed::cycle_trampoline,
this, // this,
_measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); // _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
return; // return;
} // }
} // }
/* measurement phase */ // /* measurement phase */
if (OK != measure()) // if (OK != measure())
log("measure error"); // log("measure error");
/* next phase is collection */ // /* next phase is collection */
_collect_phase = true; // _collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */ /* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK, work_queue(HPWORK,
@ -282,12 +292,23 @@ start(int i2c_bus)
if (g_dev != nullptr) if (g_dev != nullptr)
errx(1, "already started"); errx(1, "already started");
/* create the driver */ /* create the driver, try the MS4525DO first */
g_dev = new MEASAirspeed(i2c_bus); g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
/* check if the MS4525DO was instantiated */
if (g_dev == nullptr)
goto fail;
/* try the MS5525DSO next if init fails */
if (OK != g_dev->init())
delete g_dev;
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
/* check if the MS5525DSO was instantiated */
if (g_dev == nullptr) if (g_dev == nullptr)
goto fail; goto fail;
/* both versions failed if the init for the MS5525DSO fails, give up */
if (OK != g_dev->init()) if (OK != g_dev->init())
goto fail; goto fail;

Loading…
Cancel
Save