Comms/sampleERU.py at main · NGCP-GCS-2021/Comms · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
import time
from digi.xbee.devices import DigiMeshDevice
import xbee
from xbee import ToERU, ToGCS, Orientation, LatLng, ManualControl, Geofence
import threading
import struct

comm_port = "/dev/ttyAMA0" # can be swapped out for "/dev/ttyUSB0" for serial connection - Might change based off how things are plugged in
baud_rate = "9600"

device = DigiMeshDevice(port=comm_port, baud_rate=baud_rate)
device.open()

print("This device's name: ", device.get_node_id())

telemetry_data = ToGCS(0, 0, Orientation(0,0,0), LatLng(35.082094, -120.512722), 0.98, True, 0, False, LatLng(0,0), 0, False, True)
gcs_lock = threading.Lock()
gcs_addr = None

hiker_pos = LatLng(0,0)
current_pos = LatLng(35.082094, -120.512722)
current_state = 0

packet_buffer = b''
packet_counter = 0

def packet_received_with(packet):
    print('Received packet from ', packet.remote_device.get_node_id())
    global gcs_addr
    global packet_counter
    global packet_buffer
    global current_state
    global hiker_position

    with gcs_lock:
        gcs_addr = packet.remote_device.get_64bit_addr()
    data = None

    if packet_counter is 0:
        packet_counter = struct.unpack("I", packet.data[:4])[0] -1
        data = packet.data[4:]
        print("expecting ", packet_counter," packets")
        packet_buffer = b''
    else:
        packet_counter -= 1
        data = packet.data

    packet_buffer += data

    if packet_counter is 0:
        with xbee.read_lock: # Acquire lock to read command data from GCS  ||  Read data from GCS
            command_data = ToERU.deserialize(packet_buffer)
            if command_data.stop:
                print("STOPPPING AT ", current_pos)     # Emergency Stop
            hiker_pos = command_data.hiker_position     # use command_data.(whatever) to receive whatever data is needed
            current_state = command_data.perform_state

device.add_data_received_callback(packet_received_with)

#Pass data to GCS
def transmit_packet():
    with telemetry_data.lock: # Acquire lock to update telemetry data
        telemetry_data.hiker_positon = hiker_pos
        telemetry_data.gps = current_pos
        telemetry_data.orientation.yaw += 1
        telemetry_data.battery -= 0.0001
        telemetry_data.current_state = current_state
    if gcs_addr:
        telemetry_data.serialize().transmit(device, gcs_addr)
        print("Transmitting")

    time.sleep(5)

transmitThread = xbee.TransmitThread(transmit_packet)

try:
    transmitThread.start()

    while True:
        if current_state is 0:
            print("Awaiting command data")
        else:
            print("Performing normal operations for state ", current_state)
            move_vector = hiker_pos-current_pos
            print("Need to move ", move_vector)
            move_vector.lat /= 1000
            move_vector.lng /= 1000
            current_pos += move_vector

        current_state = not current_state

        time.sleep(1)
except KeyboardInterrupt:
    print('Stopping')
finally:
    device.del_data_received_callback(packet_received_with)