+ if (last_man_update_count != I2C_TIMEOUT_DISABLE) last_man_update_count++;
+
+ if (last_man_update_count > 100) {
+ // ~1s without a new i2c command
+ cmd_vel.speed = 0;
+ cmd_vel.angle = 0;
+ cmd_vel.bUpdate = 1;
+ last_man_update_count = I2C_TIMEOUT_DISABLE;
+ }