123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338 |
- #include "AP_Airspeed_SDP3X.h"
- #include <GCS_MAVLink/GCS.h>
- #include <AP_Baro/AP_Baro.h>
- #include <stdio.h>
- #define SDP3X_SCALE_TEMPERATURE 200.0f
- #define SDP3XD0_I2C_ADDR 0x21
- #define SDP3XD1_I2C_ADDR 0x22
- #define SDP3XD2_I2C_ADDR 0x23
- #define SDP3X_CONT_MEAS_AVG_MODE 0x3615
- #define SDP3X_CONT_MEAS_STOP 0x3FF9
- #define SDP3X_SCALE_PRESSURE_SDP31 60
- #define SDP3X_SCALE_PRESSURE_SDP32 240
- #define SDP3X_SCALE_PRESSURE_SDP33 20
- extern const AP_HAL::HAL &hal;
- AP_Airspeed_SDP3X::AP_Airspeed_SDP3X(AP_Airspeed &_frontend, uint8_t _instance) :
- AP_Airspeed_Backend(_frontend, _instance)
- {
- }
- bool AP_Airspeed_SDP3X::_send_command(uint16_t cmd)
- {
- uint8_t b[2] {uint8_t(cmd >> 8), uint8_t(cmd & 0xFF)};
- return _dev->transfer(b, 2, nullptr, 0);
- }
- bool AP_Airspeed_SDP3X::init()
- {
- const uint8_t addresses[3] = { SDP3XD0_I2C_ADDR,
- SDP3XD1_I2C_ADDR,
- SDP3XD2_I2C_ADDR
- };
- bool found = false;
- bool ret = false;
- for (uint8_t i=0; i<ARRAY_SIZE(addresses) && !found; i++) {
- _dev = hal.i2c_mgr->get_device(get_bus(), addresses[i]);
- if (!_dev) {
- continue;
- }
- if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
- continue;
- }
-
- _dev->set_retries(10);
-
- if (!_send_command(SDP3X_CONT_MEAS_STOP)) {
- _dev->get_semaphore()->give();
- continue;
- }
-
- _dev->get_semaphore()->give();
- hal.scheduler->delay_microseconds(20000);
- if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
- continue;
- }
-
- if (!_send_command(SDP3X_CONT_MEAS_AVG_MODE)) {
- _dev->get_semaphore()->give();
- continue;
- }
-
- _dev->get_semaphore()->give();
- hal.scheduler->delay_microseconds(20000);
- if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
- continue;
- }
-
- uint8_t val[9];
- ret = _dev->transfer(nullptr, 0, &val[0], sizeof(val));
- if (!ret) {
- _dev->get_semaphore()->give();
- continue;
- }
-
- if (!_crc(&val[0], 2, val[2]) || !_crc(&val[3], 2, val[5]) || !_crc(&val[6], 2, val[8])) {
- _dev->get_semaphore()->give();
- continue;
- }
- _scale = (((uint16_t)val[6]) << 8) | val[7];
- _dev->get_semaphore()->give();
- found = true;
- char c = 'X';
- switch (_scale) {
- case SDP3X_SCALE_PRESSURE_SDP31:
- c = '1';
- break;
- case SDP3X_SCALE_PRESSURE_SDP32:
- c = '2';
- break;
- case SDP3X_SCALE_PRESSURE_SDP33:
- c = '3';
- break;
- }
- hal.console->printf("SDP3%c: Found on bus %u address 0x%02x scale=%u\n",
- c, get_bus(), addresses[i], _scale);
- }
- if (!found) {
- return false;
- }
-
- set_use_zero_offset();
- set_skip_cal();
- set_offset(0);
-
-
- _dev->set_retries(2);
- _dev->register_periodic_callback(20000,
- FUNCTOR_BIND_MEMBER(&AP_Airspeed_SDP3X::_timer, void));
- return true;
- }
- void AP_Airspeed_SDP3X::_timer()
- {
-
- uint8_t val[6];
- int ret = _dev->transfer(nullptr, 0, &val[0], sizeof(val));
- uint32_t now = AP_HAL::millis();
- if (!ret) {
- if (now - _last_sample_time_ms > 200) {
-
- _send_command(SDP3X_CONT_MEAS_AVG_MODE);
- }
- return;
- }
-
- if (!_crc(&val[0], 2, val[2]) || !_crc(&val[3], 2, val[5])) {
- return;
- }
- int16_t P = (((int16_t)val[0]) << 8) | val[1];
- int16_t temp = (((int16_t)val[3]) << 8) | val[4];
- float diff_press_pa = float(P) / float(_scale);
- float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE;
- WITH_SEMAPHORE(sem);
- _press_sum += diff_press_pa;
- _temp_sum += temperature;
- _press_count++;
- _temp_count++;
- _last_sample_time_ms = now;
- }
- float AP_Airspeed_SDP3X::_correct_pressure(float press)
- {
- float sign = 1.0f;
-
-
- AP_Airspeed::pitot_tube_order tube_order = get_tube_order();
- switch (tube_order) {
- case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE:
- press = -press;
- sign = -1.0f;
-
- case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE:
- break;
- case AP_Airspeed::PITOT_TUBE_ORDER_AUTO:
- default:
- if (press < 0.0f) {
- sign = -1.0f;
- press = -press;
- }
- break;
- }
- if (press <= 0.0f) {
- return 0.0f;
- }
- AP_Baro *baro = AP_Baro::get_singleton();
- if (baro == nullptr) {
- return press;
- }
- float temperature;
- if (!get_temperature(temperature)) {
- return press;
- }
- float rho_air = baro->get_pressure() / (ISA_GAS_CONSTANT * (temperature + C_TO_KELVIN));
-
-
-
- float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1.0f)) * 1.29f / rho_air;
- if (flow_SDP3X < 0.0f) {
- flow_SDP3X = 0.0f;
- }
-
- float dp_pitot = 28557670.0f * (1.0f - 1.0f / (1.0f + (float)powf((flow_SDP3X / 5027611.0f), 1.227924f)));
-
- float press_uncorrected = (press + dp_pitot) / SSL_AIR_DENSITY;
-
- float dv = 0.0331582f * flow_SDP3X;
-
- float ratio = get_airspeed_ratio();
-
-
-
-
- float press_correction = sq(sqrtf(press_uncorrected*ratio)+dv)/ratio - press_uncorrected;
- return (press_uncorrected + press_correction) * sign;
- }
- bool AP_Airspeed_SDP3X::get_differential_pressure(float &pressure)
- {
- uint32_t now = AP_HAL::millis();
- if (now - _last_sample_time_ms > 100) {
- return false;
- }
- {
- WITH_SEMAPHORE(sem);
- if (_press_count > 0) {
- _press = _press_sum / _press_count;
- _press_count = 0;
- _press_sum = 0;
- }
- }
- pressure = _correct_pressure(_press);
- return true;
- }
- bool AP_Airspeed_SDP3X::get_temperature(float &temperature)
- {
- if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
- return false;
- }
- WITH_SEMAPHORE(sem);
- if (_temp_count > 0) {
- _temp = _temp_sum / _temp_count;
- _temp_count = 0;
- _temp_sum = 0;
- }
- temperature = _temp;
- return true;
- }
- bool AP_Airspeed_SDP3X::_crc(const uint8_t data[], unsigned size, uint8_t checksum)
- {
- uint8_t crc_value = 0xff;
-
- for (uint8_t i = 0; i < size; i++) {
- crc_value ^= data[i];
- for (uint8_t bit = 8; bit > 0; --bit) {
- if (crc_value & 0x80) {
- crc_value = (crc_value << 1) ^ 0x31;
- } else {
- crc_value = (crc_value << 1);
- }
- }
- }
-
- return (crc_value == checksum);
- }
|