skip non-existing sensor

src/icm20948.cpp

@@ -34,12 +34,17 @@ Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 
 	_activeDevice = spi->addSlave(config->pinCS);
 	_spi = spi;
+	_sensor_ready = false;
 
 	accSensor = new SensorAccel(spi, _activeDevice);
 	gyroSensor = new SensorGyro(spi, _activeDevice);
 	magSensor = new SensorMag(spi, _activeDevice);
 
-	while(!this->icm20948_who_am_i());
+	_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;
@@ -65,6 +70,9 @@ Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 	_sensor_ready = true;
 
 }
+uint8_t Icm20948::IsPresent(void){
+	return (int)_is_present;
+}
 
 void Icm20948::Calibrate(icm20948_Config *config)
 {
@@ -97,6 +105,8 @@ bool Icm20948::icm20948_who_am_i()
 
 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
 	HAL_Delay(100);
 

src/icm20948.h

@@ -106,6 +106,7 @@ private:
 	McuPin_typeDef *_pinINT;
 	bool _ak09916_enable;
 	bool _sensor_ready;
+	bool _is_present;
 
 	bool icm20948_who_am_i();
 	void icm20948_spi_slave_enable();
@@ -140,8 +141,13 @@ public:
 
 	void Stop(void);
 	void Start(void);
+
+	uint8_t IsPresent(void);
 };
 
 
 
+#define ASSERT_SENSOR   {if (_is_present == false) {;}}
+
+
 #endif	/* __ICM20948_H__ */