Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Restart IMU initialization for a maximum of 5 times #90

Merged
merged 5 commits into from
Aug 23, 2023
Merged

Restart IMU initialization for a maximum of 5 times #90

Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8d43317
Restart IMU initialization for a maximum of 5 times
supertom01 Jul 12, 2023
b0d98df
Forgot to insert a bracket
supertom01 Jul 12, 2023
1378a11
Check for status word and wait for MTi initialization in init() loop
supertom01 Aug 18, 2023
1c56aee
Add some documentation
supertom01 Aug 18, 2023
79ac205
Refactor constant for maximum MTi init attempts
supertom01 Aug 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 28 additions & 6 deletions Core/Src/robot.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,9 @@
uint8_t ROBOT_ID;
WIRELESS_CHANNEL ROBOT_CHANNEL;

/* How often should the IMU try to calibrate before the robot gives up? */
uint16_t MTi_max_init_attempts = 5;
supertom01 marked this conversation as resolved.
Show resolved Hide resolved

/* Whether the robot should accept an uart connection from the PC */
volatile bool ENABLE_UART_PC = true;

Expand Down Expand Up @@ -410,15 +413,34 @@ void init(void){
set_Pin(LED3_pin, 1);

{ // ====== INITIALIZE IMU (XSENS). 1 second calibration time, XFP_VRU_general = no magnetometer */
LOG("[init:"STRINGIZE(__LINE__)"] Initializing XSens\n");
MTi = MTi_Init(1, XFP_VRU_general);
if(MTi == NULL){
LOG("[init:"STRINGIZE(__LINE__)"] Failed to initialize XSens\n");
LOG("[init:"STRINGIZE(__LINE__)"] Initializing MTi\n");
MTi = NULL;
uint16_t MTi_made_init_attempts = 0;

// Check whether the MTi is already intialized.
// If the 3rd and 4th bit of the statusword are non-zero, then the initializion hasn't completed yet.
while ((MTi == NULL || (MTi->statusword & (0x18)) != 0) && MTi_made_init_attempts < MTi_max_init_attempts) {
MTi = MTi_Init(1, XFP_VRU_general);
IWDG_Refresh(iwdg);

if (MTi_made_init_attempts > 0) {
LOG_printf("[init:"STRINGIZE(__LINE__)"] Failed to initialize MTi in attempt %d out of %d\n", MTi_made_init_attempts, MTi_max_init_attempts);
}
MTi_made_init_attempts += 1;
LOG_sendAll();

// The MTi is allowed to take 1 second per attempt. Hence we wait a bit more and then check again whether the initialization succeeded.
HAL_Delay(1100);
}

// If after the maximum number of attempts the calibration still failed, play a warning sound... :(
if (MTi == NULL || (MTi->statusword & (0x18)) != 0) {
LOG_printf("[init:"STRINGIZE(__LINE__)"] Failed to initialize MTi after %d out of %d attempts\n", MTi_made_init_attempts, MTi_max_init_attempts);
buzzer_Play_WarningOne();
HAL_Delay(1500); // The duration of the sound
}
LOG_sendAll();
}
}

set_Pin(LED4_pin, 1);

Expand Down Expand Up @@ -470,6 +492,7 @@ void init(void){




/* =================================================== */
/* ==================== MAIN LOOP ==================== */
/* =================================================== */
Expand Down Expand Up @@ -523,7 +546,6 @@ void loop(void){
if (xsens_CalibrationDoneFirst && xsens_CalibrationDone) {
xsens_CalibrationDoneFirst = false;
wheels_Unbrake();
LOG_printf("[loop:"STRINGIZE(__LINE__)"] XSens calibrated after %dms\n", current_time-timestamp_initialized);
}

// Update test (if active)
Expand Down
Loading