lora_radio/lora_node.py at master · Simba-Avionic/lora_radio · 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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
# simple ROS2 node to test my definied msg from "lora_ros_msgs" package
import os
import sys
from lora_ros_msgs.msg import GpsShort, LoraStatus, LoraGnss, MaxAltitude

# import threads
import threading

# import mavlink (simba_mavlink dialect)
from pymavlink import mavutil

CURRENT_DIR = os.path.dirname(os.path.abspath(__file__))
PROJECT_ROOT = os.path.abspath(os.path.join(CURRENT_DIR, ".."))
sys.path.append(PROJECT_ROOT)
sys.path.append(os.path.join(PROJECT_ROOT, "mavlink"))

try:
    import mavlink.src.simba as simba_dialect  # Generated dialect
except:
    print("Failed to import simba dialect.")
mavutil.mavlink = simba_dialect

import rclpy
from rclpy.node import Node

# GPS message - short form
class LoRaNode(Node):

    def __init__(self):
        super().__init__('lora_node')
        self.publisher_lora_gps = self.create_publisher(GpsShort, '/rocket/lora/gps', 10)
        self.publisher_rocket_gps = self.create_publisher(GpsShort, '/rocket/gps', 10)
        self.publisher_recovery_gps = self.create_publisher(GpsShort, '/recovery/gps', 10)
        self.publisher_lora_status = self.create_publisher(LoraStatus, '/mission_control/lora/status', 10)
        self.publisher_lora_extra_status = self.create_publisher(LoraStatus, '/mission_control/lora/extra_status', 10)
        self.publisher_lora_gnss = self.create_publisher(LoraGnss, '/mission_control/lora/gnss', 10)
        self.publisher_max_alt = self.create_publisher(MaxAltitude, 'mavlink/simba_max_altitude', 10)
        self.mavlink_connection = mavutil.mavlink_connection('/dev/ttyACM0', baud=57600)

        self.r_thread = threading.Thread(target=self.reading_thread)
        self.working = True
        self.r_thread.start()

        self.h_thread = threading.Thread(target=self.send_heartbeat)
        self.h_thread.start()

    def send_heartbeat(self):
        while self.working:
            input("Press Enter to send simba gps...")
            # send a shortgps msg to rocket
            self.mavlink_connection.mav.srcComponent = 200
            self.mavlink_connection.mav.simba_gps_send(
                int(52.1234567 * 1e7),   # lat
                int(21.1234567 * 1e7),  # lon
                int(123.456 * 1e3)      # altitude mm
            )

    def reading_thread(self):
        while self.working:
            msg = self.mavlink_connection.recv_match(blocking=True)
            if msg is not None:
                if msg.get_type() == 'L76K_GPS':
                    ros_msg = LoraGnss()
                    ros_msg.header.stamp = self.get_clock().now().to_msg()
                    ros_msg.header.frame_id = '/mission_control/lora'
                    ros_msg.date = msg.date
                    ros_msg.time = msg.time
                    ros_msg.latitude_deg = msg.lat / 1e7
                    ros_msg.longitude_deg = msg.lon / 1e7
                    ros_msg.altitude_m = msg.alt / 1e3
                    ros_msg.speed_mps = msg.speed
                    ros_msg.course_deg = msg.course
                    ros_msg.satellites_visible = msg.satellites_visible
                    ros_msg.hdop = msg.hdop / 1e2
                    ros_msg.sentences_with_fix = msg.sentences_with_fix
                    self.publisher_lora_gnss.publish(ros_msg)
                    # self.get_logger().info(f'Published LoRa GNSS: {ros_msg}')
                elif msg.get_type() == 'RADIO_STATUS':
                    ros_msg = LoraStatus()
                    ros_msg.header.stamp = self.get_clock().now().to_msg()
                    ros_msg.rssi = float(msg.rssi) - 200
                    ros_msg.noise = float(msg.noise) - 200
                    ros_msg.snr = float(msg.rssi) - float(msg.noise)
                    ros_msg.tx_buff = msg.txbuf
                    ros_msg.rem_rssi = float(msg.remrssi) - 200
                    ros_msg.rem_noise = float(msg.remnoise) - 200
                    ros_msg.rem_snr = float(msg.remrssi) - float(msg.remnoise)
                    ros_msg.rx_errors = msg.rxerrors
                    ros_msg.rx_fixed = msg.fixed

                    ros_msg.header.frame_id = '/mission_control/lora'

                    if msg.get_srcComponent() == 222:
                        self.publisher_lora_extra_status.publish(ros_msg)
                    else:
                        self.publisher_lora_status.publish(ros_msg)
                    # self.get_logger().info(f'Published LoRa Status: {ros_msg}')
                elif msg.get_type() == 'SIMBA_GPS':
                    ros_msg = GpsShort()
                    ros_msg.header.stamp = self.get_clock().now().to_msg()
                    ros_msg.latitude_deg = msg.lat / 1e7
                    ros_msg.longitude_deg = msg.lon / 1e7
                    ros_msg.altitude_m = msg.altitude / 1e3

                    if msg.get_srcComponent() == 221: # GPS from lora
                        ros_msg.header.frame_id = '/rocket/lora'
                        self.publisher_lora_gps.publish(ros_msg)
                    elif msg.get_srcComponent() == 200: # GPS from rocket GNSS
                        ros_msg.header.frame_id = '/rocket/gps'
                        self.publisher_rocket_gps.publish(ros_msg)
                    elif msg.get_srcComponent() == 222:
                        ros_msg.header.frame_id = '/recovery/gps'
                        self.publisher_recovery_gps.publish(ros_msg)


                elif msg.get_type() == 'SIMBA_MAX_ALTITUDE':
                    ros_msg = MaxAltitude()
                    ros_msg.header.stamp = self.get_clock().now().to_msg()
                    ros_msg.altitude = msg.altitude
                    ros_msg.header.frame_id = 'mavlink/max_altitude'
                    self.publisher_max_alt.publish(ros_msg)

                    # self.get_logger().info(f'Published GPS: {ros_msg}')


    def destroy_node(self):
        self.get_logger().info('Destroying GPS Short Publisher')
        self.working = False
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)

    lora_node = LoRaNode()
    rclpy.spin(lora_node)
    lora_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()