dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'ms4525_fix' of github.com:PX4/Firmware into paul_estimator_numeric

This commit is contained in:
Lorenz Meier 2014-03-21 08:12:36 +01:00
parent e0e6df6733 8af83f7074
commit 3e525d0618
2 changed files with 70 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -132,7 +132,6 @@ ETSAirspeed::measure()
if (OK != ret) { if (OK != ret) {
perf_count(_comms_errors); perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
} }
return ret; return ret;
@ -308,7 +307,7 @@ fail:
g_dev = nullptr; g_dev = nullptr;
} }
errx(1, "driver start failed"); errx(1, "no ETS airspeed sensor connected");
} }
/** /**

112
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -104,7 +104,7 @@
class MEASAirspeed : public Airspeed class MEASAirspeed : public Airspeed
{ {
public: public:
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
protected: protected:
@ -123,7 +123,7 @@ protected:
*/ */
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path) CONVERSION_INTERVAL, path)
{ {
@ -161,23 +161,25 @@ MEASAirspeed::collect()
ret = transfer(nullptr, 0, &val[0], 4); ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) { if (ret < 0) {
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return ret;
} }
uint8_t status = val[0] & 0xC0; uint8_t status = (val[0] & 0xC0) >> 6;
if (status == 2) { switch (status) {
log("err: stale data"); case 0:
perf_count(_comms_errors); break;
perf_end(_sample_perf);
return ret; case 1:
} else if (status == 3) { /* fallthrough */
log("err: fault"); case 2:
perf_count(_comms_errors); /* fallthrough */
perf_end(_sample_perf); case 3:
return ret; perf_count(_comms_errors);
perf_end(_sample_perf);
return -EAGAIN;
} }
int16_t dp_raw = 0, dT_raw = 0; int16_t dp_raw = 0, dT_raw = 0;
@ -193,19 +195,21 @@ MEASAirspeed::collect()
*/ */
const float P_min = -1.0f; const float P_min = -1.0f;
const float P_max = 1.0f; const float P_max = 1.0f;
float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
if (diff_press_pa < 0.0f)
diff_press_pa = 0.0f; if (diff_press_pa < 0.0f) {
diff_press_pa = 0.0f;
}
struct differential_pressure_s report; struct differential_pressure_s report;
/* 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_press_pa > _max_differential_pressure_pa) { if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa; _max_differential_pressure_pa = diff_press_pa;
} }
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors); report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature; report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa; report.differential_pressure_pa = diff_press_pa;
report.voltage = 0; report.voltage = 0;
@ -261,8 +265,9 @@ MEASAirspeed::cycle()
} }
/* 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;
@ -303,15 +308,17 @@ start(int i2c_bus)
{ {
int fd; int fd;
if (g_dev != nullptr) if (g_dev != nullptr) {
errx(1, "already started"); errx(1, "already started");
}
/* create the driver, try the MS4525DO first */ /* create the driver, try the MS4525DO first */
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
/* check if the MS4525DO was instantiated */ /* check if the MS4525DO was instantiated */
if (g_dev == nullptr) if (g_dev == nullptr) {
goto fail; goto fail;
}
/* try the MS5525DSO next if init fails */ /* try the MS5525DSO next if init fails */
if (OK != g_dev->Airspeed::init()) { if (OK != g_dev->Airspeed::init()) {
@ -319,22 +326,26 @@ start(int i2c_bus)
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
/* check if the MS5525DSO was instantiated */ /* 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 */ /* both versions failed if the init for the MS5525DSO fails, give up */
if (OK != g_dev->Airspeed::init()) if (OK != g_dev->Airspeed::init()) {
goto fail; goto fail;
}
} }
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0) {
goto fail; goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail; goto fail;
}
exit(0); exit(0);
@ -345,7 +356,7 @@ fail:
g_dev = nullptr; g_dev = nullptr;
} }
errx(1, "driver start failed"); errx(1, "no MS4525 airspeed sensor connected");
} }
/** /**
@ -379,21 +390,24 @@ test()
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0) {
err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
}
/* do a simple demand read */ /* do a simple demand read */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report)) {
err(1, "immediate read failed"); err(1, "immediate read failed");
}
warnx("single read"); warnx("single read");
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */ /* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate"); errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */ /* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) { for (unsigned i = 0; i < 5; i++) {
@ -404,14 +418,16 @@ test()
fds.events = POLLIN; fds.events = POLLIN;
ret = poll(&fds, 1, 2000); ret = poll(&fds, 1, 2000);
if (ret != 1) if (ret != 1) {
errx(1, "timed out waiting for sensor data"); errx(1, "timed out waiting for sensor data");
}
/* now go get it */ /* now go get it */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report)) {
err(1, "periodic read failed"); err(1, "periodic read failed");
}
warnx("periodic read %u", i); warnx("periodic read %u", i);
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
@ -419,8 +435,9 @@ test()
} }
/* reset the sensor polling to its default rate */ /* reset the sensor polling to its default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default rate"); errx(1, "failed to set default rate");
}
errx(0, "PASS"); errx(0, "PASS");
} }
@ -433,14 +450,17 @@ reset()
{ {
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0) {
err(1, "failed "); err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed"); err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed"); err(1, "driver poll restart failed");
}
exit(0); exit(0);
} }
@ -451,8 +471,9 @@ reset()
void void
info() info()
{ {
if (g_dev == nullptr) if (g_dev == nullptr) {
errx(1, "driver not running"); errx(1, "driver not running");
}
printf("state @ %p\n", g_dev); printf("state @ %p\n", g_dev);
g_dev->print_info(); g_dev->print_info();
@ -491,32 +512,37 @@ meas_airspeed_main(int argc, char *argv[])
/* /*
* Start/load the driver. * Start/load the driver.
*/ */
if (!strcmp(argv[1], "start")) if (!strcmp(argv[1], "start")) {
meas_airspeed::start(i2c_bus); meas_airspeed::start(i2c_bus);
}
/* /*
* Stop the driver * Stop the driver
*/ */
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop")) {
meas_airspeed::stop(); meas_airspeed::stop();
}
/* /*
* Test the driver/device. * Test the driver/device.
*/ */
if (!strcmp(argv[1], "test")) if (!strcmp(argv[1], "test")) {
meas_airspeed::test(); meas_airspeed::test();
}
/* /*
* Reset the driver. * Reset the driver.
*/ */
if (!strcmp(argv[1], "reset")) if (!strcmp(argv[1], "reset")) {
meas_airspeed::reset(); meas_airspeed::reset();
}
/* /*
* Print driver information. * Print driver information.
*/ */
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
meas_airspeed::info(); meas_airspeed::info();
}
meas_airspeed_usage(); meas_airspeed_usage();
exit(0); exit(0);