From 913a8a03d61ff497d6978fc8b9647f45fa490023 Mon Sep 17 00:00:00 2001 From: Kiran Shila Date: Fri, 22 Nov 2019 11:20:06 -0500 Subject: [PATCH 1/5] Updated heartbeat on_message to ignore gimbal heartbeats --- dronekit/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 4f3613906..327ee671c 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1209,7 +1209,7 @@ def listener(self, name, m): @self.on_message('HEARTBEAT') def listener(self, name, m): # ignore groundstations - if m.type == mavutil.mavlink.MAV_TYPE_GCS: + if m.type == mavutil.mavlink.MAV_TYPE_GCS or m.type == mavutil.mavlink.MAV_TYPE_GIMBAL: return self._armed = (m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 self.notify_attribute_listeners('armed', self.armed, cache=True) From b64ddcf61b56f5b58dbd8bbd7517e14a73a1aef9 Mon Sep 17 00:00:00 2001 From: Kiran Shila Date: Thu, 9 Jan 2020 13:35:55 -0500 Subject: [PATCH 2/5] Don't stabalize orientation, fix gimbal targetting --- dronekit/__init__.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 327ee671c..ce655c7a2 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -2598,9 +2598,9 @@ def rotate(self, pitch, roll, yaw): msg = self._vehicle.message_factory.mount_configure_encode( 0, 1, # target system, target component mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode - 1, # stabilize roll - 1, # stabilize pitch - 1, # stabilize yaw + 0, # don't stabilize roll + 0, # don't stabilize pitch + 0, # don't stabilize yaw ) self._vehicle.send_mavlink(msg) msg = self._vehicle.message_factory.mount_control_encode( @@ -2633,9 +2633,9 @@ def target_location(self, roi): msg = self._vehicle.message_factory.mount_configure_encode( 0, 1, # target system, target component mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, # mount_mode - 1, # stabilize roll - 1, # stabilize pitch - 1, # stabilize yaw + 0, # don't stabilize roll + 0, # don't stabilize pitch + 0, # don't stabilize yaw ) self._vehicle.send_mavlink(msg) @@ -2643,10 +2643,10 @@ def target_location(self, roi): if isinstance(roi, LocationGlobalRelative): alt = roi.alt elif isinstance(roi, LocationGlobal): - if not self.home_location: - self.commands.download() - self.commands.wait_ready() - alt = roi.alt - self.home_location.alt + if not self._vehicle.home_location: + self._vehicle.commands.download() + self._vehicle.commands.wait_ready() + alt = roi.alt - self._vehicle.home_location.alt else: raise ValueError('Expecting location to be LocationGlobal or LocationGlobalRelative.') From 10b7fa71c7f4498d6ced80a85ecb318e40e7a153 Mon Sep 17 00:00:00 2001 From: Kiran Shila Date: Tue, 21 Jan 2020 09:56:07 -0500 Subject: [PATCH 3/5] Testing removing the home location altitude in gimbal point --- dronekit/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index ce655c7a2..4a732c634 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -2646,7 +2646,7 @@ def target_location(self, roi): if not self._vehicle.home_location: self._vehicle.commands.download() self._vehicle.commands.wait_ready() - alt = roi.alt - self._vehicle.home_location.alt + alt = roi.alt #- self._vehicle.home_location.alt else: raise ValueError('Expecting location to be LocationGlobal or LocationGlobalRelative.') From 36c1c4cf29632ff0dc1cc414b7e4f1b5a7cef482 Mon Sep 17 00:00:00 2001 From: Kiran Shila Date: Thu, 30 Jan 2020 14:02:19 -0500 Subject: [PATCH 4/5] Guard against no GPS fix in target_location() --- dronekit/__init__.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 4a732c634..50de8f821 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -2646,7 +2646,12 @@ def target_location(self, roi): if not self._vehicle.home_location: self._vehicle.commands.download() self._vehicle.commands.wait_ready() - alt = roi.alt #- self._vehicle.home_location.alt + # This might make sense - as what should we make alt if the vehicle + # doesn't have a GPS fix? + if self._vehicle.home_location is not None: + alt = roi.alt - self._vehicle.home_location.alt + else: + alt = roi.alt else: raise ValueError('Expecting location to be LocationGlobal or LocationGlobalRelative.') From b817af81f6785ec5f8ba5e5555dec171777288d2 Mon Sep 17 00:00:00 2001 From: Kiran Shila Date: Thu, 13 Feb 2020 12:37:22 -0500 Subject: [PATCH 5/5] Weird scoping nonsense --- dronekit/__init__.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 50de8f821..2434c2cdd 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -2640,18 +2640,18 @@ def target_location(self, roi): self._vehicle.send_mavlink(msg) # Get altitude relative to home irrespective of Location object passed in. + # Global boi because python scoping rules are silly + alt = 0 if isinstance(roi, LocationGlobalRelative): alt = roi.alt elif isinstance(roi, LocationGlobal): - if not self._vehicle.home_location: + if self._vehicle.home_location is not None: self._vehicle.commands.download() self._vehicle.commands.wait_ready() - # This might make sense - as what should we make alt if the vehicle - # doesn't have a GPS fix? - if self._vehicle.home_location is not None: - alt = roi.alt - self._vehicle.home_location.alt - else: - alt = roi.alt + alt = roi.alt - self._vehicle.home_location.alt + else: + # Just set to global instead of relative if home is none + alt = roi.alt else: raise ValueError('Expecting location to be LocationGlobal or LocationGlobalRelative.')