N3_sub/libraries/AP_Baro/AP_Baro_MPM258.cpp

/*
   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);


}