Add finalizeTri function

Function inserts into trilateration table, and then returns tri_id value
Update of the three used packets in sensor_packets table not yet integrated