Cansat
/
esp32_BNO08x
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

parse_packet refactoring

This commit is contained in:
myles-parfeniuk 2024-11-15 18:06:22 -08:00
parent 93d8c837d2
commit 58639579ed
2 changed files with 176 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.clang-format
View File

@ -13,7 +13,7 @@ BreakConstructorInitializersBeforeComma: true
ConstructorInitializerAllOnOneLineOrOnePerLine: false ConstructorInitializerAllOnOneLineOrOnePerLine: false
AllowShortBlocksOnASingleLine: false AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: None AllowShortFunctionsOnASingleLine: None
AllowShortIfStatementsOnASingleLine: false AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false AllowShortLoopsOnASingleLine: false
@ -33,3 +33,5 @@ BreakBeforeBraces: Allman
IndentAccessModifiers: true IndentAccessModifiers: true
IndentPPDirectives: AfterHash IndentPPDirectives: AfterHash
IndentCaseLabels: true

333
source/BNO08x.cpp
View File

@ -1010,35 +1010,35 @@ void BNO08x::queue_calibrate_command(uint8_t sensor_to_calibrate)
switch (sensor_to_calibrate) switch (sensor_to_calibrate)
{ {
case CALIBRATE_ACCEL: case CALIBRATE_ACCEL:
commands[3] = 1; commands[3] = 1;
break; break;
case CALIBRATE_GYRO: case CALIBRATE_GYRO:
commands[4] = 1; commands[4] = 1;
break; break;
case CALIBRATE_MAG: case CALIBRATE_MAG:
commands[5] = 1; commands[5] = 1;
break; break;
case CALIBRATE_PLANAR_ACCEL: case CALIBRATE_PLANAR_ACCEL:
commands[7] = 1; commands[7] = 1;
break; break;
case CALIBRATE_ACCEL_GYRO_MAG: case CALIBRATE_ACCEL_GYRO_MAG:
commands[3] = 1; commands[3] = 1;
commands[4] = 1; commands[4] = 1;
commands[5] = 1; commands[5] = 1;
break; break;
case CALIBRATE_STOP: case CALIBRATE_STOP:
// do nothing, send packet of all 0s // do nothing, send packet of all 0s
break; break;
default: default:
break; break;
} }
calibration_status = 1; calibration_status = 1;
@ -1243,57 +1243,70 @@ uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS #ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGW(TAG, "SHTP Header RX'd: 0x%X 0x%X 0x%X 0x%X", packet->header[0], packet->header[1], packet->header[2], packet->header[3]); ESP_LOGW(TAG, "SHTP Header RX'd: 0x%X 0x%X 0x%X 0x%X", packet->header[0], packet->header[1], packet->header[2], packet->header[3]);
#endif #endif
// clang-format on // clang-format on
switch (packet->body[0])
if (packet->body[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) // check to see that product ID matches what it should
{ {
return parse_product_id_report(packet); case SHTP_REPORT_PRODUCT_ID_RESPONSE:
return parse_product_id_report(packet);
break;
case SHTP_REPORT_GET_FEATURE_RESPONSE:
break;
case SHTP_REPORT_FRS_READ_RESPONSE:
return parse_frs_read_response_report(packet);
break;
default:
break;
} }
/*if(packet->body[0] == SHTP_REPORT_GET_FEATURE_RESPONSE) switch (packet->header[2])
{ {
case CHANNEL_REPORTS:
}*/ if (packet->body[0] == SHTP_REPORT_BASE_TIMESTAMP)
{
if (packet->body[0] == SHTP_REPORT_FRS_READ_RESPONSE) // clang-format off
{ #ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
return parse_frs_read_response_report(packet);
}
// Check to see if this packet is a sensor reporting its data to us
if (packet->header[2] == CHANNEL_REPORTS && packet->body[0] == SHTP_REPORT_BASE_TIMESTAMP)
{
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel report"); ESP_LOGI(TAG, "RX'd packet, channel report");
#endif #endif
// clang-format on // clang-format on
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature // this will update the rawAccelX, etc variables depending on which feature report is found
// report is found return parse_input_report(packet);
} }
break;
if (packet->header[2] == CHANNEL_CONTROL) case CHANNEL_CONTROL:
{ // clang-format off
// clang-format off #ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS ESP_LOGI(TAG, "RX'd packet, channel control");
ESP_LOGI(TAG, "RX'd packet, channel control"); #endif
#endif // clang-format on
// clang-format on
return parse_command_report(packet); // This will update responses to commands, calibrationStatus, etc. // this will update responses to commands, calibrationStatus, etc.
} return parse_command_report(packet);
if (packet->header[2] == CHANNEL_GYRO) break;
{
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, channel gyro");
#endif
// clang-format on
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature case CHANNEL_GYRO:
// report is found // clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, channel gyro");
#endif
// clang-format on
// this will update the rawAccelX, etc variables depending on which feature report is found
return parse_input_report(packet);
break;
default:
break;
} }
return 0; return 0;
@ -1423,122 +1436,122 @@ uint16_t BNO08x::parse_input_report(bno08x_rx_packet_t* packet)
// Store these generic values to their proper global variable // Store these generic values to their proper global variable
switch (packet->body[5]) switch (packet->body[5])
{ {
case SENSOR_REPORT_ID_ACCELEROMETER: case SENSOR_REPORT_ID_ACCELEROMETER:
accel_accuracy = status; accel_accuracy = status;
raw_accel_X = data1; raw_accel_X = data1;
raw_accel_Y = data2; raw_accel_Y = data2;
raw_accel_Z = data3; raw_accel_Z = data3;
break; break;
case SENSOR_REPORT_ID_LINEAR_ACCELERATION: case SENSOR_REPORT_ID_LINEAR_ACCELERATION:
accel_lin_accuracy = status; accel_lin_accuracy = status;
raw_lin_accel_X = data1; raw_lin_accel_X = data1;
raw_lin_accel_Y = data2; raw_lin_accel_Y = data2;
raw_lin_accel_Z = data3; raw_lin_accel_Z = data3;
break; break;
case SENSOR_REPORT_ID_GYROSCOPE: case SENSOR_REPORT_ID_GYROSCOPE:
gyro_accuracy = status; gyro_accuracy = status;
raw_gyro_X = data1; raw_gyro_X = data1;
raw_gyro_Y = data2; raw_gyro_Y = data2;
raw_gyro_Z = data3; raw_gyro_Z = data3;
break; break;
case SENSOR_REPORT_ID_UNCALIBRATED_GYRO: case SENSOR_REPORT_ID_UNCALIBRATED_GYRO:
uncalib_gyro_accuracy = status; uncalib_gyro_accuracy = status;
raw_uncalib_gyro_X = data1; raw_uncalib_gyro_X = data1;
raw_uncalib_gyro_Y = data2; raw_uncalib_gyro_Y = data2;
raw_uncalib_gyro_Z = data3; raw_uncalib_gyro_Z = data3;
raw_bias_X = data4; raw_bias_X = data4;
raw_bias_Y = data5; raw_bias_Y = data5;
raw_bias_Z = data6; raw_bias_Z = data6;
break; break;
case SENSOR_REPORT_ID_MAGNETIC_FIELD: case SENSOR_REPORT_ID_MAGNETIC_FIELD:
magf_accuracy = status; magf_accuracy = status;
raw_magf_X = data1; raw_magf_X = data1;
raw_magf_Y = data2; raw_magf_Y = data2;
raw_magf_Z = data3; raw_magf_Z = data3;
break; break;
case SENSOR_REPORT_ID_TAP_DETECTOR: case SENSOR_REPORT_ID_TAP_DETECTOR:
tap_detector = packet->body[5 + 4]; // Byte 4 only tap_detector = packet->body[5 + 4]; // Byte 4 only
break; break;
case SENSOR_REPORT_ID_STEP_COUNTER: case SENSOR_REPORT_ID_STEP_COUNTER:
step_count = data3; // Bytes 8/9 step_count = data3; // Bytes 8/9
break; break;
case SENSOR_REPORT_ID_STABILITY_CLASSIFIER: case SENSOR_REPORT_ID_STABILITY_CLASSIFIER:
stability_classifier = packet->body[5 + 4]; // Byte 4 only stability_classifier = packet->body[5 + 4]; // Byte 4 only
break; break;
case SENSOR_REPORT_ID_PERSONAL_ACTIVITY_CLASSIFIER: case SENSOR_REPORT_ID_PERSONAL_ACTIVITY_CLASSIFIER:
activity_classifier = packet->body[5 + 5]; // Most likely state activity_classifier = packet->body[5 + 5]; // Most likely state
// Load activity classification confidences into the array // Load activity classification confidences into the array
for (i = 0; i < 9; i++) // Hardcoded to max of 9. TODO - bring in array size for (i = 0; i < 9; i++) // Hardcoded to max of 9. TODO - bring in array size
activity_confidences[i] = packet->body[5 + 6 + i]; // 5 bytes of timestamp, byte 6 is first confidence activity_confidences[i] = packet->body[5 + 6 + i]; // 5 bytes of timestamp, byte 6 is first confidence
// byte // byte
break; break;
case SENSOR_REPORT_ID_RAW_ACCELEROMETER: case SENSOR_REPORT_ID_RAW_ACCELEROMETER:
mems_raw_accel_X = data1; mems_raw_accel_X = data1;
mems_raw_accel_Y = data2; mems_raw_accel_Y = data2;
mems_raw_accel_Z = data3; mems_raw_accel_Z = data3;
break; break;
case SENSOR_REPORT_ID_RAW_GYROSCOPE: case SENSOR_REPORT_ID_RAW_GYROSCOPE:
mems_raw_gyro_X = data1; mems_raw_gyro_X = data1;
mems_raw_gyro_Y = data2; mems_raw_gyro_Y = data2;
mems_raw_gyro_Z = data3; mems_raw_gyro_Z = data3;
break; break;
case SENSOR_REPORT_ID_RAW_MAGNETOMETER: case SENSOR_REPORT_ID_RAW_MAGNETOMETER:
mems_raw_magf_X = data1; mems_raw_magf_X = data1;
mems_raw_magf_Y = data2; mems_raw_magf_Y = data2;
mems_raw_magf_Z = data3; mems_raw_magf_Z = data3;
break; break;
case SHTP_REPORT_COMMAND_RESPONSE: case SHTP_REPORT_COMMAND_RESPONSE:
// The BNO080 responds with this report to command requests. It's up to use to remember which command we // The BNO080 responds with this report to command requests. It's up to use to remember which command we
// issued. // issued.
command = packet->body[5 + 2]; // This is the Command byte of the response command = packet->body[5 + 2]; // This is the Command byte of the response
if (command == COMMAND_ME_CALIBRATE) if (command == COMMAND_ME_CALIBRATE)
calibration_status = packet->body[5 + 5]; // R0 - Status (0 = success, non-zero = fail) calibration_status = packet->body[5 + 5]; // R0 - Status (0 = success, non-zero = fail)
break; break;
case SENSOR_REPORT_ID_GRAVITY: case SENSOR_REPORT_ID_GRAVITY:
gravity_accuracy = status; gravity_accuracy = status;
gravity_X = data1; gravity_X = data1;
gravity_Y = data2; gravity_Y = data2;
gravity_Z = data3; gravity_Z = data3;
break; break;
default: default:
if (packet->body[5] == SENSOR_REPORT_ID_ROTATION_VECTOR || packet->body[5] == SENSOR_REPORT_ID_GAME_ROTATION_VECTOR || if (packet->body[5] == SENSOR_REPORT_ID_ROTATION_VECTOR || packet->body[5] == SENSOR_REPORT_ID_GAME_ROTATION_VECTOR ||
packet->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_ROTATION_VECTOR || packet->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_ROTATION_VECTOR ||
packet->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_GAME_ROTATION_VECTOR) packet->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_GAME_ROTATION_VECTOR)
{ {
quat_accuracy = status; quat_accuracy = status;
raw_quat_I = data1; raw_quat_I = data1;
raw_quat_J = data2; raw_quat_J = data2;
raw_quat_K = data3; raw_quat_K = data3;
raw_quat_real = data4; raw_quat_real = data4;
// Only available on rotation vector and ar/vr stabilized rotation vector, // Only available on rotation vector and ar/vr stabilized rotation vector,
// not game rot vector and not ar/vr stabilized rotation vector // not game rot vector and not ar/vr stabilized rotation vector
raw_quat_radian_accuracy = data5; raw_quat_radian_accuracy = data5;
} }
else else
{ {
// This sensor report ID is unhandled. // This sensor report ID is unhandled.
// See reference manual to add additional feature reports as needed // See reference manual to add additional feature reports as needed
return 0; return 0;
} }
break; break;
} }
// TODO additional feature reports may be strung together. Parse them all. // TODO additional feature reports may be strung together. Parse them all.