uint16_t i = (p_queue->write_idx)++ & (capacity - 1); => uint16_t i = (p_queue->write_idx)++ % capacity ;

[wsd1]

If capacity not equal to 2\4\8\16\32... there will be some issues here.