|
| 1 | +/* |
| 2 | + This program is free software: you can redistribute it and/or modify |
| 3 | + it under the terms of the GNU General Public License as published by |
| 4 | + the Free Software Foundation, either version 3 of the License, or |
| 5 | + (at your option) any later version. |
| 6 | +
|
| 7 | + This program is distributed in the hope that it will be useful, |
| 8 | + but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 9 | + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 10 | + GNU General Public License for more details. |
| 11 | +
|
| 12 | + You should have received a copy of the GNU General Public License |
| 13 | + along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 14 | + */ |
| 15 | + |
| 16 | +/* |
| 17 | + backend driver for airspeed from a I2C AUAV sensor |
| 18 | + */ |
| 19 | +#include "AP_Airspeed_AUAV.h" |
| 20 | + |
| 21 | +#if AP_AIRSPEED_AUAV_ENABLED |
| 22 | + |
| 23 | +#define AUAV_AIRSPEED_I2C_ADDR 0x26 |
| 24 | + |
| 25 | +// the sensor supports a 2nd channel (2nd I2C address) for absolute pressure |
| 26 | +// we could use this as a baro driver but don't yet |
| 27 | +#define AUAV_AIRSPEED_I2C_ADDR_ABSOLUTE 0x27 |
| 28 | + |
| 29 | +// max measurement time for 8x averaging differential is 16.2ms |
| 30 | +#define MEASUREMENT_TIME_MAX_MS 17 |
| 31 | + |
| 32 | +#define MEASUREMENT_TIMEOUT_MS 200 |
| 33 | + |
| 34 | +#include <AP_Common/AP_Common.h> |
| 35 | +#include <AP_HAL/AP_HAL.h> |
| 36 | +#include <AP_HAL/I2CDevice.h> |
| 37 | +#include <AP_Math/AP_Math.h> |
| 38 | +#include <GCS_MAVLink/GCS.h> |
| 39 | + |
| 40 | +extern const AP_HAL::HAL &hal; |
| 41 | + |
| 42 | + |
| 43 | +AP_Airspeed_AUAV::AP_Airspeed_AUAV(AP_Airspeed &_frontend, uint8_t _instance, const float _range_inH2O) : |
| 44 | + AP_Airspeed_Backend(_frontend, _instance), |
| 45 | + range_inH2O(_range_inH2O) |
| 46 | +{ |
| 47 | +} |
| 48 | + |
| 49 | +// probe for a sensor |
| 50 | +bool AP_Airspeed_AUAV::probe(uint8_t bus, uint8_t address) |
| 51 | +{ |
| 52 | + _dev = hal.i2c_mgr->get_device(bus, address); |
| 53 | + if (!_dev) { |
| 54 | + return false; |
| 55 | + } |
| 56 | + |
| 57 | + WITH_SEMAPHORE(_dev->get_semaphore()); |
| 58 | + |
| 59 | + _dev->set_retries(10); |
| 60 | + _measure(); |
| 61 | + hal.scheduler->delay(20); |
| 62 | + _collect(); |
| 63 | + |
| 64 | + return last_sample_time_ms != 0; |
| 65 | + |
| 66 | +} |
| 67 | + |
| 68 | +// probe and initialise the sensor |
| 69 | +bool AP_Airspeed_AUAV::init() |
| 70 | +{ |
| 71 | + const auto bus = get_bus(); |
| 72 | + if (!probe(bus, AUAV_AIRSPEED_I2C_ADDR)) { |
| 73 | + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AUAV AIRSPEED[%u]: no sensor found on bus %u", get_instance(), bus); |
| 74 | + return false; |
| 75 | + } |
| 76 | + |
| 77 | + _dev->set_device_type(uint8_t(DevType::AUAV)); |
| 78 | + set_bus_id(_dev->get_bus_id()); |
| 79 | + |
| 80 | + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AUAV AIRSPEED[%u]: Found bus %u addr 0x%02x", get_instance(), _dev->bus_num(), _dev->get_bus_address()); |
| 81 | + |
| 82 | + WITH_SEMAPHORE(_dev->get_semaphore()); |
| 83 | + |
| 84 | + // drop to 2 retries for runtime |
| 85 | + _read_coefficients(); |
| 86 | + _dev->set_retries(2); |
| 87 | + _dev->register_periodic_callback(20000, |
| 88 | + FUNCTOR_BIND_MEMBER(&AP_Airspeed_AUAV::_timer, void)); |
| 89 | + return true; |
| 90 | +} |
| 91 | + |
| 92 | +// start a measurement |
| 93 | +void AP_Airspeed_AUAV::_measure() |
| 94 | +{ |
| 95 | + measurement_started_ms = 0; |
| 96 | + uint8_t cmd = 0xAE; // start average8, max 16.2ms measurement time |
| 97 | + if (_dev->transfer(&cmd, 1, nullptr, 0)) { |
| 98 | + measurement_started_ms = AP_HAL::millis(); |
| 99 | + } |
| 100 | +} |
| 101 | + |
| 102 | +// read the values from the sensor |
| 103 | +void AP_Airspeed_AUAV::_collect() |
| 104 | +{ |
| 105 | + measurement_started_ms = 0; // It should always get reset by _measure. This is a safety to handle failures of i2c bus |
| 106 | + uint8_t inbuf[7]; |
| 107 | + if (!_dev->read((uint8_t *)&inbuf, sizeof(inbuf))) { |
| 108 | + return; |
| 109 | + } |
| 110 | + const uint8_t status = inbuf[0]; |
| 111 | + const uint8_t healthy_status = 0x50; // power on, not busy, differential normal, not out of range |
| 112 | + if (status != healthy_status) { |
| 113 | + // not ready or error |
| 114 | + return; |
| 115 | + } |
| 116 | + const int32_t Tref_Counts = 7576807; // temperature counts at 25C |
| 117 | + const float TC50Scale = 100.0f * 100.0f * 167772.2f; // scale TC50 to 1.0% FS0 |
| 118 | + float Pdiff, TC50; |
| 119 | + |
| 120 | + // Convert unsigned 24-bit pressure value to signed +/- 23-bit: |
| 121 | + const int32_t iPraw = (inbuf[1]<<16) + (inbuf[2]<<8) + inbuf[3] - 0x800000; |
| 122 | + |
| 123 | + // Convert signed 23-bit valu11e to float, normalized to +/- 1.0: |
| 124 | + const float Pnorm = float(iPraw) / float(0x7FFFFF); |
| 125 | + const float AP3 = DLIN_A * Pnorm * Pnorm * Pnorm; // A*Pout^3 |
| 126 | + const float BP2 = DLIN_B * Pnorm * Pnorm; // B*Pout^2 |
| 127 | + const float CP = DLIN_C * Pnorm; // C*POut |
| 128 | + const float Corr = AP3 + BP2 + CP + DLIN_D; // Linearity correction term |
| 129 | + const float Pcorr = Pnorm + Corr; // Corrected P, range +/-1.0. |
| 130 | + |
| 131 | + // Compute difference from reference temperature, in sensor counts: |
| 132 | + const int32_t iTemp = (inbuf[4]<<16) + (inbuf[5]<<8) + inbuf[6]; // 24-bit temperature |
| 133 | + |
| 134 | + // get temperature in degrees C |
| 135 | + temp_C = (iTemp * 155) / float(1U<<24) - 45; |
| 136 | + |
| 137 | + const int32_t Tdiff = iTemp - Tref_Counts; // see constant defined above. |
| 138 | + const float Pnfso = (Pcorr + 1.0f) * 0.5; |
| 139 | + |
| 140 | + //TC50: Select High/Low, based on current temp above/below 25C: |
| 141 | + if (Tdiff > 0) { |
| 142 | + TC50 = D_TC50H; |
| 143 | + } else { |
| 144 | + TC50 = D_TC50L; |
| 145 | + } |
| 146 | + |
| 147 | + // Find absolute difference between midrange and reading (abs(Pnfso-0.5)): |
| 148 | + if (Pnfso > 0.5) { |
| 149 | + Pdiff = Pnfso - 0.5; |
| 150 | + } else { |
| 151 | + Pdiff = 0.5f - Pnfso; |
| 152 | + } |
| 153 | + |
| 154 | + const float Tcorr = (1.0f - (D_Es * 2.5f * Pdiff)) * Tdiff * TC50 / TC50Scale; |
| 155 | + const float PCorrt = Pnfso - Tcorr; // corrected P: float, [0 to +1.0) |
| 156 | + const uint32_t PComp = (uint32_t) (PCorrt * (float)0xFFFFFF); |
| 157 | + pressure_digital = PComp; |
| 158 | + |
| 159 | + last_sample_time_ms = AP_HAL::millis(); |
| 160 | +} |
| 161 | + |
| 162 | +uint32_t AP_Airspeed_AUAV::_read_register(uint8_t cmd) |
| 163 | +{ |
| 164 | + uint8_t raw_bytes1[3]; |
| 165 | + if (!_dev->transfer(&cmd,1,(uint8_t *)&raw_bytes1, sizeof(raw_bytes1))) { |
| 166 | + return 0; |
| 167 | + } |
| 168 | + uint8_t raw_bytes2[3]; |
| 169 | + uint8_t cmd2 = cmd + 1; |
| 170 | + if (!_dev->transfer(&cmd2,1,(uint8_t *)&raw_bytes2, sizeof(raw_bytes2))) { |
| 171 | + return 0; |
| 172 | + } |
| 173 | + uint32_t result = ((uint32_t)raw_bytes1[1] << 24) | ((uint32_t)raw_bytes1[2] << 16) | ((uint32_t)raw_bytes2[1] << 8) | (uint32_t)raw_bytes2[2]; |
| 174 | + return result; |
| 175 | +} |
| 176 | + |
| 177 | +bool AP_Airspeed_AUAV::_read_coefficients() |
| 178 | +{ |
| 179 | + // Differential Coefficients |
| 180 | + int32_t i32A = 0, i32B =0, i32C =0, i32D=0, i32TC50HLE=0; |
| 181 | + int8_t i8TC50H = 0, i8TC50L = 0, i8Es = 0; |
| 182 | + i32A = _read_register(0x2B); |
| 183 | + DLIN_A = ((float)(i32A))/((float)(0x7FFFFFFF)); |
| 184 | + |
| 185 | + i32B = _read_register(0x2D); |
| 186 | + DLIN_B = (float)(i32B)/(float)(0x7FFFFFFF); |
| 187 | + |
| 188 | + i32C = _read_register(0x2F); |
| 189 | + DLIN_C = (float)(i32C)/(float)(0x7FFFFFFF); |
| 190 | + |
| 191 | + i32D = _read_register(0x31); |
| 192 | + DLIN_D = (float)(i32D)/(float)(0x7FFFFFFF); |
| 193 | + |
| 194 | + i32TC50HLE = _read_register(0x33); |
| 195 | + i8TC50H = (i32TC50HLE >> 24) & 0xFF; // 55 H |
| 196 | + i8TC50L = (i32TC50HLE >> 16) & 0xFF; // 55 L |
| 197 | + i8Es = (i32TC50HLE ) & 0xFF; // 56 L |
| 198 | + D_Es = (float)(i8Es)/(float)(0x7F); |
| 199 | + D_TC50H = (float)(i8TC50H)/(float)(0x7F); |
| 200 | + D_TC50L = (float)(i8TC50L)/(float)(0x7F); |
| 201 | + |
| 202 | + return true; //Need to actually check |
| 203 | +} |
| 204 | + |
| 205 | +// 50Hz timer |
| 206 | +void AP_Airspeed_AUAV::_timer() |
| 207 | +{ |
| 208 | + if (measurement_started_ms == 0) { |
| 209 | + _measure(); |
| 210 | + } |
| 211 | + if ((AP_HAL::millis() - measurement_started_ms) > MEASUREMENT_TIME_MAX_MS) { |
| 212 | + _collect(); |
| 213 | + // start a new measurement |
| 214 | + _measure(); |
| 215 | + } |
| 216 | +} |
| 217 | + |
| 218 | +// return the current differential_pressure in Pascal |
| 219 | +bool AP_Airspeed_AUAV::get_differential_pressure(float &_pressure) |
| 220 | +{ |
| 221 | + WITH_SEMAPHORE(sem); |
| 222 | + _pressure = 248.8f*1.25f*((pressure_digital-8388608)/16777216.0f) * 2 * range_inH2O; |
| 223 | + return AP_HAL::millis() - last_sample_time_ms < MEASUREMENT_TIMEOUT_MS; |
| 224 | +} |
| 225 | + |
| 226 | +// return the current temperature in degrees C, if available |
| 227 | +bool AP_Airspeed_AUAV::get_temperature(float &_temperature) |
| 228 | +{ |
| 229 | + WITH_SEMAPHORE(sem); |
| 230 | + _temperature = temp_C; |
| 231 | + return AP_HAL::millis() - last_sample_time_ms < MEASUREMENT_TIMEOUT_MS; |
| 232 | +} |
| 233 | + |
| 234 | +#endif // AP_AIRSPEED_AUAV_ENABLED |
0 commit comments