Merge pull request #843 from PX4/sensor_err_handling

Sensor error handling
This commit is contained in:
Lorenz Meier 2014-04-25 10:14:08 +02:00
commit 5ef57b8af6
5 changed files with 56 additions and 26 deletions

View File

@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_max_differential_pressure_pa(0),
_sensor_ok(false),
_last_published_sensor_ok(true), /* initialize differently to force publication */
_measure_ticks(0),
_collect_phase(false),
_diff_pres_offset(0.0f),
_airspeed_pub(-1),
_subsys_pub(-1),
_class_instance(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
@ -149,8 +151,7 @@ Airspeed::init()
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
@ -344,22 +345,6 @@ Airspeed::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_DIFFPRESSURE
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
@ -368,12 +353,35 @@ Airspeed::stop()
work_cancel(HPWORK, &_work);
}
void
Airspeed::update_status()
{
if (_sensor_ok != _last_published_sensor_ok) {
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
_sensor_ok,
SUBSYSTEM_TYPE_DIFFPRESSURE
};
if (_subsys_pub > 0) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
_last_published_sensor_ok = _sensor_ok;
}
}
void
Airspeed::cycle_trampoline(void *arg)
{
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
dev->update_status();
}
void

View File

@ -118,14 +118,21 @@ protected:
virtual int measure() = 0;
virtual int collect() = 0;
/**
* Update the subsystem status
*/
void update_status();
work_s _work;
float _max_differential_pressure_pa;
bool _sensor_ok;
bool _last_published_sensor_ok;
int _measure_ticks;
bool _collect_phase;
float _diff_pres_offset;
orb_advert_t _airspeed_pub;
orb_advert_t _subsys_pub;
int _class_instance;

View File

@ -207,14 +207,18 @@ ETSAirspeed::collect()
void
ETSAirspeed::cycle()
{
int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
ret = collect();
if (OK != ret) {
perf_count(_comms_errors);
/* restart the measurement state machine */
start();
_sensor_ok = false;
return;
}
@ -238,8 +242,12 @@ ETSAirspeed::cycle()
}
/* measurement phase */
if (OK != measure())
log("measure error");
ret = measure();
if (OK != ret) {
debug("measure error");
}
_sensor_ok = (ret == OK);
/* next phase is collection */
_collect_phase = true;

View File

@ -715,7 +715,7 @@ HMC5883::cycle()
/* perform collection */
if (OK != collect()) {
log("collection error");
debug("collection error");
/* restart the measurement state machine */
start();
return;
@ -742,7 +742,7 @@ HMC5883::cycle()
/* measurement phase */
if (OK != measure())
log("measure error");
debug("measure error");
/* next phase is collection */
_collect_phase = true;

View File

@ -288,13 +288,17 @@ MEASAirspeed::collect()
void
MEASAirspeed::cycle()
{
int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
ret = collect();
if (OK != ret) {
/* restart the measurement state machine */
start();
_sensor_ok = false;
return;
}
@ -318,10 +322,13 @@ MEASAirspeed::cycle()
}
/* measurement phase */
if (OK != measure()) {
log("measure error");
ret = measure();
if (OK != ret) {
debug("measure error");
}
_sensor_ok = (ret == OK);
/* next phase is collection */
_collect_phase = true;