dronekit / dronekit-python

DroneKit-Python library for communicating with Drones via MAVLink.
https://readthedocs.org/projects/dronekit-python/
Apache License 2.0
1.5k stars 1.42k forks source link

Copter not responding to `set_position_target_local_ned` mavlink commands #1205

Closed jjshoots closed 9 months ago

jjshoots commented 9 months ago

I'm running a Python 3.10 system on a Jetson Orin Nano, using dronekit installed from this GH repo and Ardupilot 4.4.1 on a Pixhawk 6C. The following code constructs a mavlink message for set_position_target_local_ned, which I then send simply using self.vehicle.send_mavlink(self.setpoint_msg) once per second. But for some reason, the UAV ignores all the setpoint messages, regardless of what kind of bit mask I use.

I can take off and land autonomously, but for all messages that I send, the drone just hovers in place doing nothing.

Any help will be very appreciated! Thanks in advance.

def update_velocity_setpoint(self, frdy: np.ndarray) -> None:
        """Sets a new velocity setpoint.

        Args:
            frdy (np.ndarray): frdy
        """
        setpoint = self.enu2ned(frdy)

        # check the flight ceiling, downward is positive
        vehicle_height = self.vehicle.location.global_relative_frame.alt
        if vehicle_height > self.flight_ceiling:
            setpoint[2] = min(vehicle_height - self.flight_ceiling, 1.0)
        elif vehicle_height < self.flight_floor:
            setpoint[2] = max(vehicle_height - self.flight_floor, -1.0)

        self.setpoint_msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
            # time_boot_ms (not used)
            0,
            # target system, target component
            0,
            0,
            # frame
            mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED,
            # type_mask, addressed in reversed, and used to indicate which components should be IGNORED
            # bit1:PosX, bit2:PosY, bit3:PosZ, bit4:VelX, bit5:VelY, bit6:VelZ, bit7:AccX, bit8:AccY, bit9:AccZ, bit11:yaw, bit12:yaw rate
            0b010111000111,
            # x, y, z positions (not used)
            0,
            0,
            0,
            # x, y, z velocity in m/s
            setpoint[0],
            setpoint[1],
            setpoint[2],
            # x, y, z acceleration (not used)
            0,
            0,
            0,
            # yaw, yaw_rate (only yaw rate used)
            0,
            setpoint[3],
        )