Added Update Sensor Function

Writes the value of angle in to the NavData struct heading field.