Maciejm max alt

lora_node.py

@@ -1,14 +1,24 @@
 # simple ROS2 node to test my definied msg from "lora_ros_msgs" package
-
-from lora_ros_msgs.msg import GpsShort, LoraStatus, LoraGnss
+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
-import simba_mavlink
-mavutil.mavlink = simba_mavlink
+
+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
@@ -20,9 +30,11 @@ 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)
@@ -96,6 +108,17 @@ def reading_thread(self):
                     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}')
 

lora_ros_msgs/msg/MaxAltitude.msg

@@ -0,0 +1,4 @@
+# LoRaMaxAltitude message
+# ---
+std_msgs/Header header
+int32 altitude

rocket_rx/rocket_rx.ino

@@ -93,6 +93,22 @@ void sendRadioStatus(float rssi_dbm, float snr_db) {
   Serial.write(buf, len);
 }
 
+void sendSimbaMaxAltitude(uint8_t compid, float max_alt_m) {
+  mavlink_message_t msg;
+  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+  int32_t alt_cm = (int32_t)(max_alt_m * 100.0f);
+
+  mavlink_msg_simba_max_altitude_pack(
+    MAV_SYS_ID, compid, &msg,
+    alt_cm
+  );
+  msg.seq = mavSeq++;
+
+  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+  Serial.write(buf, len);
+}
+
 // ----------------------------------------------------------------
 void setup() {
   Serial.begin(115200);
@@ -165,6 +181,22 @@ void loop() {
   if (pkt.status & 0x02)
     sendSimbaGps(MAV_COMP_MAV_GPS, pkt.mav_lat, pkt.mav_lon, pkt.mav_alt);
 
+  float current_max_alt = -9999.0f; 
+  uint8_t target_compid = MAV_COMP_INT_GPS;
+
+  if (pkt.status & 0x01) {
+    current_max_alt = pkt.int_alt;
+  }
+  if ((pkt.status & 0x02) && (pkt.mav_alt > current_max_alt)) {
+    current_max_alt = pkt.mav_alt;
+    target_compid = MAV_COMP_MAV_GPS;
+  }
+
+  // Jeśli odebraliśmy jakąkolwiek poprawną wysokość, wysyłamy ramkę SIMBA_MAX_ALTITUDE
+  if (pkt.status & 0x03) { 
+    sendSimbaMaxAltitude(target_compid, current_max_alt);
+  }
+
   // Status łącza 
   sendRadioStatus(rssi, snr);
 }