static void data_handler(struct inv_icm30xxx * s, uint8_t sensorid,
uint32_t timestamp, uint8_t status, uint8_t accuracy,
const uint8_t * data, uint16_t size)
{
/* correspond to a GETDATA command */
if(status == FIFOPROTOCOL_STATUS_POLL) {
/* copy response back */
s->response.sensorid = sensorid;
s->response.command = FIFOPROTOCOL_CMD_GETDATA;
s->response.payload.sensor_data.timestamp = timestamp;
/* Check response size */
if (size > sizeof(s->response.payload.sensor_data.data))
{
INV_MSG(INV_MSG_LEVEL_WARNING, "Response from command longer "
"than expected. Dropping %d bytes", size - sizeof(s->response.payload.sensor_data.data));
size = sizeof(s->response.payload.sensor_data.data);
}
memcpy(s->response.payload.sensor_data.data, data, size);
s->response.payload.sensor_data.len = size;
/* signal the response */
s->response.event = true;
} else { /* asynchronous sensor event */
/* notify upper layer */
s->handler.data(s->handler.context, sensorid,
timestamp, status, accuracy, data, size);
}
}
/// Executes a statement that is not supposed to return any data.
///
/// Use this function to execute DDL and INSERT statements; i.e. statements that
/// only have one processing step and deliver no rows. This frees the caller
/// from having to deal with the return value of the step() function.
///
/// \pre The statement to execute will not produce any rows.
void
sqlite::statement::step_without_results(void)
{
const bool data = step();
INV_MSG(!data, "The statement should not have produced any rows, but it "
"did");
}
/// Gets the raw value of an option.
///
/// The raw value of an option is a collection of strings that represent all the
/// values passed to the option on the command line. It is up to the consumer
/// if he wants to honor only the last value or all of them.
///
/// The caller has to use get_option() instead; this function is internal.
///
/// \pre has_option(name) must be true.
///
/// \param name The option to query.
///
/// \return The value of the option as a plain string.
const std::vector< std::string >&
cmdline::parsed_cmdline::get_option_raw(const std::string& name) const
{
std::map< std::string, std::vector< std::string > >::const_iterator iter =
_option_values.find(name);
INV_MSG(iter != _option_values.end(), F("Undefined option --%s") % name);
return (*iter).second;
}
/// Executes a statement that is not supposed to return any data.
///
/// Use this function to execute DDL and INSERT statements; i.e. statements that
/// only have one processing step and deliver no rows. This frees the caller
/// from having to deal with the return value of the step() function.
///
/// \pre The statement to execute will not produce any rows.
void
sqlite::statement::step_without_results(void)
{
#if defined(__minix) && !defined(NDEBUG)
const bool data =
#endif /* defined(__minix) && !defined(NDEBUG) */
step();
INV_MSG(!data, "The statement should not have produced any rows, but it "
"did");
}
int inv_icm30xxx_uartfifo_get_self_test(struct inv_icm30xxx * s)
{
int rc;
/* For self test need to wait 10 seconds to get an answer */
if((rc = send_cmd(s, '0', UARTFIFOPROTOCOL_CMD_SELFTEST, 0, 0, 10000)) != 0)
return rc;
INV_MSG(INV_MSG_LEVEL_INFO, "SelfTest result is %d", s->response.payload.sensor_data.data[0]);
return 0;
}
int inv_icm30xxx_ctrl_poll_audio(struct inv_icm30xxx * s, int fifo_mask)
{
assert(s->variant == INV_ICM30XXX_VARIANT_BERLIN);
if(fifo_mask & INV_ICM30XXX_FIFO_ID_2_MASK(INV_ICM30XXX_FIFO_ID_2)) {
/* Set flat to indicate to poll FIFO for audio data */
INV_MSG(INV_MSG_LEVEL_DEBUG, "Start polling audio fifo...");
s->audio_flag |= INV_ICM30XXX_POLL_AUDIO_FLAG;
}
/* Poll audio FIFO if flag is set or when in streaming mode */
if(s->audio_flag & INV_ICM30XXX_POLL_AUDIO_FLAG) {
return do_poll_audio_data(s, INV_ICM30XXX_FIFO_ID_2);
}
/* Polling will stop upon explicit request from upper later
by calling inv_icm30xxx_ctrl_stop_poll_audio() */
return 0;
}
static int send_cmd(struct inv_icm30xxx * s, char sensor, uint8_t cmd,
uint32_t payload, uint16_t len, uint32_t wait_response)
{
int rc = 0;
if(wait_response) {
s->response.event = false;
}
if((rc = uartfifo_send_command(s, sensor, cmd, payload, len)) != 0) {
INV_MSG(INV_MSG_LEVEL_ERROR, "Error %d while sending command '%d'"
" (payload: %d bytes)", rc, cmd, len);
return rc;
}
if(wait_response) {
int timeout;
if(wait_response != 1)
timeout = wait_response;
else
timeout = 1000; /* 1s by default */
/* poll FIFO until we received the answer */
do {
if((rc = inv_icm30xxx_uartfifo_poll(s)) != 0)
return rc;
inv_icm30xxx_sleep(10);
timeout -= 10;
if(timeout <= 0)
return INV_ERROR_TIMEOUT;
} while(!s->response.event);
}
return rc;
}
/*---------------------SEND COMMANDS------------------------*/
static int uartfifo_send_command(struct inv_icm30xxx * s, char sensorid,
uint8_t command, uint32_t arg, uint16_t size)
{
int rc;
uint8_t lIdx = 0;
uint8_t buffer[128];
const struct inv_icm30xxx_fifo_state * fifo = &s->fifo_states[INV_ICM30XXX_FIFO_ID_0];
/* device must be in on */
if((rc = inv_icm30xxx_lp_enable(s, false)) != 0)
return rc;
switch (command)
{
case UARTFIFOPROTOCOL_CMD_GETINFO:
buffer[lIdx++] = 'e';
buffer[lIdx++] = ' ';
buffer[lIdx++] = 'i';
buffer[lIdx++] = ENTER_KEYCODE;
break;
case UARTFIFOPROTOCOL_CMD_ENABLE:
buffer[lIdx++] = 'e';
buffer[lIdx++] = ' ';
buffer[lIdx++] = sensorid;
buffer[lIdx++] = ENTER_KEYCODE;
break;
case UARTFIFOPROTOCOL_CMD_DISABLE:
buffer[lIdx++] = 'd';
buffer[lIdx++] = ' ';
buffer[lIdx++] = sensorid;
buffer[lIdx++] = ENTER_KEYCODE;
break;
case UARTFIFOPROTOCOL_CMD_SETODR:
{
char lOdr[5];
buffer[lIdx++] = 'o';
buffer[lIdx++] = ' ';
buffer[lIdx++] = sensorid;
buffer[lIdx++] = ' ';
sprintf(lOdr, "%u", (unsigned)arg);
strcpy((char *)&buffer[lIdx], lOdr);
lIdx += strlen(lOdr);
buffer[lIdx++] = ENTER_KEYCODE;
break;
}
case UARTFIFOPROTOCOL_CMD_BATCH:
{
char lBatch[8];
buffer[lIdx++] = 'b';
buffer[lIdx++] = ' ';
sprintf(lBatch, "%u", (unsigned)arg);
strcpy((char *)&buffer[lIdx], lBatch);
lIdx += strlen(lBatch);
buffer[lIdx++] = ENTER_KEYCODE;
break;
}
case UARTFIFOPROTOCOL_CMD_SELFTEST:
{
buffer[lIdx++] = 'e';
buffer[lIdx++] = ' ';
buffer[lIdx++] = 'T';
buffer[lIdx++] = ENTER_KEYCODE;
break;
}
default:
INV_MSG(INV_MSG_LEVEL_ERROR, "Error while sending command '%d'", command);
break;
}
size = lIdx;
/* write all bytes to fifo */
if((rc = inv_icm30xxx_fifo_write(s, fifo->fifo_idx, buffer, size)) != 0)
return rc;
inv_icm30xxx_lp_enable(s, true);
return rc;
}
/*!
******************************************************************************
* @brief Decode whole FIFO data content, containing both
* - sensor data in binary frame with 33 bytes fixed size
* - classical printf prefixed with 0xFE and suffixed with 0xFF
* @param[in] data incoming FIFO data
* @param[in] len the length of the incoming FIFO data
* @return None
******************************************************************************
*/
static void decode_fifo_content(struct inv_icm30xxx * s, uint8_t fifo_idx, unsigned short len )
{
struct inv_icm30xxx_fifo_state * fifo = &s->fifo_states[fifo_idx];
unsigned short lNbBytesToParse = len + fifo->buffer_len; // total number of bytes to be analyzed in frame
unsigned short lNextIndexToParse = 0; // index in frame at which next frame to be analyzed starts
char * data = (char *)fifo->buffer; // HW FIFO mirroring
do
{
// First byte is either a known sensor id or the printf start token
// Check for printf start token first
if (data[lNextIndexToParse] == (char)START_PRINTF_TOKEN)
{
// This is a classical printf, and not sensor data in binary format, so look for printf end token now
unsigned short index;
unsigned short lFirstCharIndex = lNextIndexToParse+1; // Very first payload byte after start printf token
// loop through whole rest of FIFO data or until the end printf token is found
for ( index = lFirstCharIndex; index < lNbBytesToParse ; index ++)
{
// Printf end token found
if ( data[index] == (char)END_PRINTF_TOKEN )
{
unsigned short i = lFirstCharIndex;
unsigned short lframesize = (index - lFirstCharIndex);
char lStrToPrint[256]= {'\0'};
// print on console the bytes we looped through
while(lframesize--)
{
strncat(lStrToPrint, &data[i++], 1);
}
INV_MSG(INV_MSG_LEVEL_VERBOSE, "%s", lStrToPrint);
// Next time frame must be parsed, it must discard the bytes we just printed out
lNextIndexToParse=index+1;
fifo->buffer_len = 0;
break;
}
}
// we found a printf start token but no associated end token, so save buffer for next time
if ( (index == lNbBytesToParse) && (data[lNbBytesToParse-1] != (char)END_PRINTF_TOKEN) )
{
memmove(&data[fifo->buffer_len],
&data[fifo->buffer_len+lNextIndexToParse],
lNbBytesToParse - lNextIndexToParse);
fifo->buffer_len += lNbBytesToParse - lNextIndexToParse;
memset (&data[fifo->buffer_len],
0x00 ,
fifo->buffer_max-fifo->buffer_len);
break;
}
} else
{
//This is a sensor data frame in binary format, look for SENSOR_BINARY_DATA_FRAME_SIZE bytes
if (lNbBytesToParse-lNextIndexToParse < SENSOR_BINARY_DATA_FRAME_SIZE )
{
// we do not have enough bytes to decode one entire binary frame, so wait for next round to have more data
// and then save remaining buffer for next time
memmove(&data[fifo->buffer_len],
&data[fifo->buffer_len+lNextIndexToParse],
lNbBytesToParse - lNextIndexToParse);
fifo->buffer_len += lNbBytesToParse - lNextIndexToParse;
memset (&data[fifo->buffer_len],
0x00 ,
fifo->buffer_max-fifo->buffer_len);
break;
} else
{
// process SENSOR_BINARY_DATA_FRAME_SIZE bytes to print them correctly
decode_one_binary_frame(s, data+lNextIndexToParse);
// we just ate SENSOR_BINARY_DATA_FRAME_SIZE bytes, remove them from list of bytes still to be parsed
lNextIndexToParse += SENSOR_BINARY_DATA_FRAME_SIZE;
fifo->buffer_len = 0;
}
}
} while ( lNextIndexToParse<lNbBytesToParse); // loop through whole frame buffer
}
static int read_and_parse_fifo(struct inv_icm30xxx * s, uint8_t fifo_idx)
{
int rc = 0;
uint16_t count;
uint32_t total_bytes;
uint32_t bytes_read;
struct inv_icm30xxx_fifo_state * fifo;
inv_bool_t reset_wm = false;
assert(fifo_idx < INV_ICM30XXX_FIFO_ID_MAX);
fifo = &s->fifo_states[fifo_idx];
if((rc = inv_icm30xxx_lp_enable(s, false)) != 0)
return rc;
/* retrieve current hw fifo count */
if((rc = inv_icm30xxx_fifo_get_count(s, fifo->fifo_idx, &count)) != 0) {
/* something bad happen, better reset the fifo to
maybe be able to recover later... */
fifo->buffer_len = 0;
INV_MSG(INV_MSG_LEVEL_DEBUG, "inv_icm30xxx_fifo_get_count() = %d", rc);
goto end;
}
/* compute total bytes */
if(s->variant == INV_ICM30XXX_VARIANT_BERLIN) {
/* for Berlin it holds the number of bytes (FPGA issue ?) */
total_bytes = count;
} else {
/* for ICM30630 and 30670, FIFO CNT holds the number of packet */
total_bytes = count*fifo->pkt_size_byte;
}
bytes_read = 0;
/* disable WM if above threshold to avoid multiple-interrupts */
if(total_bytes >= fifo->wm_byte) {
if((rc = inv_icm30xxx_fifo_set_wm(s, fifo_idx, 0xFF)) != 0) {
INV_MSG(INV_MSG_LEVEL_DEBUG, "inv_icm30xxx_fifo_set_wm() = %d", rc);
/* something bad happen, better reset the fifo to
maybe be able to recover later... */
fifo->buffer_len = 0;
goto end;
}
reset_wm = true;
}
while(bytes_read < total_bytes) {
const uint32_t max_len = fifo->buffer_max - fifo->buffer_len;
uint32_t this_len = (total_bytes - bytes_read);
uint8_t * buffer = &fifo->buffer[fifo->buffer_len];
const uint32_t max_rx_size = inv_icm30xxx_serif_max_read(&s->serif);
if(this_len > max_len) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "FIFO miror is full. Reading only %d/%d",
this_len, max_len);
this_len = max_len;
}
if(max_len < FIFOPROTOCOL_PACKET_SIZE) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "FIFO miror remaining size below < PACKET_SIZE."
" Something bad happen. Resetting buffer.");
}
/* ensure we only read complete packet */
if(this_len >= max_rx_size) {
this_len = max_rx_size - (max_rx_size % FIFOPROTOCOL_PACKET_SIZE);
}
if((rc = inv_icm30xxx_fifo_read(s, fifo->fifo_idx, buffer, this_len)) != 0) {
INV_MSG(INV_MSG_LEVEL_DEBUG, "Error (%d) while reading HW FIFO. Resetting buffer.", rc);
/* something bad happen, better reset the fifo to
maybe be able to recover later... */
fifo->buffer_len = 0;
goto end;
}
fifo->buffer_len += this_len;
rc = parse_fifo(s, fifo->buffer, fifo->buffer_len);
if(rc < 0) {
/* something bad happen, reset the fifo to maybe be able to recover later... */
fifo->buffer_len = 0;
goto end;
}
else if(rc > 0) {
/* incomplete packet, move bytes at the top of the buffer */
memmove(fifo->buffer, buffer + this_len - rc, rc);
fifo->buffer_len = rc;
rc = 0;
} else {
/* no remaining byte, no incomplete packet, reset buffer */
fifo->buffer_len = 0;
}
bytes_read += this_len;
}
/* reset back WM value */
if(reset_wm) {
if((rc = inv_icm30xxx_fifo_set_wm(s, fifo_idx, fifo->wm)) != 0){
/* something bad happen, better reset the fifo to
maybe be able to recover later... */
fifo->buffer_len = 0;
INV_MSG(INV_MSG_LEVEL_DEBUG, "inv_icm30xxx_fifo_set_wm() = %d", rc);
goto end;
}
}
end:
inv_icm30xxx_lp_enable(s, true);
return rc;
}
static int parse_fifo(struct inv_icm30xxx * s, const uint8_t * buffer, uint32_t len)
{
int rc = 0;
struct FifoProtocolPacket packet;
uint32_t index = 0;
int32_t remaining = (int32_t)len;
inv_fifo_protocol_parse_packet_reset(&packet);
packet.type = FIFOPROTOCOL_PACKET_TYPE_DATA;
while(remaining >= FIFOPROTOCOL_PACKET_SIZE) {
/* check header / footer */
if(buffer[index] != FIFOPROTOCOL_PROTECTED_HEADER
|| buffer[index+FIFOPROTOCOL_PACKET_SIZE-1] != FIFOPROTOCOL_PROTECTED_FOOTER) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Bad header/footer in fifo packet, dropping one byte...");
index += 1;
remaining -= 1;
continue;
}
/* header/footer ok - parse packet */
rc = inv_fifo_protocol_parse_packet_r(&packet, &buffer[index],
FIFOPROTOCOL_PACKET_SIZE, s->fifop_exp_pkt_cnt);
/* retry if parsing error with last packet */
if(rc == FIFOPROTOCOL_ERROR_PARSE) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Error parsing fifo packet. One packet lost."
" Retrying with current packet...");
rc = inv_fifo_protocol_parse_packet_r(&packet, &buffer[index],
FIFOPROTOCOL_PACKET_SIZE, s->fifop_exp_pkt_cnt);
}
/* invalid packet, skip it */
if(rc == FIFOPROTOCOL_ERROR_SIZE || rc == FIFOPROTOCOL_ERROR_PARSE) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Error parsing fifo packet. Dropping full packet...");
}
/* valid packet, call handlers to notify upper layer */
else if(rc == FIFOPROTOCOL_ERROR_NO) {
if(packet.type == FIFOPROTOCOL_PACKET_TYPE_ANSWER) {
response_handler(s, packet.sensorid,
(FifoProtocolCmd)packet.u.command.cmd,
packet.arg, packet.size);
} else {
data_handler(s, packet.sensorid,
packet.u.data.timestamp, packet.u.data.status,
packet.u.data.accuracy, packet.arg, packet.size);
}
}
else {
/* incomplete packet */
}
/* parse next packet */
index += FIFOPROTOCOL_PACKET_SIZE;
remaining -= FIFOPROTOCOL_PACKET_SIZE;
}
/* If we did not receive all packets needed for current sensor id to be fully parsed, then make
sure the already analyzed packet(s) will be part of next call to this function */
if (rc == FIFOPROTOCOL_ERROR_INCOMPLETE) {
// INV_MSG(INV_MSG_LEVEL_DEBUG, "Incomplete fifo packet, rewind fifo index.");
remaining += packet.states.packet_cnt * FIFOPROTOCOL_PACKET_SIZE;
}
/* return number of remaining bytes from input buffer */
return remaining;
}
static void response_handler(struct inv_icm30xxx * s, uint8_t sensorid,
uint8_t command, const uint8_t * data, uint16_t size)
{
INV_MSG(INV_MSG_LEVEL_DEBUG, "Receive response (id=%d, command=%d, payload=%d)",
sensorid, command, size);
s->response.sensorid = sensorid;
s->response.command = command;
switch(command) {
case FIFOPROTOCOL_CMD_GETFIRMWAREINFO:
inv_fifo_protocol_helper_decode_firmware(data, size,
&s->response.payload.fwversion.major,
&s->response.payload.fwversion.minor,
&s->response.payload.fwversion.patch,
(uint8_t *)s->response.payload.fwversion.hash,
&s->response.payload.fwversion.crc);
break;
case FIFOPROTOCOL_CMD_LOAD:
s->response.payload.load.who = data[0];
s->response.payload.load.what = data[1];
s->response.payload.load.arg = data[2];
s->response.payload.load.arg |= data[3] << 8;
s->response.payload.load.arg |= data[4] << 16;
s->response.payload.load.arg |= data[5] << 24;
break;
case FIFOPROTOCOL_CMD_GETCALIBRATIONOFFSETS:
inv_fifo_protocol_helper_decode_calibrationoffsets(data, size,
s->response.payload.bias);
break;
case FIFOPROTOCOL_CMD_GETCALIBRATIONGAINS:
inv_fifo_protocol_helper_decode_calibrationgains(data, size,
s->response.payload.gain);
break;
case FIFOPROTOCOL_CMD_PING:
s->response.payload.ping = (data[0] != 0);
break;
case FIFOPROTOCOL_CMD_GETCLOCKRATE:
s->response.payload.clock_rate =
inv_fifo_protocol_helper_decode_clockrate(data, size);
break;
case FIFOPROTOCOL_CMD_GETCONFIG:
if(size > sizeof(s->response.payload.config_data)) {
size = sizeof(s->response.payload.config_data);
}
memcpy(s->response.payload.config_data, data, size);
break;
default:
/* unexpected command id */
INV_MSG(INV_MSG_LEVEL_WARNING, "Unexpected response data frame "
"received for command id %d. Ignored.", command);
return;
}
/* signal the response */
s->response.event = true;
}
