|
@@ -0,0 +1,210 @@
|
|
|
+/*
|
|
|
+ This program is free software: you can redistribute it and/or modify
|
|
|
+ it under the terms of the GNU General Public License as published by
|
|
|
+ the Free Software Foundation, either version 3 of the License, or
|
|
|
+ (at your option) any later version.
|
|
|
+
|
|
|
+ This program is distributed in the hope that it will be useful,
|
|
|
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
+ GNU General Public License for more details.
|
|
|
+
|
|
|
+ You should have received a copy of the GNU General Public License
|
|
|
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
+ */
|
|
|
+#include "AP_Baro_MPM258.h"
|
|
|
+
|
|
|
+#include <utility>
|
|
|
+#include <stdio.h>
|
|
|
+
|
|
|
+#include <AP_Math/AP_Math.h>
|
|
|
+#include <AP_Math/crc.h>
|
|
|
+#include "../../ArduSub/Sub.h"
|
|
|
+
|
|
|
+extern const AP_HAL::HAL &hal;
|
|
|
+
|
|
|
+static const uint8_t MPM258_ADDR =0x6D;
|
|
|
+
|
|
|
+static const uint8_t MPM258_ADDR_CMD_I2C_WRITE =0xDA;
|
|
|
+static const uint8_t MPM258_ADDR_CMD_I2C_READ= 0xDB;
|
|
|
+
|
|
|
+
|
|
|
+/* write to one of these addresses to start pressure conversion */
|
|
|
+
|
|
|
+static const uint8_t ADDR_CMD_PRESS1= 0x06;
|
|
|
+static const uint8_t ADDR_CMD_PRESS2= 0x07;
|
|
|
+static const uint8_t ADDR_CMD_PRESS3= 0x08;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+/* write to one of these addresses to start temperature conversion */
|
|
|
+static const uint8_t ADDR_CMD_TEMPRATURE1= 0x09;
|
|
|
+static const uint8_t ADDR_CMD_TEMPRATURE2= 0x0A;
|
|
|
+static const uint8_t ADDR_CMD_TEMPRATURE3= 0x0B;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+/*
|
|
|
+ constructor
|
|
|
+ */
|
|
|
+AP_Baro_MPM258::AP_Baro_MPM258(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
|
|
|
+ : AP_Baro_Backend(baro)
|
|
|
+ , _dev(std::move(dev))
|
|
|
+{
|
|
|
+}
|
|
|
+
|
|
|
+AP_Baro_Backend *AP_Baro_MPM258::probe(AP_Baro &baro,
|
|
|
+ AP_HAL::OwnPtr<AP_HAL::Device> dev)
|
|
|
+{
|
|
|
+ if (!dev) {
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ AP_Baro_MPM258 *sensor = new AP_Baro_MPM258(baro, std::move(dev));
|
|
|
+ if (!sensor || !sensor->_init()) {
|
|
|
+ delete sensor;
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ return sensor;
|
|
|
+}
|
|
|
+
|
|
|
+bool AP_Baro_MPM258::_init()
|
|
|
+{
|
|
|
+ //uint8_t press_data[3];
|
|
|
+ //uint8_t temp_data[3];
|
|
|
+
|
|
|
+ if (!_dev) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
|
|
+ AP_HAL::panic("PANIC: AP_Baro_MPM258: failed to take serial semaphore for init");
|
|
|
+ }
|
|
|
+
|
|
|
+ // high retries for init
|
|
|
+ _dev->set_retries(10);
|
|
|
+ //如果不能发送成功说明这个I2C口没有传感器
|
|
|
+ if(!_dev->transfer(&MPM258_ADDR_CMD_I2C_WRITE, 1, nullptr, 0)){
|
|
|
+ _dev->get_semaphore()->give();
|
|
|
+ return false;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ _state = 0;
|
|
|
+ memset(&_accum, 0, sizeof(_accum));
|
|
|
+ _instance = _frontend.register_sensor();
|
|
|
+ _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);
|
|
|
+ // lower retries for run
|
|
|
+ _dev->set_retries(3);
|
|
|
+ _dev->get_semaphore()->give();
|
|
|
+ hal.scheduler->delay(150);
|
|
|
+
|
|
|
+ /* Request 50HZpdate */
|
|
|
+ _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC,
|
|
|
+ FUNCTOR_BIND_MEMBER(&AP_Baro_MPM258::_timer, void));
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+/*
|
|
|
+ * Read the sensor with a state machine
|
|
|
+ * We read one time temperature (state=0) and then 4 times pressure (states 1-4)
|
|
|
+ *
|
|
|
+ * Temperature is used to calculate the compensated pressure and doesn't vary
|
|
|
+ * as fast as pressure. Hence we reuse the same temperature for 4 samples of
|
|
|
+ * pressure.
|
|
|
+*/
|
|
|
+void AP_Baro_MPM258::_timer(void)
|
|
|
+{
|
|
|
+ _read();
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void AP_Baro_MPM258::_update_and_wrap_accumulator(int32_t pressure, int32_t temperature, uint8_t max_count)
|
|
|
+{
|
|
|
+ _accum.sum_pressure += pressure;
|
|
|
+ _accum.sum_temperature += temperature;
|
|
|
+ _accum.num_samples += 1;
|
|
|
+
|
|
|
+ if (_accum.num_samples == max_count) {
|
|
|
+ _accum.sum_pressure /= 2;
|
|
|
+ _accum.sum_temperature /= 2;
|
|
|
+ _accum.num_samples /= 2;
|
|
|
+ }
|
|
|
+}
|
|
|
+bool AP_Baro_MPM258::_read(){
|
|
|
+ uint8_t press_data[3]={0,0,0};
|
|
|
+ uint8_t temp_data[3] = {0,0,0};
|
|
|
+ uint32_t pressure_raw =0;
|
|
|
+ uint32_t temperature_raw =0;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ _dev->transfer(&MPM258_ADDR_CMD_I2C_WRITE, 1, nullptr, 0);
|
|
|
+ _dev->transfer(&ADDR_CMD_PRESS1, 1, nullptr, 0);
|
|
|
+ if(!_dev->transfer(&MPM258_ADDR_CMD_I2C_READ, 0,press_data, 3))
|
|
|
+ {
|
|
|
+ return FALSE;
|
|
|
+ }
|
|
|
+
|
|
|
+ _dev->transfer(&MPM258_ADDR_CMD_I2C_WRITE, 1, nullptr, 0);
|
|
|
+ _dev->transfer(&ADDR_CMD_TEMPRATURE1, 1, nullptr, 0);
|
|
|
+ if(!_dev->transfer(&MPM258_ADDR_CMD_I2C_READ, 0,temp_data, 3))
|
|
|
+ {
|
|
|
+ return FALSE;
|
|
|
+ }
|
|
|
+
|
|
|
+ WITH_SEMAPHORE(_sem);
|
|
|
+ pressure_raw = ((int32_t)press_data[0] << 16) |((int32_t)press_data[1] << 8) | press_data[2];
|
|
|
+ temperature_raw = ((int32_t)temp_data[0] << 16) |((int32_t)temp_data[1] << 8) | temp_data[2];
|
|
|
+ _update_and_wrap_accumulator(pressure_raw, temperature_raw, 128);
|
|
|
+
|
|
|
+ return TRUE;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void AP_Baro_MPM258::update()
|
|
|
+{
|
|
|
+ int32_t sum_pressure, sum_temperature;
|
|
|
+ int32_t num_samples;
|
|
|
+
|
|
|
+ {
|
|
|
+ WITH_SEMAPHORE(_sem);
|
|
|
+
|
|
|
+ if (_accum.num_samples == 0) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ sum_pressure = _accum.sum_pressure;
|
|
|
+ sum_temperature = _accum.sum_temperature;
|
|
|
+ num_samples = _accum.num_samples;
|
|
|
+ memset(&_accum, 0, sizeof(_accum));
|
|
|
+ }
|
|
|
+
|
|
|
+ int32_t raw_pressure_avg = sum_pressure / num_samples;
|
|
|
+ int32_t raw_temperature_avg = sum_temperature / num_samples;
|
|
|
+ if(raw_pressure_avg>8388608){
|
|
|
+ raw_pressure_avg = raw_pressure_avg- 16777216;
|
|
|
+ }
|
|
|
+ if(raw_temperature_avg>8388608){
|
|
|
+ raw_temperature_avg = raw_temperature_avg- 16777216;
|
|
|
+ }
|
|
|
+
|
|
|
+ float pressure = 1000000*((float)raw_pressure_avg/8388608-0.1)/(0.9-0.1);//量程是1MPA
|
|
|
+ float temperature = (float)raw_temperature_avg/65536+25;
|
|
|
+
|
|
|
+ static int16_t cnt = 0;
|
|
|
+ cnt++;
|
|
|
+ if(cnt>20){
|
|
|
+ cnt = 0;
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "pressure %d %d %f %f.",raw_pressure_avg,raw_temperature_avg,pressure,temperature);
|
|
|
+ }
|
|
|
+
|
|
|
+ _copy_to_frontend(_instance, pressure, temperature);
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|