uprava pre nbus

src/IcmSpiManager.cpp

@@ -22,6 +22,10 @@ uint8_t IcmSpiManager::addSlave(McuPin_typeDef *pin)
 	return _numDevices-1;
 }
 
+IcmSpiManager* IcmSpiManager::getInstance(){
+	return this;
+}
+
 void IcmSpiManager::cs_low(uint8_t index)
 {
 	_pinCS[index]->port->BRR = (uint32_t)_pinCS[index]->pin;

src/IcmSpiManager.h

@@ -225,6 +225,8 @@ public:
     void write_single_external_reg(uint8_t sensorNum, uint8_t reg, uint8_t val);
     uint8_t* read_multiple_external_reg(uint8_t sensorNum, uint8_t reg, uint8_t len);
 
+    IcmSpiManager* getInstance();
+
 
 };
 

src/icm20948.cpp

@@ -29,11 +29,55 @@ uint16_t ACCEL_SampleRate_Table[15] = {
 	4095,
 };
 
+Icm20948::Icm20948(){
+	_activeDevice = -1;
+	_spi_manager = NULL;
+	_sensor_ready = false;
+}
+
+void Icm20948::setDevice(IcmSpiManager *spi, icm20948_Config *config) {
+	_activeDevice = spi->addSlave(config->pinCS);
+	_spi_manager = spi->getInstance();
+	_sensor_ready = false;
+
+	accSensor = new SensorAccel(spi, _activeDevice);
+	gyroSensor = new SensorGyro(spi, _activeDevice);
+	magSensor = new SensorMag(spi, _activeDevice);
+
+	_is_present = this->icm20948_who_am_i();
+	if (_is_present == false){
+		return;
+	}
+//	while(!this->icm20948_who_am_i());
+
+	this->accSensor->data.type = ICM20948_ACCEL;		// + _activeDevice;
+	this->gyroSensor->data.type = ICM20948_GYRO;		// + _activeDevice;
+	this->magSensor->data.type = ICM20948_MAG;			// + _activeDevice;
+
+	this->Reset();
+	this->Wakeup();
+
+	this->SetInterruptSource(config->int_source);
+
+	this->gyroSensor->SetLowPassFilter(config->gyro.low_pass_filter);
+	this->accSensor->SetLowPassFilter(config->accel.low_pass_filter);
+
+	this->gyroSensor->SetSampleRate(config->gyro.sample_rate);
+	this->accSensor->SetSampleRate(config->accel.sample_rate);
+
+	this->accSensor->SetRange(config->accel.full_scale);
+	this->gyroSensor->SetRange(config->gyro.full_scale);
+
+	this->ak09916_init(&config->mag);
+
+
+	_sensor_ready = true;
+}
 
 Icm20948::Icm20948(IcmSpiManager *spi, icm20948_Config *config){
 
 	_activeDevice = spi->addSlave(config->pinCS);
-	_spi = spi;
+	_spi_manager = spi;
 	_sensor_ready = false;
 
 	accSensor = new SensorAccel(spi, _activeDevice);
@@ -98,7 +142,7 @@ void Icm20948::Calibrate(icm20948_Config *config)
 
 bool Icm20948::icm20948_who_am_i()
 {
-	uint8_t icm20948_id = _spi->read_single_reg(_activeDevice, ub_0, B0_WHO_AM_I);
+	uint8_t icm20948_id = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_WHO_AM_I);
 
 	if(icm20948_id == ICM20948_ID)
 		return true;
@@ -110,7 +154,7 @@ void Icm20948::Reset(void)
 {
 	ASSERT_SENSOR;
 
-	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, 0xC1); // reset - 0x80, sleep 0x40, source PLL auto 0x01
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, 0xC1); // reset - 0x80, sleep 0x40, source PLL auto 0x01
 	HAL_Delay(100);
 
 	this->Wakeup();
@@ -124,16 +168,16 @@ void Icm20948::Reset(void)
 
 void Icm20948::Wakeup(void)
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
 	new_val &= 0xBF;
 
-	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
 	HAL_Delay(50);
 }
 
 uint8_t Icm20948::IsSleep(void)
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
 	return new_val & 0x40;
 }
 
@@ -145,87 +189,87 @@ bool Icm20948::IsReady(void)
 void Icm20948::Stop(void)
 {
 //	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x3F);	// vypne vsetky senzory
-	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, 0x0);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, 0x0);
 	_sensor_ready = false;
 }
 
 void Icm20948::Start(void)
 {
 //	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x0);
-	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, 0x1);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, 0x1);
 	_sensor_ready = true;
 }
 
 void Icm20948::Sleep(void)
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
 	new_val |= 0x40;
 
-	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
 	HAL_Delay(100);
 }
 
 void Icm20948::icm20948_spi_slave_enable()
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
 	new_val |= 0x10;
 
-	_spi->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
 }
 
 void Icm20948::icm20948_i2c_master_reset()
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
 	new_val |= 0x02;
 
-	_spi->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
 }
 
 void Icm20948::icm20948_i2c_master_enable()
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
 	new_val |= 0x20;
 
-	_spi->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
 	HAL_Delay(100);
 }
 
 
 void Icm20948::icm20948_i2c_master_clk_frq(uint8_t config)
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_3, B3_I2C_MST_CTRL);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_3, B3_I2C_MST_CTRL);
 	new_val |= config;
 
-	_spi->write_single_reg(_activeDevice, ub_3, B3_I2C_MST_CTRL, new_val);
+	_spi_manager->write_single_reg(_activeDevice, ub_3, B3_I2C_MST_CTRL, new_val);
 }
 
 void Icm20948::icm20948_clock_source(uint8_t source)
 {
-	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
 	new_val |= source;
 
-	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
 }
 
 void Icm20948::icm20948_odr_align_enable()
 {
-	_spi->write_single_reg(_activeDevice, ub_2, B2_ODR_ALIGN_EN, 0x01);
+	_spi_manager->write_single_reg(_activeDevice, ub_2, B2_ODR_ALIGN_EN, 0x01);
 }
 
 void Icm20948::SetInterruptSource(interrupt_source_enum int_source)
 {
-	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_PIN_CFG, (INT_PIN_CFG_2CLEAR));  // ( INT_PIN_CFG_LATCH | ...
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_INT_PIN_CFG, (INT_PIN_CFG_2CLEAR));  // ( INT_PIN_CFG_LATCH | ...
     uint8_t source1 = (uint8_t)(int_source & 0xFF);
     uint8_t source2 = (uint8_t)((int_source >> 8) & 0xFF);
-	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE, source1);
-	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, source2);
+    _spi_manager->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE, source1);
+    _spi_manager->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, source2);
 
 }
 
 
 void Icm20948::Read(void)
 {
-	uint8_t* temp =_spi->read_multiple_reg(_activeDevice, ub_0, B0_ACCEL_XOUT_H, 12);
+	uint8_t* temp =_spi_manager->read_multiple_reg(_activeDevice, ub_0, B0_ACCEL_XOUT_H, 12);
 
 	this->accSensor->data.x = (int16_t)(temp[0] << 8 | temp[1]);
 	this->accSensor->data.y = (int16_t)(temp[2] << 8 | temp[3]);
@@ -281,14 +325,14 @@ void Icm20948::ak09916_init(Config_Mag_t *config)
 
 void Icm20948::ak09916_soft_reset()
 {
-	_spi->write_single_external_reg(_activeDevice, MAG_CNTL3, 0x01);
+	_spi_manager->write_single_external_reg(_activeDevice, MAG_CNTL3, 0x01);
 	HAL_Delay(100);
 }
 
 
 bool Icm20948::ak09916_who_am_i()
 {
-	uint8_t ak09916_id = _spi->read_single_external_reg(_activeDevice, MAG_WIA2);
+	uint8_t ak09916_id = _spi_manager->read_single_external_reg(_activeDevice, MAG_WIA2);
 
 	if(ak09916_id == AK09916_ID)
 		return true;
@@ -298,7 +342,7 @@ bool Icm20948::ak09916_who_am_i()
 
 void Icm20948::ak09916_operation_mode_setting(AK09916_operation_mode mode)
 {
-	_spi->write_single_external_reg(_activeDevice, MAG_CNTL2, mode);
+	_spi_manager->write_single_external_reg(_activeDevice, MAG_CNTL2, mode);
 	HAL_Delay(100);
 }
 

src/icm20948.h

@@ -102,7 +102,7 @@ class Icm20948{
 private:
 	uint8_t _numDevices;
 	int8_t _activeDevice;
-	IcmSpiManager *_spi;
+	IcmSpiManager *_spi_manager;
 	McuPin_typeDef *_pinINT;
 	bool _ak09916_enable;
 	bool _sensor_ready;
@@ -126,7 +126,9 @@ public:
 	SensorGyro *gyroSensor;
 	SensorMag *magSensor;
 
+    Icm20948();
     Icm20948(IcmSpiManager *spi, icm20948_Config *config);
+    void setDevice(IcmSpiManager *spi, icm20948_Config *config);
     void SetInterruptSource(interrupt_source_enum int_source);
     void Calibrate(icm20948_Config *config);
     void Read(void);