Fix 16 bit mode for heart rate measurement

At the same time the code has been streamlined to enhace its visual
clarity (the section is directly used in the documentation)

Change-Id: I9a126c6f7b412fc0918d45d308540651cb105f0e
Reviewed-by: Nedim Hadzic <nedimhadzija@gmail.com>
Reviewed-by: Alex Blasche <alexander.blasche@digia.com>