fix hardware safety swith param; add satellite # and fix type to debug

misko · Mar 9, 2024 · 28432fd · 28432fd
1 parent 6f332c5
commit 28432fd
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 2 deletions.
diff --git a/data_collection_model_and_results/rover/rover_v3.1/rover3_base_parameters.params b/data_collection_model_and_results/rover/rover_v3.1/rover3_base_parameters.params
@@ -1,7 +1,7 @@
 AHRS_EKF_TYPE    3
 AHRS_GPS_USE     1
 AHRS_ORIENTATION 0
-BRD_SAFETYENABLE 0
+BRD_SAFETY_DEFLT 0
 CRUISE_SPEED     3.000000
 CRUISE_THROTTLE  70
 FS_THR_ENABLE    0

diff --git a/spf/mavlink/mavlink_controller.py b/spf/mavlink/mavlink_controller.py
@@ -38,6 +38,18 @@
 }
 EKF_STATUS_STRING_TO_DEC = {v: k for k, v in EKF_STATUS_DEC_TO_STRING.items()}
 
+gps_fix_type = {
+    0: "GPS_FIX_TYPE_NO_GPS",
+    1: "GPS_FIX_TYPE_NO_FIX",
+    2: "GPS_FIX_TYPE_2D_FIX",
+    3: "GPS_FIX_TYPE_3D_FIX",
+    4: "GPS_FIX_TYPE_DGPS",
+    5: "GPS_FIX_TYPE_RTK_FLOAT",
+    6: "GPS_FIX_TYPE_RTK_FIXED",
+    7: "GPS_FIX_TYPE_STATIC",
+    8: "GPS_FIX_TYPE_PPP",
+}
+
 mav_states_list = [
     "MAV_STATE_UNINIT",
     "MAV_STATE_BOOT",
@@ -239,6 +251,9 @@ def __init__(
         self.planner_started_moving = False
         self.last_heartbeat_log = None
         self.armed = False
+
+        self.gps_satellites = -1
+        self.gps_fix_type = "NOT_SET_YET"
         # self.mission_item_condition = threading.Condition()
         # self.mission_item_reached = False
 
@@ -619,6 +634,10 @@ def handle_GLOBAL_POSITION_INT(self, msg):
         self.gps = np.array([self.long, self.lat])
         self.heading = msg.hdg / 100
 
+    def handle_GPS_RAW_INT(self, msg):
+        self.gps_satellites = msg.satellites_visible
+        self.gps_fix_type = gps_fix_type[msg.fix_type]
+
     def handle_EKF_STATUS_REPORT(self, msg):
         if msg.flags & self.healthy_ekf_flag == self.healthy_ekf_flag:
             self.ekf_healthy = True
@@ -653,7 +672,8 @@ def handle_HEARTBEAT(self, msg, log_interval=5):
                 or time.time() - self.last_heartbeat_log > log_interval
             ):
                 logging.info(
-                    f"HEARTBEAT STATUS: mav_state:{mav_state_check}, gps:{gps_check},"
+                    f"HEARTBEAT STATUS: mav_state:{mav_state_check}"
+                    + f"gps:{gps_check}({self.gps_satellites}sats,{self.gps_fix_type}),"
                     + f"gps_healthy:{gps_healthy}, guided_mode:{guided_mode}, ekf:{self.ekf_healthy}"
                 )
                 self.last_heartbeat_log = time.time()