diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp index d4a01e4f6950055d30ce301f82b2ce0a42cf3f63..931881be09518145107e8519a720a31ef417487c 100644 --- a/src/shared/sensors/VN100/VN100Spi.cpp +++ b/src/shared/sensors/VN100/VN100Spi.cpp @@ -46,6 +46,10 @@ bool VN100Spi::init() return false; } + // First communication after startup might fail + // Send dummy data to clean up + sendDummyPacket(); + if (checkModelNumber() == false) { LOG_ERR(logger, "Got bad CHIPID"); @@ -103,13 +107,25 @@ bool VN100Spi::checkModelNumber() if (err != 0) { // An error occurred while attempting to service the request - LOG_ERR(logger, "Error, cannot get CHIPID"); + LOG_ERR(logger, "Error while reading CHIPID"); return false; } // Check the model number - return strncmp(VN100SpiDefs::MODEL_NUMBER, buf, - strlen(VN100SpiDefs::MODEL_NUMBER)) == 0; + if (strncmp(VN100SpiDefs::MODEL_NUMBER, buf, + strlen(VN100SpiDefs::MODEL_NUMBER)) != 0) + { + LOG_ERR(logger, "Error, invalid CHIPID"); + return false; + } + + return true; +} + +void VN100Spi::sendDummyPacket() +{ + SPITransaction transaction(spiSlave); + transaction.write32(0); } bool VN100Spi::selfTest() diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h index 2fb076c8fae1d3f9962d8f675e016872386dc833..cd105e0b50d7e8ed2cc478aae5d340b54dcfb44c 100644 --- a/src/shared/sensors/VN100/VN100Spi.h +++ b/src/shared/sensors/VN100/VN100Spi.h @@ -74,6 +74,13 @@ private: */ bool checkModelNumber(); + /** + * @brief Utility function used to clean the junk before starting to + * communicate with the sensor. It send a 4 bytes packet of zeros to the + * sensor. + */ + void sendDummyPacket(); + /** * @brief Extracts floating point measurement from the data received from * the sensor.