int MEASAirspeed::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[4] = {0, 0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } uint8_t status = (val[0] & 0xC0) >> 6; switch (status) { case 0: break; case 1: /* fallthrough */ case 2: /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; /* mask the used bits */ dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; // Calculate differential pressure. As its centered around 8000 // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; const float PSI_to_Pa = 6894.757f; /* this equation is an inversion of the equation in the pressure transfer function figure on page 4 of the datasheet We negate the result so that positive differential pressures are generated when the bottom port is used as the static port on the pitot and top port is used as the dynamic port */ float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible voltage_correction(diff_press_pa_raw, temperature); float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); /* note that we return both the absolute value with offset applied and a raw value without the offset applied. This makes it possible for higher level code to detect if the user has the tubes connected backwards, and also makes it possible to correctly use offsets calculated by a higher level airspeed driver. With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port Also note that the _diff_pres_offset is applied before the fabsf() not afterwards. It needs to be done this way to prevent a bias at low speeds, but this also means that when setting a offset you must set it based on the raw value, not the offset value */ struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa; } report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
int MEASAirspeed::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[4] = {0, 0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } uint8_t status = (val[0] & 0xC0) >> 6; switch (status) { case 0: break; case 1: /* fallthrough */ case 2: /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; /* mask the used bits */ dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; // Calculate differential pressure. As its centered around 8000 // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; const float PSI_to_Pa = 6894.757f; /* this equation is an inversion of the equation in the pressure transfer function figure on page 4 of the datasheet We negate the result so that positive differential pressures are generated when the bottom port is used as the static port on the pitot and top port is used as the dynamic port */ float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible voltage_correction(diff_press_pa_raw, temperature); // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; /* With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port */ struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa_raw > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa_raw; } report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub != nullptr && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
int MEASAirspeed::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[4] = {0, 0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } uint8_t status = (val[0] & 0xC0) >> 6; switch (status) { case 0: // Normal Operation. Good Data Packet break; case 1: // Reserved return -EAGAIN; case 2: // Stale Data. Data has been fetched since last measurement cycle. return -EAGAIN; case 3: // Fault Detected perf_count(_comms_errors); perf_end(_sample_perf); return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; /* mask the used bits */ dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; // dT max is almost certainly an invalid reading if (dT_raw == 2047) { perf_count(_comms_errors); return -EAGAIN; } float temperature = ((200.0f * dT_raw) / 2047) - 50; // Calculate differential pressure. As its centered around 8000 // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; const float PSI_to_Pa = 6894.757f; /* this equation is an inversion of the equation in the pressure transfer function figure on page 4 of the datasheet We negate the result so that positive differential pressures are generated when the bottom port is used as the static port on the pitot and top port is used as the dynamic port */ float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible voltage_correction(diff_press_pa_raw, temperature); /* With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port */ struct differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; report.device_id = _device_id.devid; if (_airspeed_pub != nullptr && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } ret = OK; perf_end(_sample_perf); return ret; }