That code path for GRIM was already there. ;-) I tried to avoid making any changes to GRIM code and so I just shuffled the lines and moved them into the condition to select between EMI and GRIM.

Please let me know if you want me to change it to:
float roll = deg2rad(-_currSetup->_roll)