PN532 Trying to obtain Model number of device from Felica tags - c++

I'm just an Arduino Uno to obtain the device number from Felica tags but the memory would just output 0xff. I'm also using I2C instead of SPI and I'm not sure if this matters.
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_PN532.h>
// If using the breakout with SPI, define the pins for SPI communication.
#define PN532_SCK (2)
#define PN532_MOSI (3)
#define PN532_SS (4)
#define PN532_MISO (5)
// If using the breakout or shield with I2C, define just the pins connected
// to the IRQ and reset lines. Use the values below (2, 3) for the shield!
#define PN532_IRQ (2)
#define PN532_RESET (3) // Not connected by default on the NFC Shield
// Uncomment just _one_ line below depending on how your breakout or shield
// is connected to the Arduino:
// Use this line for a breakout with a SPI connection:
//Adafruit_PN532 nfc(PN532_SCK, PN532_MISO, PN532_MOSI, PN532_SS);
// Use this line for a breakout with a hardware SPI connection. Note that
// the PN532 SCK, MOSI, and MISO pins need to be connected to the Arduino's
// hardware SPI SCK, MOSI, and MISO pins. On an Arduino Uno these are
// SCK = 13, MOSI = 11, MISO = 12. The SS line can be any digital IO pin.
//Adafruit_PN532 nfc(PN532_SS);
// Or use this line for a breakout or shield with an I2C connection:
Adafruit_PN532 nfc(PN532_IRQ, PN532_RESET);
#if defined(ARDUINO_ARCH_SAMD)
// for Zero, output on USB Serial console, remove line below if using programming port to program the Zero!
// also change #define in Adafruit_PN532.cpp library file
#define Serial SerialUSB
#endif
uint8_t _prevIDm[8]; //IDm of the card previously detected
unsigned long _prevTime;
void setup(void) {
#ifndef ESP8266
while (!Serial); // for Leonardo/Micro/Zero
#endif
Serial.begin(115200);
Serial.println("Hello!");
nfc.begin();
uint32_t versiondata = nfc.getFirmwareVersion();
if (! versiondata) {
Serial.print("Didn't find PN53x board");
while (1); // halt
}
// Got ok data, print it out!
Serial.print("Found chip PN5"); Serial.println((versiondata>>24) & 0xFF, HEX);
Serial.print("Firmware ver. "); Serial.print((versiondata>>16) & 0xFF, DEC);
Serial.print('.'); Serial.println((versiondata>>8) & 0xFF, DEC);
// Set the max number of retry attempts to read from a card
// This prevents us from waiting forever for a card, which is
// the default behaviour of the PN532.
nfc.setPassiveActivationRetries(0xFF);
// configure board to read RFID tags
nfc.SAMConfig();
Serial.println("Waiting for an FeliCa card");
}
void loop(void) {
uint8_t ret;
uint16_t systemCode = 0xFFFF;
uint8_t requestCode = 0x01; // System Code request
uint8_t idm[8];
uint8_t pmm[8];
uint16_t systemCodeResponse;
// Wait for an FeliCa type cards.
// When one is found, some basic information such as IDm, PMm, and System Code are retrieved.
Serial.print("Waiting for an FeliCa card... ");
ret = nfc.felica_Polling(systemCode, requestCode, idm, pmm, &systemCodeResponse, 5000);
if (ret != 1)
{
Serial.println("Could not find a card");
delay(500);
return;
}
if ( memcmp(idm, _prevIDm, 8) == 0 ) {
if ( (millis() - _prevTime) < 3000 ) {
Serial.println("Same card");
delay(500);
return;
}
}
Serial.println("Found a card!");
Serial.print(" IDm: ");
nfc.PrintHex(idm, 8);
Serial.print(" PMm: ");
nfc.PrintHex(pmm, 8);
Serial.print(" System Code: ");
nfc.PrintHex16(systemCodeResponse);
Serial.print("\n");
memcpy(_prevIDm, idm, 8);
_prevTime = millis();
Serial.print("Request Service command -> ");
//Code to indicate System FFFFh
uint16_t nodeCodeList[3] = {0x0000, 0x1000, 0xFFFF};
uint16_t keyVersions[3];
ret = nfc.felica_RequestService(3, nodeCodeList, keyVersions);
if (ret != 1)
{
Serial.println("error");
} else {
Serial.println("OK!");
for(int i=0; i<3; i++ ) {
Serial.print(" Node Code: "); nfc.PrintHex16(nodeCodeList[i]);
Serial.print(" -> Key Version: "); nfc.PrintHex16(keyVersions[i]);
Serial.println("");
}
}
Serial.print("Read Without Encryption command -> ");
uint8_t blockData[3][16];
uint16_t serviceCodeList[1] = {0x000B};
uint16_t blockList[3] = {0x8000, 0x8001, 0x8002};
ret = nfc.felica_ReadWithoutEncryption(1, serviceCodeList, 3, blockList, blockData);
if (ret != 1)
{
Serial.println("error");
} else {
Serial.println("OK!");
for(int i=0; i<3; i++ ) {
Serial.print(" Block no. "); Serial.print(i, DEC); Serial.print(": ");
nfc.PrintHex(blockData[i], 16);
}
}
Serial.print("Request Response command -> ");
uint8_t mode;
ret = nfc.felica_RequestResponse(&mode);
if (ret != 1)
{
Serial.println("error");
} else {
Serial.println("OK!");
Serial.print(" mode: "); Serial.println(mode, DEC);
}
Serial.print("Request System Code command -> ");
uint8_t numSystemCode;
uint16_t systemCodeList[16];
ret = nfc.felica_RequestSystemCode(&numSystemCode, systemCodeList);
if (ret != 1)
{
Serial.println("error");
} else {
Serial.println("OK!");
for(int i=0; i< numSystemCode; i++) {
Serial.print(" System code: "); nfc.PrintHex16(systemCodeList[i]); Serial.println("");
}
}
// Wait 1 second before continuing
Serial.println("Card access completed!\n");
//delay(1000);
}
Here is my library .cpp
/***** FeliCa Functions ******/
/**************************************************************************/
/*!
#brief Poll FeliCa card. PN532 acting as reader/initiator,
peer acting as card/responder.
#param[in] systemCode Designation of System Code. When sending FFFFh as System Code,
all the FeliCa cards can return response.
#param[in] requestCode Designation of Request Data as follows:
00h: No Request
01h: System Code request (to acquire System Code of the card)
02h: Communication perfomance request
#param[out] idm IDm of the card (8 bytes)
#param[out] pmm PMm of the card (8 bytes)
#param[out] systemCodeResponse System Code of the card (2bytes)
#return 1: A FeliCa card has detected, 0: No card has detected, -1: error
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_Polling(uint16_t systemCode, uint8_t requestCode,
uint8_t *idm, uint8_t *pmm, uint16_t *systemCodeResponse, uint16_t timeout)
{
pn532_packetbuffer[0] = PN532_COMMAND_INLISTPASSIVETARGET;
pn532_packetbuffer[1] = 1;
pn532_packetbuffer[2] = 1;
pn532_packetbuffer[3] = FELICA_CMD_POLLING;
pn532_packetbuffer[4] = (systemCode >> 8) & 0xff;
pn532_packetbuffer[5] = systemCode & 0xFF;
pn532_packetbuffer[6] = requestCode;
pn532_packetbuffer[7] = 0;
if (!sendCommandCheckAck(pn532_packetbuffer,8,timeout)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Could not send Polling command"));
#endif
return -1;
}
// Wait card response
if (!waitready(timeout)) {
return -2;
}
readdata(pn532_packetbuffer, 9+20+2);
if ( !felica_checkResponse(PN532_RESPONSE_INLISTPASSIVETARGET) ) {
return -2;
}
// Check NbTg (pn532_packetbuffer[7])
if (pn532_packetbuffer[7] == 0) {
// No card have found
return 0;
} else if (pn532_packetbuffer[7] != 1) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.print (F("Unhandled number of targets inlisted. NbTg = "));
PN532DEBUGPRINT.println(pn532_packetbuffer[7], HEX);
#endif
return -3;
}
_inListedTag = pn532_packetbuffer[8];
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Tag number: "));
PN532DEBUGPRINT.println(_inListedTag);
#endif
// length check
uint8_t responseLength = pn532_packetbuffer[9];
if (responseLength != 18 && responseLength != 20) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Wrong response length"));
#endif
return -4;
}
uint8_t i;
for (i=0; i<8; ++i) {
idm[i] = pn532_packetbuffer[11+i];
_felicaIDm[i] = pn532_packetbuffer[11+i];
pmm[i] = pn532_packetbuffer[19+i];
_felicaPMm[i] = pn532_packetbuffer[19+i];
}
if ( responseLength == 20 ) {
*systemCodeResponse = (uint16_t)((pn532_packetbuffer[26] << 8) + pn532_packetbuffer[27]);
}
return 1;
}
/**************************************************************************/
/*!
#brief Sends an FeliCa command with the currently inlisted peer
#param[in] commandBuf FeliCa command packet. (e.g. 00 FF FF 00 00 for Polling command)
#param[in] commandlength Length of the FeliCa command packet. (e.g. 0x05 for above Polling command )
#param[out] responseBuf FeliCa response packet. (e.g. 01 NFCID2(8 bytes) PAD for Polling response)
#param[out] responselength Length of the FeliCa response packet. (e.g. 0x12 for above Polling command )
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_SendCommand (const uint8_t *commandBuf, uint8_t commandlength,
uint8_t *responseBuf, uint8_t expectedResLen, uint8_t *responseLength)
{
if (commandlength > PN532_PACKBUFFSIZ-3) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Command length too long for packet buffer"));
#endif
return -1;
}
pn532_packetbuffer[0] = 0x40; // PN532_COMMAND_INDATAEXCHANGE;
pn532_packetbuffer[1] = _inListedTag;
pn532_packetbuffer[2] = commandlength + 1;
memcpy(&pn532_packetbuffer[3], commandBuf, commandlength);
if (!sendCommandCheckAck(pn532_packetbuffer,commandlength+3,200)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Could not send Command"));
#endif
return -2;
}
// Wait card response ( longer than 102.4ms )
if (!waitready(110)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Could not receive response"));
#endif
return -3;
}
readdata(pn532_packetbuffer,expectedResLen);
if ( !felica_checkResponse(PN532_RESPONSE_INDATAEXCHANGE) ) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Could not receive response"));
#endif
return -3;
}
// Check status (pn532_packetbuffer[7])
if ((pn532_packetbuffer[7] & 0x3F)!=0) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Status code indicates an error: "));
PN532DEBUGPRINT.println(pn532_packetbuffer[7], HEX);
#endif
return -4;
}
// length check
*responseLength = pn532_packetbuffer[8] - 1;
if (pn532_packetbuffer[3] - 4 != *responseLength) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Wrong response length"));
#endif
return -5;
}
memcpy(responseBuf, &pn532_packetbuffer[9], *responseLength);
return 1;
}
/**************************************************************************/
/*!
#brief Sends FeliCa Request Service command
#param[in] numNode Number of Node
#param[in] nodeCodeList Node Code List (Big Endian)
#param[out] keyVersions Key Version of each Node (Big Endian)
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_RequestService(uint8_t numNode, uint16_t *nodeCodeList, uint16_t *keyVersions)
{
if (numNode > FELICA_REQ_SERVICE_MAX_NODE_NUM) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("numNode is too large"));
#endif
return -1;
}
uint8_t i, j=0;
uint8_t cmdLen = 1 + 8 + 1 + 2*numNode;
uint8_t cmd[cmdLen];
cmd[j++] = FELICA_CMD_REQUEST_SERVICE;
for (i=0; i<8; ++i) {
cmd[j++] = _felicaIDm[i];
}
cmd[j++] = numNode;
for (i=0; i<numNode; ++i) {
cmd[j++] = nodeCodeList[i] & 0xFF;
cmd[j++] = (nodeCodeList[i] >> 8) & 0xff;
}
uint8_t responseBuf[10+2*numNode];
uint8_t responseLength;
if (!felica_SendCommand(cmd, cmdLen, responseBuf, 8 + 11 + 2*numNode + 2, &responseLength)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Request Service command failed"));
#endif
return -2;
}
// length check
if ( responseLength != 10+2*numNode ) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Request Service command failed (wrong response length)"));
#endif
return -3;
}
for(i=0; i<numNode; i++) {
keyVersions[i] = (uint16_t)(responseBuf[10+i*2] + (responseBuf[10+i*2+1] << 8));
}
return 1;
}
/**************************************************************************/
/*!
#brief Sends FeliCa Request Service command
#param[out] mode Current Mode of the card
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_RequestResponse(uint8_t *mode)
{
uint8_t cmd[9];
cmd[0] = FELICA_CMD_REQUEST_RESPONSE;
memcpy(&cmd[1], _felicaIDm, 8);
uint8_t responseBuf[10];
uint8_t responseLength;
if (!felica_SendCommand(cmd, 9, responseBuf, 8 + 11 + 2, &responseLength)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Request Response command failed"));
#endif
return -1;
}
// length check
if ( responseLength != 10) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Request Response command failed (wrong response length)"));
#endif
return -2;
}
*mode = responseBuf[9];
return 1;
}
/**************************************************************************/
/*!
#brief Sends FeliCa Read Without Encryption command
#param[in] numService Number of Service
#param[in] serviceCodeList Service Code List (Big Endian)
#param[in] numBlock Number of Block (This API only accepts 2-byte block list element)
#param[in] blockList Block List
#param[out] blockData Block Data
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_ReadWithoutEncryption (uint8_t numService, const uint16_t *serviceCodeList,
uint8_t numBlock, const uint16_t *blockList, uint8_t blockData[][16])
{
if (numService > FELICA_READ_MAX_SERVICE_NUM) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("numService is too large"));
#endif
return -1;
}
if (numBlock > FELICA_READ_MAX_BLOCK_NUM) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("numBlock is too large"));
#endif
return -2;
}
uint8_t i, j=0;
uint8_t cmdLen = 1 + 8 + 1 + 2*numService + 1 + 2*numBlock;
uint8_t cmd[cmdLen];
cmd[j++] = FELICA_CMD_READ_WITHOUT_ENCRYPTION;
for (i=0; i<8; ++i) {
cmd[j++] = _felicaIDm[i];
}
cmd[j++] = numService;
for (i=0; i<numService; ++i) {
cmd[j++] = serviceCodeList[i] & 0xFF;
cmd[j++] = (serviceCodeList[i] >> 8) & 0xff;
}
cmd[j++] = numBlock;
for (i=0; i<numBlock; ++i) {
cmd[j++] = (blockList[i] >> 8) & 0xFF;
cmd[j++] = blockList[i] & 0xff;
}
uint8_t responseBuf[12+16*numBlock];
uint8_t responseLength;
if (!felica_SendCommand(cmd, cmdLen, responseBuf, 8+13+16*numBlock + 2, &responseLength)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Read Without Encryption command failed"));
#endif
return -3;
}
// length check
if ( responseLength != 12+16*numBlock ) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Read Without Encryption command failed (wrong response length)"));
#endif
return -4;
}
// status flag check
if ( responseBuf[9] != 0 || responseBuf[10] != 0 ) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Read Without Encryption command failed (Status Flag: "));
PrintHex8(pn532_packetbuffer[9]);
PrintHex8(pn532_packetbuffer[10]);
PN532DEBUGPRINT.println(F(")"));
#endif
return -5;
}
int k = 12;
for(i=0; i<numBlock; i++ ) {
for(j=0; j<16; j++ ) {
blockData[i][j] = responseBuf[k++];
}
}
return 1;
}
/**************************************************************************/
/*!
#brief Sends FeliCa Write Without Encryption command
#param[in] numService Number of Service.
#param[in] serviceCodeList Service Code List.
#param[in] numBlock Number of Block. (This API only accepts 2-byte block list element)
#param[in] blockList Block List.
#param[out] blockData Block Data.
#return = 1: Success
< 0: error
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_WriteWithoutEncryption (uint8_t numService, const uint16_t *serviceCodeList,
uint8_t numBlock, const uint16_t *blockList, uint8_t blockData[][16])
{
if (numService > FELICA_WRITE_MAX_SERVICE_NUM) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("numService is too large"));
#endif
}
if (numBlock > FELICA_WRITE_MAX_BLOCK_NUM) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("numBlock is too large"));
#endif
return -2;
}
uint8_t i, j=0, k;
uint8_t cmdLen = 1 + 8 + 1 + 2*numService + 1 + 2*numBlock + 16 * numBlock;
uint8_t cmd[cmdLen];
cmd[j++] = FELICA_CMD_WRITE_WITHOUT_ENCRYPTION;
for (i=0; i<8; ++i) {
cmd[j++] = _felicaIDm[i];
}
cmd[j++] = numService;
for (i=0; i<numService; ++i) {
cmd[j++] = serviceCodeList[i] & 0xFF;
cmd[j++] = (serviceCodeList[i] >> 8) & 0xff;
}
cmd[j++] = numBlock;
for (i=0; i<numBlock; ++i) {
cmd[j++] = (blockList[i] >> 8) & 0xFF;
cmd[j++] = blockList[i] & 0xff;
}
for (i=0; i<numBlock; ++i) {
for(k=0; k<16; k++) {
cmd[j++] = blockData[i][k];
}
}
uint8_t response[11];
uint8_t responseLength;
if (felica_SendCommand(cmd, cmdLen, response, 8+12+2, &responseLength) != 1) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Write Without Encryption command failed"));
#endif
return -3;
}
// length check
if ( responseLength != 11 ) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Write Without Encryption command failed (wrong response length)"));
#endif
return -4;
}
// status flag check
if ( response[9] != 0 || response[10] != 0 ) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Write Without Encryption command failed (Status Flag: "));
PrintHex8(pn532_packetbuffer[9]);
PrintHex8(pn532_packetbuffer[10]);
PN532DEBUGPRINT.println(F(")"));
#endif
return -5;
}
return 1;
}
/**************************************************************************/
/*!
#brief Sends FeliCa Request System Code command
#param[out] numSystemCode Number of System Code (1 byte)
#param[out] systemCodeList System Code list
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_RequestSystemCode(uint8_t *numSystemCode, uint16_t *systemCodeList)
{
uint8_t cmd[9];
cmd[0] = FELICA_CMD_REQUEST_SYSTEM_CODE;
memcpy(&cmd[1], _felicaIDm, 8);
uint8_t responseBuf[10 + 2 * 16];
uint8_t responseLength;
if (!felica_SendCommand(cmd, 9, responseBuf, 8+11+32+2, &responseLength)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Request System Code command failed"));
#endif
return -1;
}
*numSystemCode = responseBuf[9];
// length check
if ( responseLength < 10 + 2 * *numSystemCode ) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Request System Code command failed (wrong response length)"));
#endif
return -2;
}
uint8_t i;
for(i=0; i<*numSystemCode; i++) {
systemCodeList[i] = (uint16_t)((responseBuf[10+i*2]<< 8) + responseBuf[10+i*2+1]);
}
return 1;
}
/**************************************************************************/
/*!
#brief Release FeliCa card
*/
/**************************************************************************/
int8_t Adafruit_PN532::felica_Release()
{
// InRelease
pn532_packetbuffer[0] = PN532_COMMAND_INRELEASE;
pn532_packetbuffer[1] = 0x00; // All target
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Release all FeliCa target"));
#endif
if (! sendCommandCheckAck(pn532_packetbuffer, 2))
return -1; // no ACK
readdata(pn532_packetbuffer,sizeof(pn532_packetbuffer));
if ( !felica_checkResponse(PN532_RESPONSE_INRELEASE) ) {
return -2;
}
if ((pn532_packetbuffer[7] & 0x3F)!=0) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Status code indicates an error: "));
PN532DEBUGPRINT.println(pn532_packetbuffer[7], HEX);
#endif
return -3;
}
/*
// Power off RF
pn532_packetbuffer[0] = PN532_COMMAND_RFCONFIGURATION;
pn532_packetbuffer[1] = 1; // Config item 1 (RF field )
pn532_packetbuffer[2] = 0x00; // AutoRFCA is off, RF is off
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Turning RF off"));
#endif
if (! sendCommandCheckAck(pn532_packetbuffer, 3))
return false; // no ACK
// Wait more than 20 ms
delay(20);
// Power on RF
pn532_packetbuffer[0] = PN532_COMMAND_RFCONFIGURATION;
pn532_packetbuffer[1] = 1; // Config item 1 (RF field )
pn532_packetbuffer[2] = 0x01; // AutoRFCA is off, RF is off
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Turning RF on"));
#endif
if (! sendCommandCheckAck(pn532_packetbuffer, 3))
return false; // no ACK
*/
return 1;
}
bool Adafruit_PN532::felica_checkResponse(uint8_t PN532_command)
{
if (pn532_packetbuffer[0] != 0 || pn532_packetbuffer[1] != 0 || pn532_packetbuffer[2] != 0xff) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Preamble missing"));
#endif
return false;
}
uint8_t length = pn532_packetbuffer[3];
if (pn532_packetbuffer[4]!=(uint8_t)(~length+1)) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.println(F("Length check invalid"));
PN532DEBUGPRINT.println(length,HEX);
PN532DEBUGPRINT.println((~length)+1,HEX);
#endif
return false;
}
if (pn532_packetbuffer[5]!=PN532_PN532TOHOST || pn532_packetbuffer[6]!=PN532_command) {
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Wrong TFI in response: "));
PN532DEBUGPRINT.println(pn532_packetbuffer[6],HEX);
#endif
return false;
}
return true;
}
/************** high level communication functions (handles both I2C and SPI) */
/**************************************************************************/
/*!
#brief Tries to read the SPI or I2C ACK signal
*/
/**************************************************************************/
bool Adafruit_PN532::readack() {
uint8_t ackbuff[6];
readdata(ackbuff, 6);
return (0 == strncmp((char *)ackbuff, (char *)pn532ack, 6));
}
/**************************************************************************/
/*!
#brief Return true if the PN532 is ready with a response.
*/
/**************************************************************************/
bool Adafruit_PN532::isready() {
if (_usingSPI) {
// SPI read status and check if ready.
#ifdef SPI_HAS_TRANSACTION
if (_hardwareSPI) SPI.beginTransaction(PN532_SPI_SETTING);
#endif
digitalWrite(_ss, LOW);
delay(2);
spi_write(PN532_SPI_STATREAD);
// read byte
uint8_t x = spi_read();
digitalWrite(_ss, HIGH);
#ifdef SPI_HAS_TRANSACTION
if (_hardwareSPI) SPI.endTransaction();
#endif
// Check if status is ready.
return x == PN532_SPI_READY;
}
else {
// I2C check if status is ready by IRQ line being pulled low.
uint8_t x = digitalRead(_irq);
return x == 0;
}
}
/**************************************************************************/
/*!
#brief Waits until the PN532 is ready.
#param timeout Timeout before giving up
*/
/**************************************************************************/
bool Adafruit_PN532::waitready(uint16_t timeout) {
uint16_t timer = 0;
while(!isready()) {
if (timeout != 0) {
timer += 10;
if (timer > timeout) {
PN532DEBUGPRINT.println("TIMEOUT!");
return false;
}
}
delay(10);
}
return true;
}
/**************************************************************************/
/*!
#brief Reads n bytes of data from the PN532 via SPI or I2C.
#param buff Pointer to the buffer where data will be written
#param n Number of bytes to be read
*/
/**************************************************************************/
void Adafruit_PN532::readdata(uint8_t* buff, uint8_t n) {
if (_usingSPI) {
// SPI write.
#ifdef SPI_HAS_TRANSACTION
if (_hardwareSPI) SPI.beginTransaction(PN532_SPI_SETTING);
#endif
digitalWrite(_ss, LOW);
delay(2);
spi_write(PN532_SPI_DATAREAD);
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Reading: "));
#endif
for (uint8_t i=0; i<n; i++) {
delay(1);
buff[i] = spi_read();
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F(" 0x"));
PN532DEBUGPRINT.print(buff[i], HEX);
#endif
}
#ifdef PN532DEBUG
PN532DEBUGPRINT.println();
#endif
digitalWrite(_ss, HIGH);
#ifdef SPI_HAS_TRANSACTION
if (_hardwareSPI) SPI.endTransaction();
#endif
}
else {
// I2C write.
uint16_t timer = 0;
delay(2);
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F("Reading: "));
#endif
// Start read (n+1 to take into account leading 0x01 with I2C)
WIRE.requestFrom((uint8_t)PN532_I2C_ADDRESS, (uint8_t)(n+2));
// Discard the leading 0x01
i2c_recv();
for (uint8_t i=0; i<n; i++) {
delay(1);
buff[i] = i2c_recv();
#ifdef PN532DEBUG
PN532DEBUGPRINT.print(F(" 0x"));
PN532DEBUGPRINT.print(buff[i], HEX);
#endif
}
// Discard trailing 0x00 0x00
// i2c_recv();
#ifdef PN532DEBUG
PN532DEBUGPRINT.println();
#endif
}
}
}
Here is my output
Waiting for an FeliCa card... Found a card!
IDm: 0x01 0x27 0x04 0x7E 0x49 0x5F 0x4E 0xB3
PMm: 0x00 0xF2 0x00 0x00 0x00 0x01 0x12 0x00
System Code: 0x0088
Request Service command -> error
Read Without Encryption command -> OK!
Block no. 0: 0x10 0x04 0x01 0x00 0x0D 0x00 0x00 0x00 0x00 0x00 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
Block no. 1: 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
Block no. 2: 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
Request Response command -> error
Request System Code command -> OK!
System code: 0x0B00
Card access completed!
My goal is to decode the memory and obtain the model numbers and if possible the MAC address so I can sync it up with Bluetooth with RPI.

Related

FLASH_If_Write and HardFault

I am using stm32f4 and I am trying to write some data on dual bank flash.
I am calling the below function in my code after the clean flash.
uint32_t totalBlockNumber;
uint32_t blockNumber;
uint8_t blockData[512];
uint32_t frameDataCount;
(frameDataCount always > 8)
Flash_Write_Start(totalBlockNumber,blockNumber,(frameDataCount - 8), blockData);
Than this is the Flash_Write_Start function:
void Flash_Write_Start( uint32_t totalBlockNumber, uint32_t blockNumber, uint32_t packet_length, uint8_t * blockData)
{
HAL_StatusTypeDef errorCheck = 1;
uint32_t bankActive;
if(fwFrameCounter == blockNumber)
{
errorCheck = FLASH_If_Write(Flash_Write_Address, blockData, packet_length);
}
if(errorCheck != FLASHIF_OK)
{
SerialPrint("FLASH ERROR\n");
}
Not every time but I have a hardfault, when code comes in FLASH_If_Write(Flash_Write_Address, blockData, packet_length); function. What is the reason for ?
FLASHIF_StatusTypeDef FLASH_If_Write(uint32_t destination, uint32_t *p_source, uint32_t length)
{
FLASHIF_StatusTypeDef status ;
HAL_StatusTypeDef halErrorFlag;
/* Unlock the Flash to enable the flash control register access *************/
if (HAL_FLASH_Unlock() != HAL_OK)
{
return FLASHIF_CRCKO;
}
uint32_t i ;
status = FLASHIF_OK;
/* DataLength must be a multiple of 32 bit */
for ( i = 0U; (i < (length >> 2U)) && (destination <= (USER_FLASH_END_ADDRESS - 4U)); i++) //
{
/* Device voltage range supposed to be [2.7V to 3.6V], the operation will
be done by word */
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, (destination + (i * 4U)), *(uint32_t *)(p_source + i)) == HAL_OK)
{
/* Check the written value */
if (*(__IO uint32_t *)(destination + (i * 4U)) != *(uint32_t *)(p_source + i))
{
status = FLASHIF_WRITINGCTRL_ERROR;
}
/* Increment FLASH destination address */
// destination += 4;
}
else
{
/* Error occurred while writing data in Flash memory */
status = FLASHIF_WRITING_ERROR;
break;
}
// SerialPrint(" 4 \n");
if (status != FLASHIF_OK)
{
break;
}
}
/* Lock the Flash to disable the flash control register access (recommended
to protect the FLASH memory against possible unwanted operation) *********/
halErrorFlag = HAL_FLASH_Lock();
if( halErrorFlag != HAL_OK)
{
}
return status;
}

Arduino NRF24l01 RC sends nothing

I'm trying to execute following source on my RC transmitter and receiver which uses NRF24 module. Today i uploaded same sketch as i did half year ago and it stopped receiving any data from transmitter, nothing is printed in DEBUG_PRINT sections.
On transmitter NRF24 is connected to 7, 8.
On receiver NRF24 is connected to 9, 10.
Wiring checked 7 times, correct, power supply of the receiver is 7.4V to the VIN, power supply for the transmitter is USB source.
Running on the arduino nano.
Source for transmitter is:
// Flash for self-diy NRF24lo* transmiter controller.
// 6 buttons, 4-way sticks, based on CX-10c controller.
#include <EEPROM.h>
#include <SPI.h>
#include "RF24.h"
#define DEBUG_PRINT
// #define DEBUG_PRINT_RAW
#ifdef DEBUG_PRINT
#include <printf.h>
#endif
// IO mappings
#define STICK0 A3
#define STICK1 A2
#define STICK2 A0
#define STICK3 A1
#define BTN0 3
#define BTN1 6
#define BTN2 5
#define BTN3 4
#define BTN4 9
#define BTN5 A4
#define STLED 2
// Optional moddings
#define BTN_LONG_PRESS 1000
#define LED_ST_OFF 0
#define LED_ST_CONST 1
#define LED_ST_FLASH 2
#define LED_ST_FAST_FLASH 3
#define LED_ST_FLASH_TIME 250
#define LED_ST_FAST_FLASH_TIME 100
#define LED_POWER 150
// Package types
#define PACKAGE_STICKS 3
#define PACKAGE_BUTTON 5
#define PACKAGE_PING 7
#define PACKAGE_TIMEOUT 100
// Amount of packages should be dropped sequently to detect disconnect
#define TX_DROP_EDGE 16
// Long press stick 0 to start calibration, long press to stop
// Struct with min/max calibration values
struct STICK_CALIBRATION {
int stmx[4];
int stmn[4];
};
// Struct with info of button presses
struct BTN_STATE {
int press[6];
unsigned long time[6];
// Used in long press to avoid multiple pressing
int acted[6];
};
// Struct with state of flashing/idle/working LED
// FLASH --> CONST/OFF
// FAST_FLASH --> CONST/OFF
struct LED_STATE {
// Previous type (for FLASH return to OFF/CONST state)
int ptype;
// Type of operation
int type;
// ON/OFF
int state;
// Time since action
unsigned long time;
// Count of flashes
int count;
};
// Sender package type.
// Contains type and values for button or values for sticks
struct Package {
int type;
union {
struct {
int number;
int lpress;
int dummy[2];
} button;
struct {
int sticks[4];
} sticks;
} data;
};
// Info of sticks calibration
STICK_CALIBRATION calibration;
bool calibration_mode = 0;
int sticks[4];
// When entering lock mode, stick data is not updating from input
bool lock_mode = 0;
// info of buttons states
BTN_STATE buttons;
LED_STATE led_state;
// Use NRF24lo1 transmitter on pins 7, 8
RF24 radio(7, 8);
byte addresses[][6] = { "TRAAA", "REEEE" };
// Amount of sequently dropped packages. Used to detect disconnect
int tx_dropped = 0;
// Used to prevent longpress repeat
// On long press button could be used as sticky press function,
// to avoid button sticking and long press act once during while push down,
// use this function to mark button as acted
void btn_act(int nubmer);
// Called on button press/longpress
void btn_action(int number, int lpress);
// Set led ON/OFF with change of state flag
void led_set_state(int state);
// Set led action type (FLAST, CONST, e.t.c.)
void led_set(int type, int count);
// Sends single package with given amount of attempts.
// Allows waiting for responce of receiver. The response should be 0.
// Returns 1 on success, 0 on failture.
int send_package(byte* pack, int size);
void setup() {
#ifdef DEBUG_PRINT
Serial.begin(115200);
#endif
// Set up transmitter
radio.begin();
radio.setChannel(77);
radio.enableAckPayload();
radio.setPALevel(RF24_PA_MAX);
radio.openWritingPipe(addresses[1]);
radio.openReadingPipe(1, addresses[0]);
#ifdef DEBUG_PRINT
printf_begin();
radio.printDetails();
#endif
// Read calibration values
byte* cal_struct = (byte*) &calibration;
for (int i = 0; i < sizeof(STICK_CALIBRATION); ++i)
cal_struct[i] = EEPROM.read(i);
#ifdef DEBUG_PRINT
Serial.println("CALIBRATION: ");
for (int i = 0; i < 4; ++i) {
Serial.print('(');
Serial.print(calibration.stmn[i]);
Serial.print(", ");
Serial.print(calibration.stmx[i]);
Serial.print(") ");
}
Serial.println();
#endif
buttons = {{0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}};
// Clear LED state
led_state.ptype = 0;
led_state.type = 0;
led_state.state = 0;
led_state.time = 0;
led_state.count = 0;
// Prepare pinout
pinMode(STLED, OUTPUT);
pinMode(STICK0, INPUT_PULLUP);
pinMode(STICK1, INPUT_PULLUP);
pinMode(STICK2, INPUT_PULLUP);
pinMode(STICK3, INPUT_PULLUP);
pinMode(BTN0, INPUT_PULLUP);
pinMode(BTN1, INPUT_PULLUP);
pinMode(BTN2, INPUT_PULLUP);
pinMode(BTN3, INPUT_PULLUP);
pinMode(BTN4, INPUT_PULLUP);
pinMode(BTN5, INPUT_PULLUP);
}
void loop() {
// Read sticks & map to calibration
if (!lock_mode) {
sticks[0] = analogRead(STICK0);
sticks[1] = analogRead(STICK1);
sticks[2] = 1024 - analogRead(STICK2);
sticks[3] = 1024 - analogRead(STICK3);
}
// Read stick buttons
int rbtn[6];
rbtn[0] = !digitalRead(BTN0);
rbtn[1] = !digitalRead(BTN1);
// read optional buttons
rbtn[2] = !digitalRead(BTN2);
rbtn[3] = !digitalRead(BTN3);
rbtn[4] = !digitalRead(BTN4);
rbtn[5] = !digitalRead(BTN5);
#ifdef DEBUG_PRINT
#ifdef DEBUG_PRINT_RAW
// Debug out
Serial.print(sticks[0]); Serial.print(' ');
Serial.print(sticks[1]); Serial.print(' ');
Serial.print(sticks[2]); Serial.print(' ');
Serial.print(sticks[3]); Serial.print(' ');
Serial.print(rbtn[0]); Serial.print(' ');
Serial.print(rbtn[1]); Serial.print(' ');
Serial.print(rbtn[2]); Serial.print(' ');
Serial.print(rbtn[3]); Serial.print(' ');
Serial.print(rbtn[4]); Serial.print(' ');
Serial.print(rbtn[5]); Serial.print(' ');
Serial.println();
#endif
#endif
// Map to calibration
if (!calibration_mode && !lock_mode) {
sticks[0] = map(sticks[0], calibration.stmn[0], calibration.stmx[0], 0, 1023);
sticks[1] = map(sticks[1], calibration.stmn[1], calibration.stmx[1], 0, 1023);
sticks[2] = map(sticks[2], calibration.stmn[2], calibration.stmx[2], 0, 1023);
sticks[3] = map(sticks[3], calibration.stmn[3], calibration.stmx[3], 0, 1023);
}
// Check buttons states and update timings
for (int i = 0; i < 6; ++i) {
if (buttons.press[i] && !rbtn[i]) { // Button released
if (!buttons.acted[i])
btn_action(i, (millis() - buttons.time[i]) > BTN_LONG_PRESS);
buttons.press[i] = 0;
buttons.time[i] = 0;
} else if (buttons.press[i]) { // Button keeps down
if ((millis() - buttons.time[i]) > BTN_LONG_PRESS && !buttons.acted[i]) { // Toggle long press
btn_action(i, 1);
// buttons.press[i] = 0;
buttons.time[i] = 0;
}
} else if (rbtn[i]) { // Button pressed
buttons.press[i] = 1;
buttons.acted[i] = 0;
buttons.time[i] = millis();
}
}
// Update LED
if (led_state.type == LED_ST_FLASH && (led_state.time + LED_ST_FLASH_TIME) < millis()
||
led_state.type == LED_ST_FAST_FLASH && (led_state.time + LED_ST_FAST_FLASH_TIME) < millis()) { // Flash period done
if (!led_state.state) { // Count cycle as finished, try to begin another one
--led_state.count;
if (led_state.count <= 0) { // Flashing cycles is done
led_state.type = led_state.ptype;
led_set_state(led_state.type == LED_ST_CONST);
} else { // Turn led ON again, begin next flash cycle
led_state.time = millis();
led_set_state(1);
}
} else { // Just turn the led OFF
led_state.time = millis();
led_set_state(0);
}
}
// Update led flashing
if (led_state.type != LED_ST_FLASH && led_state.type != LED_ST_FAST_FLASH) {
if (calibration_mode)
led_set(LED_ST_FAST_FLASH, 4);
else {
// Update led lighting
if (lock_mode)
led_set(LED_ST_CONST, 0);
else
led_set(LED_ST_OFF, 0);
}
}
// If !paired
if (calibration_mode) {
for (int i = 0; i < 4; ++i) {
if (calibration.stmn[i] > sticks[i])
calibration.stmn[i] = sticks[i];
if (calibration.stmx[i] < sticks[i])
calibration.stmx[i] = sticks[i];
}
} else {
// Sending package with sticks
Package pack;
pack.type = PACKAGE_STICKS;
pack.data.sticks.sticks[0] = sticks[0];
pack.data.sticks.sticks[1] = sticks[1];
pack.data.sticks.sticks[2] = sticks[2];
pack.data.sticks.sticks[3] = sticks[3];
bool result = send_package((byte*) &pack, sizeof(Package));
if (!result) {
if (tx_dropped > TX_DROP_EDGE && led_state.type != LED_ST_FLASH && led_state.type != LED_ST_FAST_FLASH)
led_set(LED_ST_FLASH, 4);
#ifdef DEBUG_PRINT
Serial.println("TX failed");
#endif
}
}
}
int send_package(byte* pack, int size) {
if (!radio.write(pack, size)) {
++tx_dropped;
return 0;
} else {
byte payload;
if (radio.isAckPayloadAvailable()) {
radio.read(&payload, sizeof(byte));
tx_dropped = 0;
return 1;
}
++tx_dropped;
return 0;
}
};
void led_set_state(int state) {
if (led_state.state && !state) {
digitalWrite(STLED, 0);
led_state.state = !led_state.state;
} else if (!led_state.state && state) {
analogWrite(STLED, LED_POWER);
led_state.state = !led_state.state;
}
};
void led_set(int type, int count) {
if (type == LED_ST_CONST || type == LED_ST_OFF) {
led_set_state(type == LED_ST_CONST);
led_state.type = type;
} else {
// if was flashing --> rewrite
// if was constant --> move type to ptype & do flashing
if (led_state.type == LED_ST_CONST || led_state.type == LED_ST_OFF)
led_state.ptype = led_state.type;
led_state.type = type;
led_state.count = count;
led_state.time = millis();
led_set_state(1);
}
};
void btn_act(int number) {
buttons.acted[number] = 1;
};
// Override calls for button presses
void btn_action(int number, int lpress) {
#ifdef DEBUG_PRINT
Serial.print("Press for "); Serial.println(number);
#endif
switch(number) {
// calibration
case 0: {
// press = ?
// Lpress = calibration mode
if (lpress) {
if (calibration_mode && !lock_mode) {
// Write calibration values
byte* cal_struct = (byte*) &calibration;
for (int i = 0; i < sizeof(STICK_CALIBRATION); ++i)
EEPROM.write(i, cal_struct[i]);
calibration_mode = 0;
#ifdef DEBUG_PRINT
Serial.println("NEW CALIBRATION: ");
for (int i = 0; i < 4; ++i) {
Serial.print('(');
Serial.print(calibration.stmn[i]);
Serial.print(", ");
Serial.print(calibration.stmx[i]);
Serial.print(") ");
}
Serial.println();
#endif
} else {
for (int i = 0; i < 4; ++i) {
calibration.stmn[i] = 1024;
calibration.stmx[i] = 0;
}
calibration_mode = 1;
}
}
break;
}
case 1: {
// press = ?
// Lpress = lock mode
if (lpress) {
lock_mode = !lock_mode;
}
break;
}
}
led_set(LED_ST_FAST_FLASH, 2);
btn_act(number);
Package pack;
pack.type = PACKAGE_BUTTON;
pack.data.button.number = number;
pack.data.button.lpress = lpress;
bool result = send_package((byte*) &pack, sizeof(Package));
#ifdef DEBUG_PRINT
if (!result)
Serial.println("Button TX failed");
#endif
};
Source for receiver is:
// Flash for self-diy NRF24lo* receiver controller.
// 6 buttons, 4-way sticks.
#include <Servo.h>
#include <SPI.h>
#include "RF24.h"
#define DEBUG_PRINT
#ifdef DEBUG_PRINT
#include <printf.h>
#endif
// IO mappings
#define CONNECT_0 2
#define CONNECT_1 3
#define CONNECT_2 4
#define CONNECT_3 5
#define CONNECT_4 6
#define CONNECT_5 7
#define CONNECT_6 8
#define CONNECT_A0 A0
#define CONNECT_A1 A1
#define CONNECT_A2 A2
#define CONNECT_A3 A3
#define CONNECT_A4 A4
#define CONNECT_A5 A5
// A6, A7 on nano are only analog input
#define CONNECT_A6 A6
#define CONNECT_A7 A7
// Inverters for sticks
#define INVERT_STICK0 0
#define INVERT_STICK1 0
#define INVERT_STICK2 0
#define INVERT_STICK3 0
// Mappings for sticks
#define SERVO_MAP_STICK0 0, 180
#define SERVO_MAP_STICK1 0, 180
#define SERVO_MAP_STICK2 0, 180
#define SERVO_MAP_STICK3 60, 120
#define ANALOG_MAP_STICK0 0, 255
#define ANALOG_MAP_STICK1 0, 255
#define ANALOG_MAP_STICK2 0, 255
#define ANALOG_MAP_STICK3 0, 255
// Optional moddings
// Package types
#define PACKAGE_STICKS 3
#define PACKAGE_BUTTON 5
#define PACKAGE_PING 7
// Used to detect disconnect from the controller
#define CONNECTION_TIMEOUT 100
// Mpdes for output of the motor
// #define MODE_2_DIGITAL_1_ANALOG
#define MODE_2_ANALOG
// Receiver package struct.
// Contains type and values for button or values for sticks
struct Package {
int type;
union {
struct {
int number;
int lpress;
// Used to complete size of the package.
// Without it, payload is not sending for sticks.
int dummy[2];
} button;
struct {
int sticks[4];
} sticks;
} data;
};
// Describes state of a single buttons on controller
// Single press -> 1
// Second press -> 0
struct Button {
bool state;
bool lstate;
};
// Use NRF24lo1 transmitter on pins 7, 8
RF24 radio(9, 10);
byte addresses[][6] = { "TRAAA", "REEEE" };
// Used to detect timed disconnection from the controller
unsigned long last_receive_time;
bool disconnected;
// Servos mapped to sticks
Servo servo[4];
// Buttons states
Button buttons[6];
// Called on connection restored after drop
void on_connection();
// Called on connection dropped
void on_disconnection();
// Called on button state received
void button_action(int button, int lpress);
// Called on sticks state received
void sticks_action(int sticks[4]);
void setPWMNanofrequency(int freq) {
TCCR2B = TCCR2B & 0b11111000 | freq;
TCCR1B = TCCR1B & 0b11111000 | freq;
}
void setup() {
#ifdef DEBUG_PRINT
Serial.begin(115200);
#endif
// Set up receiver
radio.begin();
radio.setChannel(77);
//radio.setAutoAck(1);
//radio.setRetries(0, 8);
radio.enableAckPayload();
radio.setPALevel(RF24_PA_MAX);
//radio.setCRCLength(RF24_CRC_8);
radio.openWritingPipe(addresses[0]);
radio.openReadingPipe(1, addresses[1]);
#ifdef DEBUG_PRINT
// Debugger output
printf_begin();
radio.printDetails();
#endif
radio.startListening();
// Init buttons with 0
buttons[0] = { 0, 0 };
buttons[1] = { 0, 0 };
buttons[2] = { 0, 0 };
buttons[3] = { 0, 0 };
buttons[4] = { 0, 0 };
buttons[5] = { 0, 0 };
// Set up pinout
pinMode(CONNECT_0, OUTPUT);
pinMode(CONNECT_2, OUTPUT);
pinMode(CONNECT_3, OUTPUT);
pinMode(CONNECT_4, OUTPUT);
pinMode(CONNECT_5, OUTPUT);
pinMode(CONNECT_6, OUTPUT);
pinMode(CONNECT_A0, OUTPUT);
pinMode(CONNECT_A1, OUTPUT);
pinMode(CONNECT_A2, OUTPUT);
pinMode(CONNECT_A3, OUTPUT);
pinMode(CONNECT_A4, OUTPUT);
pinMode(CONNECT_A5, OUTPUT);
pinMode(CONNECT_A6, INPUT);
pinMode(CONNECT_A7, INPUT);
// setPWMNanofrequency(0x02);
// Set up servos
servo[0].attach(CONNECT_A4); // Remapped servos to leave three PWM pins
servo[1].attach(CONNECT_A5);
servo[2].attach(CONNECT_5);
servo[3].attach(CONNECT_6);
// Clear disconnect trigger
last_receive_time = millis();
disconnected = 0;
// Reset outputs
digitalWrite(CONNECT_0, LOW);
digitalWrite(CONNECT_2, LOW);
digitalWrite(CONNECT_3, LOW);
digitalWrite(CONNECT_4, LOW);
digitalWrite(CONNECT_5, LOW);
digitalWrite(CONNECT_6, LOW);
digitalWrite(CONNECT_A0, LOW);
digitalWrite(CONNECT_A1, LOW);
digitalWrite(CONNECT_A2, LOW);
digitalWrite(CONNECT_A3, LOW);
digitalWrite(CONNECT_A4, LOW);
digitalWrite(CONNECT_A5, LOW);
// digitalWrite(CONNECT_A6, LOW);
// digitalWrite(CONNECT_A7, LOW);
}
void loop() {
byte payload = 13;
radio.writeAckPayload(1, &payload, sizeof(byte));
if (radio.available()) {
Package pack;
radio.read((byte*) &pack, sizeof(Package));
// Update trigger
last_receive_time = millis();
if (disconnected)
on_connection();
disconnected = 0;
switch(pack.type) {
case PACKAGE_STICKS: {
#ifdef DEBUG_PRINT
Serial.print(pack.data.sticks.sticks[0]); Serial.print(' ');
Serial.print(pack.data.sticks.sticks[1]); Serial.print(' ');
Serial.print(pack.data.sticks.sticks[2]); Serial.print(' ');
Serial.print(pack.data.sticks.sticks[3]); Serial.println();
#endif
if (INVERT_STICK0) pack.data.sticks.sticks[0] = 1024 - pack.data.sticks.sticks[0];
if (INVERT_STICK1) pack.data.sticks.sticks[1] = 1024 - pack.data.sticks.sticks[1];
if (INVERT_STICK2) pack.data.sticks.sticks[2] = 1024 - pack.data.sticks.sticks[2];
if (INVERT_STICK3) pack.data.sticks.sticks[3] = 1024 - pack.data.sticks.sticks[3];
int ssticks[4];
ssticks[0] = map(pack.data.sticks.sticks[0], 0, 1023, SERVO_MAP_STICK0);
ssticks[1] = map(pack.data.sticks.sticks[1], 0, 1023, SERVO_MAP_STICK1);
ssticks[2] = map(pack.data.sticks.sticks[2], 0, 1023, SERVO_MAP_STICK2);
ssticks[3] = map(pack.data.sticks.sticks[3], 0, 1023, SERVO_MAP_STICK3);
int asticks[4];
asticks[0] = map(pack.data.sticks.sticks[0], 0, 1023, ANALOG_MAP_STICK0);
asticks[1] = map(pack.data.sticks.sticks[1], 0, 1023, ANALOG_MAP_STICK1);
asticks[2] = map(pack.data.sticks.sticks[2], 0, 1023, ANALOG_MAP_STICK2);
asticks[3] = map(pack.data.sticks.sticks[3], 0, 1023, ANALOG_MAP_STICK3);
#ifdef DEBUG_PRINT
Serial.print("Servo data: ");
Serial.print(ssticks[0]); Serial.print(' ');
Serial.print(ssticks[1]); Serial.print(' ');
Serial.print(ssticks[2]); Serial.print(' ');
Serial.print(ssticks[3]); Serial.println();
Serial.print("Analog data: ");
Serial.print(asticks[0]); Serial.print(' ');
Serial.print(asticks[1]); Serial.print(' ');
Serial.print(asticks[2]); Serial.print(' ');
Serial.print(asticks[3]); Serial.println();
#endif
sticks_action(pack.data.sticks.sticks, ssticks, asticks);
break;
}
case PACKAGE_BUTTON: {
#ifdef DEBUG_PRINT
Serial.print(pack.data.button.number); Serial.print(' ');
Serial.print(pack.data.button.lpress); Serial.println();
#endif
// Update states of the buttons
if (pack.data.button.lpress)
buttons[pack.data.button.number].lstate = !buttons[pack.data.button.number].lstate;
else
buttons[pack.data.button.number].state = !buttons[pack.data.button.number].state;
button_action(pack.data.button.number, pack.data.button.lpress);
break;
}
}
} else if (!disconnected && millis() - last_receive_time > CONNECTION_TIMEOUT) {
disconnected = 1;
on_disconnection();
}
}
void on_connection() {
};
void on_disconnection() {
// servo[0].write(0);
// servo[1].write(0);
// servo[2].write(0);
// servo[3].write(0);
// analogWrite(CONNECT_A0, 0);
// analogWrite(CONNECT_A1, 0);
// analogWrite(CONNECT_A2, 0);
// analogWrite(CONNECT_A3, 0);
#ifdef MODE_2_DIGITAL_1_ANALOG
digitalWrite(CONNECT_1, LOW);
digitalWrite(CONNECT_2, LOW);
digitalWrite(CONNECT_3, LOW);
#endif
#ifdef MODE_2_ANALOG
digitalWrite(CONNECT_3, LOW);
digitalWrite(CONNECT_4, LOW);
digitalWrite(CONNECT_0, LOW);
#endif
};
void button_action(int button, int lpress) {
switch (button) {
case 0:
case 1:
break;
case 2: {
digitalWrite(CONNECT_A0, buttons[button].state);
break;
}
case 3: {
digitalWrite(CONNECT_A1, buttons[button].state);
break;
}
case 4: {
digitalWrite(CONNECT_A2, buttons[button].state);
break;
}
case 5: {
digitalWrite(CONNECT_A3, buttons[button].state);
break;
}
}
};
void sticks_action(int sticks[4], int ssticks[4], int asticks[4]) {
servo[0].write(ssticks[0]);
servo[1].write(ssticks[1]);
servo[2].write(ssticks[2]);
servo[3].write(ssticks[3]);
// analogWrite(CONNECT_A0, asticks[0]);
// analogWrite(CONNECT_A1, asticks[1]);
// analogWrite(CONNECT_A2, asticks[2]);
// analogWrite(CONNECT_A3, asticks[3]);
// Control H-Bridge over 3, 4 outputs with right stick
int value = (sticks[2] > 500) ? (sticks[2] - 500) : (sticks[2] < 480) ? (480 - sticks[2]) : (0);
if (value < 500)
value = map(value, 0, 480, 0, 255);
else
value = map(value, 0, 1023 - 500, 0, 255);
value = (value > 255) ? 255 : (value < 0) ? 0 : value;
// connecting 2 digital pins as HIGH,LOW / LOW,HIGH and analog as speed value (used in bts79603 bridge)
#ifdef MODE_2_DIGITAL_1_ANALOG
if (sticks[2] > 500) {
digitalWrite(CONNECT_1, HIGH);
digitalWrite(CONNECT_2, LOW);
analogWrite(CONNECT_3, value);
} else if (sticks[2] < 480) {
digitalWrite(CONNECT_1, LOW);
digitalWrite(CONNECT_2, HIGH);
analogWrite(CONNECT_3, value);
} else {
digitalWrite(CONNECT_1, LOW);
digitalWrite(CONNECT_2, LOW);
analogWrite(CONNECT_3, 0);
}
#endif
// connecting 2 analog pins to simple H-Bridge
#ifdef MODE_2_ANALOG
analogWrite(CONNECT_3, sticks[2] > 500 ? value : 0);
analogWrite(CONNECT_4, sticks[2] < 480 ? value : 0);
analogWrite(CONNECT_0, sticks[2] > 500 || sticks[2] < 480);
#endif
};
Transmitter printDetails output:
STATUS = 0x0e RX_DR=0 TX_DS=0 MAX_RT=0 RX_P_NO=7 TX_FULL=0
RX_ADDR_P0-1 = 0x4141415254 0x4545454552
RX_ADDR_P2-5 = 0xc3 0xc4 0xc5 0xc6
TX_ADDR = 0x4141415254
RX_PW_P0-6 = 0x20 0x20 0x00 0x00 0x00 0x00
EN_AA = 0x3f
EN_RXADDR = 0x03
RF_CH = 0x4d
RF_SETUP = 0x07
CONFIG = 0x0e
DYNPD/FEATURE = 0x00 0x00
Data Rate = 1MBPS
Model = nRF24L01+
CRC Length = 16 bits
PA Power = PA_MAX
Receiver printDetails output:
STATUS = 0x0e RX_DR=0 TX_DS=0 MAX_RT=0 RX_P_NO=7 TX_FULL=0
RX_ADDR_P0-1 = 0x4545454552 0x4141415254
RX_ADDR_P2-5 = 0xc3 0xc4 0xc5 0xc6
TX_ADDR = 0x4545454552
RX_PW_P0-6 = 0x20 0x20 0x00 0x00 0x00 0x00
EN_AA = 0x3f
EN_RXADDR = 0x02
RF_CH = 0x4d
RF_SETUP = 0x07
CONFIG = 0x0e
DYNPD/FEATURE = 0x00 0x00
Data Rate = 1MBPS
Model = nRF24L01+
CRC Length = 16 bits
PA Power = PA_MAX
Code used
Following code on receiver caused IO break.
pinMode(CONNECT_A6, INPUT);
pinMode(CONNECT_A7, INPUT);
A6, A7 pins DOES NOT support mode change and this issue made NRF24 connection to fail.

C/C++ FLV muxer encapsulating H.264 not working as expected

I'm trying to make a FLV muxer. And I'm started to test on a h264 stream I capture from my camera (c920). After encoding the .flv file, it doesn't play correctly.
First I tried to find NALs in h264 searching for the pattern 0x00 0x00 0x00 0x01... but I found in the internet that there are two patterns to find NALs... So I implemented the searching for 0x00 0x00 0x01 and the other...
In firsts tests I found that are few NALs starting with four bytes, but after changing the code to search the 3 bytes pattern I found a lot of NALs...
The references I found in the internet shows code using few NAL types to encapsulate the FLV file, but after the change to detect 3 bytes, the program found a lot of NALs and I don't know how to stream them...
Some code :-)
I'm implementing in C++, so I have the classes.
The FLVWritter
class FLVWritter{
public:
std::vector<uint8_t> buffer;
void flushToFD(int fd) {
if (buffer.size() <= 0)
return;
::write(fd,&buffer[0],buffer.size());
buffer.clear();
}
void reset(){
buffer.clear();
}
void writeFLVFileHeader(bool haveAudio, bool haveVideo){
uint8_t flv_header[13] = {
0x46, 0x4c, 0x56, 0x01, 0x05,
0x00, 0x00, 0x00, 0x09, 0x00,
0x00, 0x00, 0x00
};
if (haveAudio && haveVideo) {
flv_header[4] = 0x05;
} else if (haveAudio && !haveVideo) {
flv_header[4] = 0x04;
} else if (!haveAudio && haveVideo) {
flv_header[4] = 0x01;
} else {
flv_header[4] = 0x00;
}
for(int i=0;i<13;i++)
buffer.push_back(flv_header[i]);
}
void writeUInt8(uint8_t v) {
buffer.push_back(v);
}
void writeUInt16(uint32_t v){
buffer.push_back((uint8_t)(v >> 8));
buffer.push_back((uint8_t)(v));
}
void writeUInt24(uint32_t v){
buffer.push_back((uint8_t)(v >> 16));
buffer.push_back((uint8_t)(v >> 8));
buffer.push_back((uint8_t)(v));
}
void writeUInt32(uint32_t v){
buffer.push_back((uint8_t)(v >> 24));
buffer.push_back((uint8_t)(v >> 16));
buffer.push_back((uint8_t)(v >> 8));
buffer.push_back((uint8_t)(v));
}
void writeUInt32Timestamp(uint32_t v){
buffer.push_back((uint8_t)(v >> 16));
buffer.push_back((uint8_t)(v >> 8));
buffer.push_back((uint8_t)(v));
buffer.push_back((uint8_t)(v >> 24));
}
};
The h264 parser:
class ParserH264 {
std::vector<uint8_t> buffer;
enum State{
None,
Data,
NAL0,
NAL1
};
State nalState; // nal = network abstraction layer
State writingState;
void putByte(uint8_t byte) {
//
// Detect start code 00 00 01 and 00 00 00 01
//
// It returns the buffer right after the start code
//
if (byte == 0x00 && nalState == None)
nalState = NAL0;
else if (byte == 0x00 && (nalState == NAL0 || nalState == NAL1) )
nalState = NAL1;
else if (byte == 0x01 && nalState == NAL1){
nalState = None;
if (writingState == None){
writingState = Data;
return;
} else if (writingState == Data){
buffer.pop_back();// 0x00
buffer.pop_back();// 0x00
//in the case using the format 00 00 00 01, remove the last element detected
if (buffer[buffer.size()-1] == 0x00 &&
buffer[buffer.size()-2] != 0x03 )//keep value, if emulation prevention is present
buffer.pop_back();
chunkDetectedH264(&buffer[0],buffer.size());
buffer.clear();
return;
}
} else
nalState = None;
if (writingState == Data){
//
// increase raw buffer size
//
buffer.push_back(byte);
}
}
public:
ParserH264() {
nalState = None;
writingState = None;
}
virtual ~ParserH264(){
}
virtual void chunkDetectedH264(const uint8_t* ibuffer, int size){
}
void endOfStreamH264() {
if (buffer.size() <= 0)
return;
chunkDetectedH264(&buffer[0],buffer.size());
buffer.clear();
writingState = None;
nalState = None;
}
void parseH264(const uint8_t* ibuffer, int size) {
for(int i=0;i<size;i++){
putByte(ibuffer[i]);
}
}
};
And finally the main class:
class FLVFileWriter: public ParserH264 {
std::vector<uint8_t> lastSPS;
public:
FLVWritter mFLVWritter;
int fd;
bool firstAudioWrite;
uint32_t audioTimestamp_ms;
uint32_t videoTimestamp_ms;
FLVFileWriter ( const char* file ) {
fd = open(file, O_WRONLY | O_CREAT | O_TRUNC , 0644 );
if (fd < 0){
fprintf(stderr,"error to create flv file\n");
exit(-1);
}
//have audio, have video
mFLVWritter.writeFLVFileHeader(false, true);
mFLVWritter.flushToFD(fd);
firstAudioWrite = true;
audioTimestamp_ms = 0;
videoTimestamp_ms = 0;
}
virtual ~FLVFileWriter(){
if (fd >= 0){
mFLVWritter.flushToFD(fd);
close(fd);
}
}
void chunkDetectedH264(const uint8_t* ibuffer, int size) {
printf("[debug] Detected NAL chunk size: %i\n",size);
if (size <= 0){
fprintf(stdout, " error On h264 chunk detection\n");
return;
}
uint8_t nal_bit = ibuffer[0];
uint8_t nal_type = (nal_bit & 0x1f);
//0x67
//if (nal_bit == (NAL_IDC_PICTURE | NAL_TYPE_SPS) ) {
if ( nal_type == (NAL_TYPE_SPS) ) {
fprintf(stdout, " processing: 0x%x (SPS)\n",nal_bit);
//store information to use when arrise PPS nal_bit, probably the next NAL detection
lastSPS.clear();
for(int i=0;i<size;i++)
lastSPS.push_back(ibuffer[i]);
}
//else if (nal_bit == (NAL_IDC_PICTURE | NAL_TYPE_PPS) ) {
else if ( nal_type == (NAL_TYPE_PPS) ) {
fprintf(stdout, " processing: 0x%x (PPS)\n",nal_bit);
//must be called just after the SPS detection
int32_t bodyLength = lastSPS.size() + size + 16;
//
// flv tag header = 11 bytes
//
mFLVWritter.writeUInt8(0x09);//tagtype video
mFLVWritter.writeUInt24( bodyLength );//data len
mFLVWritter.writeUInt32Timestamp( videoTimestamp_ms );//timestamp
mFLVWritter.writeUInt24( 0 );//stream id 0
//
// Message Body = 16 bytes + SPS bytes + PPS bytes
//
//flv VideoTagHeader
mFLVWritter.writeUInt8(0x17);//key frame, AVC 1:keyframe 7:h264
mFLVWritter.writeUInt8(0x00);//avc sequence header
mFLVWritter.writeUInt24( 0x00 );//composit time ??????????
//flv VideoTagBody --AVCDecoderCOnfigurationRecord
mFLVWritter.writeUInt8(0x01);//configurationversion
mFLVWritter.writeUInt8(lastSPS[1]);//avcprofileindication
mFLVWritter.writeUInt8(lastSPS[2]);//profilecompatibilty
mFLVWritter.writeUInt8(lastSPS[3]);//avclevelindication
mFLVWritter.writeUInt8(0xFC | 0x03); //reserved + lengthsizeminusone
mFLVWritter.writeUInt8(0xe0 | 0x01); // first reserved, second number of SPS
mFLVWritter.writeUInt16( lastSPS.size() ); //sequence parameter set length
//H264 sequence parameter set raw data
for(int i=0;i<lastSPS.size();i++)
mFLVWritter.writeUInt8(lastSPS[i]);
mFLVWritter.writeUInt8(0x01); // number of PPS
//sanity check with the packet size...
if ( size-4 > 0xffff ){
fprintf(stderr, "PPS Greater than 64k. This muxer does not support it.\n");
exit(-1);
}
mFLVWritter.writeUInt16(size); //picture parameter set length
//H264 picture parameter set raw data
for(int i=0;i<size;i++)
mFLVWritter.writeUInt8(ibuffer[i]);
//
// previous tag size
//
uint32_t currentSize = mFLVWritter.buffer.size();
if (currentSize != bodyLength + 11 ){
fprintf(stderr, "error to write flv video tag NAL_TYPE_PPS\n");
exit(-1);
}
mFLVWritter.writeUInt32(currentSize);//data len
mFLVWritter.flushToFD(fd);
videoTimestamp_ms += 1000/30;
}
//0x65
//else if (nal_bit == (NAL_IDC_PICTURE | NAL_TYPE_CSIDRP) ) {
else if ( nal_type == (NAL_TYPE_CSIDRP) ) {
fprintf(stdout, " processing: 0x%x (0x65)\n",nal_bit);
uint32_t bodyLength = size + 5 + 4;//flv VideoTagHeader + NALU length
//
// flv tag header = 11 bytes
//
mFLVWritter.writeUInt8(0x09);//tagtype video
mFLVWritter.writeUInt24( bodyLength );//data len
mFLVWritter.writeUInt32Timestamp( videoTimestamp_ms );//timestamp
mFLVWritter.writeUInt24( 0 );//stream id 0
//
// Message Body = VideoTagHeader(5) + NALLength(4) + NAL raw data
//
//flv VideoTagHeader
mFLVWritter.writeUInt8(0x17);//key frame, AVC 1:keyframe 2:inner frame 7:H264
mFLVWritter.writeUInt8(0x01);//avc NALU unit
mFLVWritter.writeUInt24(0x00);//composit time ??????????
mFLVWritter.writeUInt32(size);//nal length
//nal raw data
for(int i=0;i<size;i++)
mFLVWritter.writeUInt8(ibuffer[i]);
//
// previous tag size
//
uint32_t currentSize = mFLVWritter.buffer.size();
if (currentSize != bodyLength + 11 ){
fprintf(stderr, "error to write flv video tag NAL_TYPE_CSIDRP\n");
exit(-1);
}
mFLVWritter.writeUInt32(currentSize);//data len
mFLVWritter.flushToFD(fd);
videoTimestamp_ms += 1000/30;
}
//0x61
//else if (nal_bit == (NAL_IDC_FRAME | NAL_TYPE_CSNIDRP) ) {
else if ( nal_type == (NAL_TYPE_CSNIDRP) ) {
fprintf(stdout, " processing: 0x%x (0x61)\n",nal_bit);
uint32_t bodyLength = size + 5 + 4;//flv VideoTagHeader + NALU length
//
// flv tag header = 11 bytes
//
mFLVWritter.writeUInt8(0x09);//tagtype video
mFLVWritter.writeUInt24( bodyLength );//data len
mFLVWritter.writeUInt32Timestamp( videoTimestamp_ms );//timestamp
mFLVWritter.writeUInt24( 0 );//stream id 0
//
// Message Body = VideoTagHeader(5) + NALLength(4) + NAL raw data
//
//flv VideoTagHeader
mFLVWritter.writeUInt8(0x27);//key frame, AVC 1:keyframe 2:inner frame 7:H264
mFLVWritter.writeUInt8(0x01);//avc NALU unit
mFLVWritter.writeUInt24(0x00);//composit time ??????????
mFLVWritter.writeUInt32(size);//nal length
// raw nal data
for(int i=0;i<size;i++)
mFLVWritter.writeUInt8(ibuffer[i]);
//
// previous tag size
//
uint32_t currentSize = mFLVWritter.buffer.size();
if (currentSize != bodyLength + 11 ){
fprintf(stderr, "error to write flv video tag NAL_TYPE_CSNIDRP\n");
exit(-1);
}
mFLVWritter.writeUInt32(currentSize);//data len
mFLVWritter.flushToFD(fd);
videoTimestamp_ms += 1000/30;
}
else if (nal_type == (NAL_TYPE_SEI)) {
fprintf(stdout, " ignoring SEI bit: 0x%x type: 0x%x\n",nal_bit, nal_type);
} else {
// nal_bit type not implemented...
fprintf(stdout, "Error: unknown NAL bit: 0x%x type: 0x%x\n",nal_bit, nal_type);
exit(-1);
}
}
};
To use these classes in main we can write this:
volatile bool exit_requested = false;
void signal_handler(int signal) {
exit_requested = true;
}
int main(int argc, char* argv[]) {
int fd_stdin = fileno(stdin);
signal(SIGINT, signal_handler);
signal(SIGTERM, signal_handler);
signal(SIGQUIT, signal_handler);
unsigned char buffer[65536];
FLVFileWriter flv("out.flv");
while (!exit_requested) {
int readedSize = read(fd_stdin,buffer,65536);
if (readedSize==0)
break;
flv.parseH264(buffer,readedSize);
}
flv.endOfStreamH264();
signal(SIGINT, SIG_DFL);
signal(SIGTERM, SIG_DFL);
signal(SIGQUIT, SIG_DFL);
return 0;
}
Compiling the program above we can execute as follows:
cat test.h264 | ./flv
Then I get this:
I used the ffmpeg from command line to check if my source h264 stream file is corrupted. So I ran the following:
cat test.h264 | ffmpeg -i - -c:v copy out2.flv
With ffmpeg the result is OK:
I put the video I tested with the full source code here.
The h264 file I used for the test.
WIP (work in progress)
But I found a lot of new information :-).
First: The FLV container does not work packing one by one NAL buffers from h264.
Second: The timestamp needs to be increased just when a new frame is available.
The problem with the green screen I got is related to the fragmentation on NAL packages with the timestamp issue.
The difficult part is to detect when the stream is getting a new frame. There are several ways as mentioned here.
But the h264 stream have data members with different bit sizes in its structure.
Reading Bits
ue(v) data type. To read this structure you need to count the leading 0 bits until reach bit 1 and read the same amount of 0 bits after the 1 bit we found.
The content of this field inside the headers of h264 can vary... one common value is called golomb, and it is calculated with this formula: (1 << leadingZeros) - 1 + readbits(leadingZeros).
RBSP - Raw Byte Sequence Payload
We cannot read the stream values directly, because it may contains the emulation prevention bytes. We need to remove the emulation prevention bytes before read the headers.
The Code
So to start mitigate the problem first I rewrite the H264 parser as follow:
struct sps_info{
uint8_t profile_idc;
uint8_t constraints;
uint8_t level_idc;
uint8_t log2_max_frame_num;
bool set;
};
class ParserH264 {
std::vector<uint8_t> buffer;
enum State{
None,
Data,
NAL0,
NAL1,
EMULATION0,
EMULATION1,
EMULATION_FORCE_SKIP
};
State nalState; // nal = network abstraction layer
State writingState;
void putByte(uint8_t byte) {
//
// Detect start code 00 00 01 and 00 00 00 01
//
// It returns the buffer right after the start code
//
if (byte == 0x00 && nalState == None)
nalState = NAL0;
else if (byte == 0x00 && (nalState == NAL0 || nalState == NAL1) )
nalState = NAL1;
else if (byte == 0x01 && nalState == NAL1){
nalState = None;
if (writingState == None){
writingState = Data;
return;
} else if (writingState == Data){
buffer.pop_back();// 0x00
buffer.pop_back();// 0x00
//in the case using the format 00 00 00 01, remove the last element detected
if (buffer.size()-2 >=0 &&
buffer[buffer.size()-1] == 0x00 &&
buffer[buffer.size()-2] != 0x03 )//keep value, if emulation prevention is present
buffer.pop_back();
chunkDetectedH264(buffer);
buffer.clear();
return;
}
} else
nalState = None;
if (writingState == Data){
//
// increase raw buffer size
//
buffer.push_back(byte);
}
}
public:
ParserH264() {
nalState = None;
writingState = None;
}
virtual ~ParserH264(){
}
// rawBuffer might has emulation bytes
// Raw Byte Sequence Payload (RBSP)
virtual void chunkDetectedH264(const std::vector<uint8_t> &nal){
}
void endOfStreamH264() {
if (buffer.size() <= 0)
return;
chunkDetectedH264(buffer);
buffer.clear();
writingState = None;
nalState = None;
}
void parseH264(const uint8_t* ibuffer, int size) {
for(int i=0;i<size;i++){
putByte(ibuffer[i]);
}
}
static inline uint32_t readbit(int bitPos, const uint8_t* data, int size){
int dataPosition = bitPos / 8;
int bitPosition = 7 - bitPos % 8;
if (dataPosition >= size){
fprintf(stderr,"error to access bit...\n");
exit(-1);
}
return (data[dataPosition] >> bitPosition) & 0x01;
}
// leading 0`s count equals the number of next bits after bit 1
//
// Example: 01x 001xx 0001xxx 00001xxxx
//
// The max number of bits is equal 32 in this sintax
//
static inline int bitsToSkip_ue( int start, const uint8_t* data, int size){
int bitPos = start;
int dataPosition = start / 8;
int bitPosition;
while (dataPosition < size){
dataPosition = bitPos / 8;
bitPosition = 7 - bitPos % 8;
int bit = (data[dataPosition] >> bitPosition) & 0x01;
if (bit == 1)
break;
bitPos++;
}
int leadingZeros = bitPos - start;
int totalBits = leadingZeros + 1 + leadingZeros;
if (totalBits > 32){
fprintf(stderr,"bitsToSkip_ue length greated than 32\n");
exit(-1);
}
return totalBits;
}
static inline uint32_t read_golomb_ue(int start, const uint8_t* data, int size){
int bitPos = start;
int dataPosition = start / 8;
int bitPosition;
while (dataPosition < size){
dataPosition = bitPos / 8;
bitPosition = 7 - bitPos % 8;
int bit = (data[dataPosition] >> bitPosition) & 0x01;
if (bit == 1)
break;
bitPos++;
}
uint32_t leadingZeros = (uint32_t)(bitPos - start);
uint32_t num = readbits(bitPos+1, leadingZeros, data, size);
num += (1 << leadingZeros) - 1;
return num;
}
static inline uint32_t readbits(int bitPos, int length, const uint8_t* data, int size){
if (length > 32){
fprintf(stderr,"readbits length greated than 32\n");
exit(-1);
}
uint32_t result = 0;
for(int i=0;i<length;i++){
result <<= 1;
result = result | readbit(bitPos+i, data, size);
}
return result;
}
static inline sps_info parseSPS(const std::vector<uint8_t> sps_rbsp) {
const uint8_t *data = &sps_rbsp[0];
uint32_t size = sps_rbsp.size();
sps_info result;
result.profile_idc = sps_rbsp[1];
result.constraints = sps_rbsp[2];
result.level_idc = sps_rbsp[3];
uint32_t startIndex = 8+24;//NAL bit + profile_idc (8bits) + constraints (8bits) + level_idc (8bits)
startIndex += bitsToSkip_ue(startIndex, data, size);//seq_parameter_set_id (ue)
uint32_t log2_max_frame_num_minus4 = read_golomb_ue(startIndex, data, size);
if (log2_max_frame_num_minus4 < 0 ||
log2_max_frame_num_minus4 > 12){
fprintf(stderr,"parseSPS_log2_max_frame_num_minus4 value not in range [0-12] \n");
exit(-1);
}
result.log2_max_frame_num = log2_max_frame_num_minus4 + 4;
result.set = true;
return result;
}
// Raw Byte Sequence Payload (RBSP) -- without the emulation prevention bytes
// maxSize is used to parse just the beggining of the nal structure...
// avoid process all buffer size on NAL new frame check
static inline void nal2RBSP(const std::vector<uint8_t> &buffer,
std::vector<uint8_t> *rbsp,
int maxSize = -1){
if (maxSize <= 0)
maxSize = buffer.size();
rbsp->resize(maxSize);
State emulationState = None;
int count = 0;
for(int i=0; i < maxSize ;i++){
uint8_t byte = buffer[i];
if (byte == 0x00 && emulationState == None)
emulationState = EMULATION0;
else if (byte == 0x00 && (emulationState == EMULATION0 || emulationState == EMULATION1) )
emulationState = EMULATION1;
else if (byte == 0x03 && emulationState == EMULATION1)
{
emulationState = EMULATION_FORCE_SKIP;
continue;
}
else if (emulationState == EMULATION_FORCE_SKIP) { //skip 00 01 02 or 03
if ( byte != 0x00 && byte != 0x01 && byte != 0x02 && byte != 0x03 ){
fprintf(stdout, "H264 NAL emulation prevention pattern detected error (%u)\n", byte);
exit(-1);
}
emulationState = None;
} else
emulationState = None;
(*rbsp)[count] = byte;
count++;
}
if (count != rbsp->size())
rbsp->resize(count);
}
};
I updated the FLVWriter to write the whole frame (with 1 or more NALs) and to write the video sequence header.
The code:
class FLVWritter{
public:
std::vector<uint8_t> buffer;
void flushToFD(int fd) {
if (buffer.size() <= 0)
return;
::write(fd,&buffer[0],buffer.size());
buffer.clear();
}
void reset(){
buffer.clear();
}
void writeFLVFileHeader(bool haveAudio, bool haveVideo){
uint8_t flv_header[13] = {
0x46, 0x4c, 0x56, 0x01, 0x05,
0x00, 0x00, 0x00, 0x09, 0x00,
0x00, 0x00, 0x00
};
if (haveAudio && haveVideo) {
flv_header[4] = 0x05;
} else if (haveAudio && !haveVideo) {
flv_header[4] = 0x04;
} else if (!haveAudio && haveVideo) {
flv_header[4] = 0x01;
} else {
flv_header[4] = 0x00;
}
for(int i=0;i<13;i++)
buffer.push_back(flv_header[i]);
}
void writeUInt8(uint8_t v) {
buffer.push_back(v);
}
void writeUInt16(uint32_t v){
buffer.push_back((v >> 8) & 0xff);
buffer.push_back((v) & 0xff);
}
void writeUInt24(uint32_t v){
buffer.push_back((v >> 16) & 0xff);
buffer.push_back((v >> 8) & 0xff);
buffer.push_back((v) & 0xff);
}
void writeUInt32(uint32_t v){
buffer.push_back((v >> 24) & 0xff);
buffer.push_back((v >> 16) & 0xff);
buffer.push_back((v >> 8) & 0xff);
buffer.push_back((v) & 0xff);
}
void writeUInt32(uint32_t v, std::vector<uint8_t> *data){
data->push_back((v >> 24) & 0xff);
data->push_back((v >> 16) & 0xff);
data->push_back((v >> 8) & 0xff);
data->push_back((v) & 0xff);
}
void writeUInt32Timestamp(uint32_t v){
buffer.push_back((v >> 16) & 0xff);
buffer.push_back((v >> 8) & 0xff);
buffer.push_back((v) & 0xff);
buffer.push_back((v >> 24) & 0xff);
}
void writeVideoSequenceHeader(const std::vector<uint8_t> &sps, const std::vector<uint8_t> &pps, const sps_info &spsinfo) {
//
// flv tag header = 11 bytes
//
writeUInt8(0x09);//tagtype video
writeUInt24( sps.size() + pps.size() + 16 );//data len
writeUInt32Timestamp( 0 );//timestamp
writeUInt24( 0 );//stream id 0
//
// Message Body = 16 bytes + SPS bytes + PPS bytes
//
//flv VideoTagHeader
writeUInt8(0x17);//key frame, AVC 1:keyframe 7:h264
writeUInt8(0x00);//avc sequence header
writeUInt24( 0x00 );//composition time
//flv VideoTagBody --AVCDecoderCOnfigurationRecord
writeUInt8(0x01);//configurationversion
writeUInt8(spsinfo.profile_idc);//avcprofileindication
writeUInt8(spsinfo.constraints);//profilecompatibilty
writeUInt8(spsinfo.level_idc);//avclevelindication
writeUInt8(0xFC | 0x03); //reserved (6 bits), NULA length size - 1 (2 bits)
writeUInt8(0xe0 | 0x01); // first reserved, second number of SPS
writeUInt16( sps.size() ); //sequence parameter set length
//H264 sequence parameter set raw data
for(int i=0;i<sps.size();i++)
writeUInt8(sps[i]);
writeUInt8(0x01); // number of PPS
writeUInt16(pps.size()); //picture parameter set length
//H264 picture parameter set raw data
for(int i=0;i<pps.size();i++)
writeUInt8(pps[i]);
if (buffer.size() != sps.size() + pps.size() + 16 + 11 ){
fprintf(stderr, "error writeVideoSequenceHeader\n");
exit(-1);
}
// previous tag size
writeUInt32(buffer.size());
}
void writeVideoFrame(const std::vector<uint8_t> &data, bool keyframe, uint32_t timestamp_ms, int streamID){
writeUInt8(0x09);//tagtype video
writeUInt24( data.size() + 5 );//data len
writeUInt32Timestamp( timestamp_ms );//timestamp
writeUInt24( streamID );//stream id 0)
if (keyframe)
writeUInt8(0x17);//key frame, AVC 1:keyframe 2:inner frame 7:H264
else
writeUInt8(0x27);//key frame, AVC 1:keyframe 2:inner frame 7:H264
writeUInt8(0x01);//avc NALU unit
writeUInt24(0x00);//composit time ??????????
for(int i=0;i<data.size();i++)
writeUInt8(data[i]);
if (buffer.size() != data.size() + 5 + 11 ){
fprintf(stderr, "error writeVideoFrame\n");
exit(-1);
}
// previous size
writeUInt32(buffer.size());
}
void writeVideoEndOfStream(uint32_t timestamp_ms, int streamID){
writeUInt8(0x09);//tagtype video
writeUInt24( 5 );//data len
writeUInt32Timestamp( timestamp_ms );//timestamp
writeUInt24( streamID );//stream id 0)
writeUInt8(0x17);//key frame, AVC 1:keyframe 2:inner frame 7:H264
writeUInt8(0x02);//avc EOS
writeUInt24(0x00);//composit time ??????????
if (buffer.size() != 5 + 11 ){
fprintf(stderr, "error writeVideoEOS\n");
exit(-1);
}
// previous size
writeUInt32(buffer.size());
}
};
Next we need to detect new frame from the stream.
I've implemented several methods and the frame_num method also.
To read the frame_num, we need to skip 3 ue(v) structures before read the frame_num bits.
Now the h264 new frame detector class:
class H264NewFrameDetection {
std::vector<uint8_t> aux;
bool newFrameOnNextIDR;
uint32_t old_frame_num;
bool spsinfo_set;
bool firstFrame;
public:
uint32_t currentFrame;
bool newFrameFound;
H264NewFrameDetection() {
currentFrame = 0;
newFrameOnNextIDR = false;
old_frame_num = 0;
spsinfo_set = false;
firstFrame = true;
}
void analyseBufferForNewFrame(const std::vector<uint8_t> &nal, const sps_info &spsinfo){
uint8_t nal_bit = nal[0];
uint8_t nal_type = (nal_bit & 0x1f);
if ( nal_type == (NAL_TYPE_SPS) ||
nal_type == (NAL_TYPE_PPS) ||
nal_type == (NAL_TYPE_AUD) ||
nal_type == (NAL_TYPE_SEI) ||
(nal_type >= 14 && nal_type <= 18)
) {
newFrameOnNextIDR = true;
}
if ((nal_type == NAL_TYPE_CSIDRP ||
nal_type == NAL_TYPE_CSNIDRP)
&& spsinfo.set ){
aux.clear();
//(8 + 3*(32*2+1) + 16) = max header per NALU slice bits = 27.375 bytes
// 32 bytes + 8 (Possibility of Emulation in 32 bytes)
int RBSPMaxBytes = 32 + 8;
if (nal.size() < (32 + 8))
RBSPMaxBytes = nal.size();
ParserH264::nal2RBSP(nal, &aux, RBSPMaxBytes );
uint8_t * rbspBuffer = &aux[0];
uint32_t rbspBufferSize = aux.size();
uint32_t frame_num_index = 8;//start counting after the nal_bit
//first_mb_in_slice (ue)
frame_num_index += ParserH264::bitsToSkip_ue(frame_num_index, rbspBuffer, rbspBufferSize);
//slice_type (ue)
frame_num_index += ParserH264::bitsToSkip_ue(frame_num_index, rbspBuffer, rbspBufferSize);
//pic_parameter_set_id (ue)
frame_num_index += ParserH264::bitsToSkip_ue(frame_num_index, rbspBuffer, rbspBufferSize);
//now can read frame_num
uint32_t frame_num = ParserH264::readbits(frame_num_index,
spsinfo.log2_max_frame_num,
rbspBuffer, rbspBufferSize);
if (!spsinfo_set){
old_frame_num = frame_num;
spsinfo_set = true;//spsinfo.set
}
if (old_frame_num != frame_num){
newFrameOnNextIDR = true;
old_frame_num = frame_num;
}
}
if (newFrameOnNextIDR &&(nal_type == NAL_TYPE_CSIDRP ||
nal_type == NAL_TYPE_CSNIDRP)) {
newFrameOnNextIDR = false;
if (firstFrame){//skip the first frame
firstFrame = false;
} else {
newFrameFound = true;
currentFrame++;
}
}
}
void reset(){
newFrameFound = false;
}
};
Finally the writer class.
In this implementation I store the NALs until a new frame is detected. When it founds a new frame I write the FLV Tag with the NAL set (all NALs stored in the list).
The code:
class FLVFileWriter: public ParserH264 {
std::vector<uint8_t> sps;
std::vector<uint8_t> pps;
sps_info spsInfo;
H264NewFrameDetection mH264NewFrameDetection;
public:
FLVWritter mFLVWritter;
int fd;
uint32_t videoTimestamp_ms;
//contains the several NALs until complete a frame... after that can write to FLV
std::vector<uint8_t> nalBuffer;
FLVFileWriter ( const char* file ) {
fd = open(file, O_WRONLY | O_CREAT | O_TRUNC , 0644 );
if (fd < 0){
fprintf(stderr,"error to create flv file\n");
exit(-1);
}
//have audio, have video
mFLVWritter.writeFLVFileHeader(false, true);
mFLVWritter.flushToFD(fd);
videoTimestamp_ms = 0;
spsInfo.set = false;
}
virtual ~FLVFileWriter(){
if (fd >= 0){
//force write the last frame
if (nalBuffer.size() > 0){
uint8_t firstNALtype = nalBuffer[4] & 0x1f;
bool iskeyframe = (firstNALtype == NAL_TYPE_CSIDRP);
mFLVWritter.writeVideoFrame(nalBuffer, iskeyframe, videoTimestamp_ms, 0);
mFLVWritter.flushToFD(fd);
nalBuffer.clear();
mFLVWritter.writeVideoEndOfStream(videoTimestamp_ms,0);
mFLVWritter.flushToFD(fd);
} else {
if (mH264NewFrameDetection.currentFrame > 0)
videoTimestamp_ms = ((mH264NewFrameDetection.currentFrame-1) * 1000)/30;
mFLVWritter.writeVideoEndOfStream(videoTimestamp_ms,0);
mFLVWritter.flushToFD(fd);
}
close(fd);
}
}
//decoding time stamp (DTS) and presentation time stamp (PTS)
void chunkDetectedH264(const std::vector<uint8_t> &data) {
if (data.size() <= 0){
fprintf(stdout, " error On h264 chunk detection\n");
return;
}
uint8_t nal_bit = data[0];
uint8_t nal_type = (nal_bit & 0x1f);
mH264NewFrameDetection.analyseBufferForNewFrame(data, spsInfo);
if (mH264NewFrameDetection.newFrameFound){
mH264NewFrameDetection.reset();
uint8_t firstNALtype = nalBuffer[4] & 0x1f;
bool iskeyframe = (firstNALtype == NAL_TYPE_CSIDRP);
mFLVWritter.writeVideoFrame(nalBuffer, iskeyframe, videoTimestamp_ms, 0);
mFLVWritter.flushToFD(fd);
nalBuffer.clear();
videoTimestamp_ms = (mH264NewFrameDetection.currentFrame * 1000)/30;
}
//0x67
if ( nal_type == (NAL_TYPE_SPS) ) {
fprintf(stdout, " processing: 0x%x (SPS)\n",nal_bit);
sps.clear();
//max 26 bytes on sps header (read until log2_max_frame_num_minus4)
nal2RBSP(data, &sps, 26 );
spsInfo = parseSPS(sps);
sps.clear();
for(int i=0;i<data.size();i++)
sps.push_back(data[i]);
}
//0x68
else if ( nal_type == (NAL_TYPE_PPS) ) {
fprintf(stdout, " processing: 0x%x (PPS)\n",nal_bit);
pps.clear();
for(int i=0;i<data.size();i++)
pps.push_back(data[i]);
mFLVWritter.writeVideoSequenceHeader(sps, pps, spsInfo);
mFLVWritter.flushToFD(fd);
}
//0x65, 0x61, 0x41
else if ( nal_type == NAL_TYPE_CSIDRP ||
nal_type == NAL_TYPE_CSNIDRP ) {
//convert annexb to AVCC (length before the NAL structure)
mFLVWritter.writeUInt32( data.size(), &nalBuffer );
for(int i=0;i<data.size();i++)
nalBuffer.push_back(data[i]);
} else if (nal_type == (NAL_TYPE_SEI)) {
fprintf(stdout, " ignoring SEI bit: 0x%x type: 0x%x\n",nal_bit, nal_type);
} else {
// nal_bit type not implemented...
fprintf(stdout, "Error: unknown NAL bit: 0x%x type: 0x%x\n",nal_bit, nal_type);
exit(-1);
}
}
};
Conclusions
After testing, the video works correctly with this new implementation.
The only issue I have now is related to PTS (presentation timestamp) and DTS (decoder timestamp). I don't know how to extract them from the stream and how to use them to compute the frame timestamp.
This current implementation uses the hardcoded ratio 1000/30 (e.g., 30fps) as basis to increment the timestamp.
The timestamp is in milliseconds.

Adafruit SHT31-D and Raspberry Pi2 -- Unable to read data from sensor

hopefully one of you out there can help me!
I am trying to use the Adafruit SHT31-D (an i2c device) board with my Pi2. I am going off of this datasheet to guide my coding efforts. I am using Wiring Pi (wiringpi.com) to facilitate things.
I am able to successfully open a connection to the device, and sending commands seems to work fine, but I am unable to read data back! Here is the little mini library I have put together. I am hoping that one of you might have some experience with this sort of thing and be able to help me see where I've gone wrong.
To rule out any possible issues with the sensor hardware, I have tested it with my Arduino UNO and it works without issues.
Here is my C++ code:
SHT3x.h
#pragma once
/* Sensor Commands */
#define DEFAULT_SHT_ADDR 0x44
#define MEAS_HREP_STRETCH 0x2C06
#define MEAS_MREP_STRETCH 0x2C0D
#define MEAS_LREP_STRETCH 0x2C10
#define MEAS_HREP 0x2400
#define MEAS_MREP 0x240B
#define MEAS_LREP 0x2416
#include <cstdint>
class SHT3x {
public:
SHT3x(const uint8_t& i2cAddr);
float readHumidity(const uint16_t& command) const;
float readTempC(const uint16_t& command) const;
float readTempF(const uint16_t& command) const;
private:
int8_t _fd;
uint8_t _header;
uint32_t getMeasurement(const uint16_t& command) const;
void sendCommand(const uint16_t& command) const;
uint32_t receiveData(void) const;
};
SHT3x.cpp
#include <stdexcept>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "SHT3x.h"
SHT3x::SHT3x(const uint8_t& i2cAddr) {
_fd = wiringPiI2CSetup(i2cAddr);
_header = i2cAddr << 1;
if (_fd < 0) {
throw std::runtime_error("Unable to connect");
}
}
float SHT3x::readHumidity(const uint16_t& command) const {
uint32_t raw_data = getMeasurement(command);
if (!raw_data) {
throw std::runtime_error("Bad Reading.");
}
uint16_t raw_humidity = raw_data & 0xFFFF;
float humidity = 100.0 * ((float) raw_humidity / (float) 0xFFFF);
return humidity;
}
float SHT3x::readTempC(const uint16_t& command) const {
uint32_t raw_data = getMeasurement(command);
if (!raw_data) {
throw std::runtime_error("Bad Reading.");
}
uint16_t raw_temp = raw_data >> 16;
float tempC = -45.0 + (175.0 * ((float) raw_temp / (float) 0xFFFF));
return tempC;
}
float SHT3x::readTempF(const uint16_t& command) const {
uint32_t raw_data = getMeasurement(command);
if (!raw_data) {
throw std::runtime_error("Bad Reading.");
}
uint16_t raw_temp = raw_data >> 16;
float tempF = -49.0 + (315.0 * ((float) raw_temp / (float) 0xFFFF));
return tempF;
}
uint32_t SHT3x::getMeasurement(const uint16_t& command) const {
try {
sendCommand(command);
} catch (std::runtime_error& e) {
throw;
}
return receiveData();
}
void SHT3x::sendCommand(const uint16_t& command) const {
// break command into bytes
uint8_t MSB = command >> 8;
uint8_t LSB = command & 0xFF;
// send header
int8_t ack = wiringPiI2CWrite(_fd, _header);
// send command
ack &= wiringPiI2CWrite(_fd, MSB);
ack &= wiringPiI2CWrite(_fd, LSB);
// handle errors
if (ack) {
throw std::runtime_error("Sending command failed.");
}
}
uint32_t SHT3x::receiveData(void) const {
uint32_t data;
// send header
uint8_t read_header = _header | 0x01;
int8_t ack = wiringPiI2CWrite(_fd, read_header);
// handle errors
if (ack) throw std::runtime_error("Unable to read data.");
// read data
data = wiringPiI2CRead(_fd);
for (size_t i = 0; i < 4; i++) {
printf("Data: %d\n", data);
data <<= 8;
if (i != 1) {
data |= wiringPiI2CRead(_fd);
} else {
wiringPiI2CRead(_fd); // skip checksum
}
}
wiringPiI2CRead(_fd); // second checksum
return data;
}
The SHT31 uses 16bit read and write, rather than using 2 8bit writes you might be better off using wiringpi's 16bit write. wiringPiI2CWriteReg16(). Same thing applies to the read.
Below is a very early copy of what I've done to read the sht31-d on a PI. It has no dependencies except i2c-dev. Heater enable/disable is not working, but softreset, clearstatus, getserial & get temp/humid are all fine.
/*
* Referances
* https://www.kernel.org/doc/Documentation/i2c/dev-interface
* https://github.com/adafruit/Adafruit_SHT31
* https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/Humidity_and_Temperature_Sensors/Sensirion_Humidity_and_Temperature_Sensors_SHT3x_Datasheet_digital.pdf
*
* This depends on i2c dev lib
* sudo apt-get install libi2c-dev
*
* Below is also a good one to have, but be careful i2cdump from the below cause the sht31 interface to become unstable for me
* and requires a hard-reset to recover correctly.
* sudo apt-get install i2c-tools
*
* on PI make sure below 2 commands are in /boot/config.txt
* dtparam=i2c_arm=on
* dtparam=i2c1_baudrate=10000
* I know we are slowing down the baurate from optimal, but it seems to be the most stable setting in my testing.
* add another 0 to the above baudrate for max setting, ie dtparam=i2c1_baudrate=100000
*/
#include <linux/i2c-dev.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <fcntl.h>
#include <elf.h>
#include <unistd.h>
#define SHT31_INTERFACE_ADDR 1
#define SHT31_DEFAULT_ADDR 0x44
#define SHT31_READ_SERIALNO 0x3780
#define SHT31_MEAS_HIGHREP_STRETCH 0x2C06 // Doesn't work on PI
#define SHT31_MEAS_MEDREP_STRETCH 0x2C0D // Seems to work on PI but shouldn't
#define SHT31_MEAS_LOWREP_STRETCH 0x2C10 // Seems to work on PI but shouldn't
#define SHT31_MEAS_HIGHREP 0x2400 // Doesn't work on PI
#define SHT31_MEAS_MEDREP 0x240B
#define SHT31_MEAS_LOWREP 0x2416
#define SHT31_READSTATUS 0xF32D
#define SHT31_CLEARSTATUS 0x3041
#define SHT31_SOFTRESET 0x30A2
#define SHT31_HEATER_ENABLE 0x306D
#define SHT31_HEATER_DISABLE 0x3066
#define CHECK_BIT(var,pos) (((var)>>(pos)) & 1)
/*
* delay:
* Wait for some number of milliseconds
*********************************************************************************
*/
void delay (unsigned int howLong)
{
struct timespec sleeper, dummy ;
sleeper.tv_sec = (time_t)(howLong / 1000) ;
sleeper.tv_nsec = (long)(howLong % 1000) * 1000000 ;
nanosleep (&sleeper, &dummy) ;
}
/*
*
* CRC-8 formula from page 14 of SHT spec pdf
*
* Test data 0xBE, 0xEF should yield 0x92
*
* Initialization data 0xFF
* Polynomial 0x31 (x8 + x5 +x4 +1)
* Final XOR 0x00
*/
uint8_t crc8(const uint8_t *data, int len)
{
const uint8_t POLYNOMIAL = 0x31;
uint8_t crc = 0xFF;
int j;
int i;
for (j = len; j; --j ) {
crc ^= *data++;
for ( i = 8; i; --i ) {
crc = ( crc & 0x80 )
? (crc << 1) ^ POLYNOMIAL
: (crc << 1);
}
}
return crc;
}
/*
*
* buffer should return with data read, size defined by readsize
*********************************************************************************
*/
int writeandread(int fd, uint16_t sndword, uint8_t *buffer, int readsize)
{
int rtn;
uint8_t snd[3];
// Split the 16bit word into two 8 bits that are flipped.
snd[0]=(sndword >> 8) & 0xff;
snd[1]=sndword & 0xff;
rtn = write(fd, snd, 2);
if ( rtn != 2 ) {
return 1;
}
if (readsize > 0) {
delay(10);
rtn = read(fd, buffer, readsize);
if ( rtn < readsize) {
return 2;
}
}
return 0;
}
void printserialnum(int file)
{
uint8_t buf[10];
int rtn;
rtn = writeandread(file, SHT31_READ_SERIALNO, buf, 6);
if (rtn != 0)
printf("ERROR:- Get serial i2c %s failed\n",(rtn==1?"write":"read"));
else {
if (buf[2] != crc8(buf, 2) || buf[5] != crc8(buf+3, 2))
printf("WARNING:- Get serial CRC check failed, don't trust result\n");
uint32_t serialNo = ((uint32_t)buf[0] << 24)
| ((uint32_t)buf[1] << 16)
| ((uint32_t)buf[3] << 8)
| (uint32_t)buf[4];
printf("Serial# = %d\n",serialNo);
}
}
void printtempandhumidity(int file)
{
uint8_t buf[10];
int rtn;
rtn = writeandread(file, SHT31_MEAS_MEDREP_STRETCH, buf, 6);
if (rtn != 0)
printf("ERROR:- Get temp/humidity i2c %s failed\n",(rtn==1?"write":"read"));
else {
if ( buf[2] != crc8(buf, 2) || buf[5] != crc8(buf+3, 2))
printf("WARNING:- Get temp/humidity CRC check failed, don't trust results\n");
uint16_t ST, SRH;
ST = buf[0];
ST <<= 8;
ST |= buf[1];
SRH = buf[3];
SRH <<= 8;
SRH |= buf[4];
double stemp = ST;
stemp *= 175;
stemp /= 0xffff;
stemp = -45 + stemp;
double stempf = ST;
stempf *= 315;
stempf /= 0xffff;
stempf = -49 + stempf;
printf("Temperature %.2fc - %.2ff\n",stemp,stempf);
double shum = SRH;
shum *= 100;
shum /= 0xFFFF;
printf("Humidity %.2f%%\n",shum);
}
}
void printBitStatus(uint16_t stat)
{
printf("Status\n");
printf(" Checksum status %d\n", CHECK_BIT(stat,0));
printf(" Last command status %d\n", CHECK_BIT(stat,1));
printf(" Reset detected status %d\n", CHECK_BIT(stat,4));
printf(" 'T' tracking alert %d\n", CHECK_BIT(stat,10));
printf(" 'RH' tracking alert %d\n", CHECK_BIT(stat,11));
printf(" Heater status %d\n", CHECK_BIT(stat,13));
printf(" Alert pending status %d\n", CHECK_BIT(stat,15));
}
void printstatus(int file)
{
uint8_t buf[10];
int rtn;
rtn = writeandread(file, SHT31_READSTATUS, buf, 3);
if (rtn != 0)
printf("ERROR:- readstatus %s failed\n",(rtn==1?"write":"read"));
else {
if ( buf[2] != crc8(buf, 2))
printf("WARNING:- Get status CRC check failed, don't trust results\n");
uint16_t stat = buf[0];
stat <<= 8;
stat |= buf[1];
printBitStatus(stat);
}
}
void clearstatus(int file)
{
if( writeandread(file, SHT31_CLEARSTATUS, NULL, 0) != 0)
printf("ERROR:- sht31 clear status failed\n");
else
printf("Clearing status - ok\n");
}
void softreset(int file)
{
if( writeandread(file, SHT31_SOFTRESET, NULL, 0) != 0)
printf("ERROR:- sht31 soft reset failed\n");
else
printf("Soft reset - ok\n");
}
void enableheater(int file)
{
if( writeandread(file, SHT31_HEATER_ENABLE, NULL, 0) != 0)
printf("ERROR:- sht31 heater enable failed\n");
else
printf("Enabiling heater - ok\n");
}
void disableheater(int file)
{
if( writeandread(file, SHT31_HEATER_DISABLE, NULL, 0) != 0)
printf("ERROR:- sht31 heater enable failed\n");
else
printf("Disableing heater - ok\n");
}
int main()
{
int file;
char filename[20];
snprintf(filename, 19, "/dev/i2c-%d", SHT31_INTERFACE_ADDR);
file = open(filename, O_RDWR);
if (file < 0) {
printf("ERROR:- Can't open %s\n",filename);
exit(1);
}
if (ioctl(file, I2C_SLAVE, SHT31_DEFAULT_ADDR) < 0) {
printf("ERROR:- Connecting to sht31 I2C address 0x%02hhx\n", SHT31_DEFAULT_ADDR);
exit(1);
}
softreset(file);
printtempandhumidity(file);
printstatus(file);
close(file);
return 0;
}

AVR UART receives wrong bytes

after a long pause I started to program AVRs again. I was able to setup a UART communication between my Linux computer and an Atmega8. Unfortunately the Atmega8 seems to receive the wrong bytes when I send bytes greater than 0x1f.
The UART is running with 9600 BAUD and the data format 8N1.
// clock frequency 1Mhz
#define F_CPU 1000000UL
// baud rate
#define BAUD 9600
#define BAUDRATE ((F_CPU)/(BAUD*16UL)-1)
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>
void uart_init (void);
void uart_putc (unsigned char data);
void uart_puts (unsigned char * str, uint8_t size);
// interrupt service routine for UART receiver interupt
ISR(USART_RXC_vect) {
// get received char
unsigned char received_char = UDR;
uart_puts("received=", 9);
if((received_char & (1<<7)) == (1<<7)) {
uart_putc('1');
} else {
uart_putc('0');
}
if((received_char & (1<<6)) == (1<<6)) {
uart_putc('1');
} else {
uart_putc('0');
}
if((received_char & (1<<5)) == (1<<5)) {
uart_putc('1');
} else {
uart_putc('0');
}
if((received_char & (1<<4)) == (1<<4)) {
uart_putc('1');
} else {
uart_putc('0');
}
if((received_char & (1<<3)) == (1<<3)) {
uart_putc('1');
} else {
uart_putc('0');
}
if((received_char & (1<<2)) == (1<<2)) {
uart_putc('1');
} else {
uart_putc('0');
}
if((received_char & (1<<1)) == (1<<1)) {
uart_putc('1');
} else {
uart_putc('0');
}
if((received_char & (1<<0)) == (1<<0)) {
uart_putc('1');
} else {
uart_putc('0');
}
uart_puts("\n\r",2);
}
// function to initialize UART
// dataformat 8N1
void uart_init (void) {
// shift the register right by 8 bits
UBRRH = (BAUDRATE>>8);
// set baud rate
UBRRL = BAUDRATE;
// enable receiver, transmitter and receiver interrupt
UCSRB|= (1<<TXEN)|(1<<RXEN)|(1<<RXCIE);
// 8bit data format
UCSRC|= (1<<URSEL)|(1<<UCSZ0)|(1<<UCSZ1);
}
// sends a single char over the UART
void uart_putc (unsigned char data) {
// wait while register is free
while (!( UCSRA & (1<<UDRE)));
// load data in the register
UDR = data;
}
// sends a string over the UART
void uart_puts (unsigned char * str, uint8_t size) {
uint8_t i;
for(i = 0; i < size; i++) {
uart_putc(str[i]);
}
}
// receives a single char over the UART
unsigned char uart_getc (void) {
// wait while data is being received
while(!(UCSRA) & (1<<RXC));
// return 8-bit data
return UDR;
}
uint8_t main (void) {
// enable interrupts
sei();
// enable uart
uart_init();
uart_puts("ready\n\r", 7);
while(1) {
}
return 0;
}
In GtkTerm I send the following byte sequence: 0x01, 0x02, 0x03, 0x1d, 0x1e, 0x1f, 0x20 and interrupt service routine ISR responds with the following sequence.
However, I should receive received=00100000 for 0x20
Whats wrong?
From my comment:
Maybe the baud rate is off due to using a slow clock rate / internal RC oszillator? The slower the baud rate, the greater the offset from the "middle" of each bit. This can lead to skipping bits or the controller seeing a bit twice.