forked from Archive/PX4-Autopilot
Merge pull request #843 from PX4/sensor_err_handling
Sensor error handling
This commit is contained in:
commit
5ef57b8af6
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue