25 lines
785 B
GDScript
25 lines
785 B
GDScript
|
extends BaseSensorNode
|
||
|
|
class_name MagnetSensorNode
|
||
|
|
|
||
|
|
|
||
|
|
var heading_rad: float = 0
|
||
|
|
|
||
|
|
|
||
|
|
# Called when the node enters the scene tree for the first time.
|
||
|
|
func _ready() -> void:
|
||
|
|
super._ready()
|
||
|
|
DataSignals.magnetometer_data_received.connect(_on_data_received)
|
||
|
|
|
||
|
|
func _on_data_received(source_ip: String, timestamp_list: Array, mag_vec: Array):
|
||
|
|
if source_ip != ip_address:
|
||
|
|
return
|
||
|
|
var msg_timestamp = timestamp_list[0] * 3600 + timestamp_list[1] * 60 + timestamp_list[2] + timestamp_list[3]
|
||
|
|
if not Constants.is_timestamp_newer(timestamp, msg_timestamp):
|
||
|
|
return
|
||
|
|
timestamp = msg_timestamp
|
||
|
|
var mag = Vector2(mag_vec[0], mag_vec[1])
|
||
|
|
mag.normalized()
|
||
|
|
heading_rad = atan2(-mag[1], mag[0])
|
||
|
|
Signals.sensor_mag_updated.emit(ip_address)
|
||
|
|
Signals.heading_updated.emit(ip_address, heading_rad)
|