Fix - logGetVarId - Return Log ID that matches the Log ID in Logs TOC

[sabulaban]

Current implementation does not return the correct log ID that matches TOC for logGetVarId, this is a fix to address that, within the fix, the return value matches the log ID as defined in the TOC.

[ataffanel]

Thanks for the PR!

This bug surprises me since we have been using this functionality in a lot of our demos and we have not seen any problem with it. I will take a closer look.

[ataffanel]

This PR does not seems to work.

I tested the example/app_internal_param_log and without the PR I get expected output from internal logs (the yaw is a slowly drifting number starting around 0):

Yaw is now: 0.013700 deg
Estimator type is now: 1 deg
ESTIMATOR: Using Kalman (2) estimator
Yaw is now: 0.006027 deg
Estimator type is now: 2 deg
Yaw is now: 0.002112 deg
Estimator type is now: 2 deg
Yaw is now: -0.003149 deg
Estimator type is now: 2 deg

With the PR I do not (this is not showing the yaw log value):

Yaw is now: 0.0 deg
Estimator type is now: 1 deg
ESTIMATOR: Using Kalman (2) estimator
Yaw is now: 0.0 deg
Estimator type is now: 2 deg
Yaw is now: 0.0 deg
Estimator type is now: 2 deg
Yaw is now: 0.0 deg
Estimator type is now: 2 deg
Yaw is now: 0.0 deg

I am closing the PR. If you feel like I missed something do not hesitate to re-open it.