test11/MainSystem.py at main · tolgayarkinusta/test11 · 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
from pymavlink import mavutil
import math
import cv2
import numpy
import math
import time


def map_value_to_range(value, in_min=-1, in_max=1, out_min=-2000, out_max=2000):
    return int((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)


class USVController:
    def __init__(self, connection_string):
        # connecting to the vehicle
        print("Heartbeat waiting...")
        self.master = mavutil.mavlink_connection(connection_string)
        self.master.wait_heartbeat()
        print("Heartbeat found!")

        self.first_position = None
        self.current_position = None

    def arm_vehicle(self):
        # arming vehicle
        self.master.mav.command_long_send(
            self.master.target_system,
            self.master.target_component,
            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
            0,
            1, 0, 0, 0, 0, 0, 0
        )
        print("Waiting for the vehicle to arm...")
        self.master.motors_armed_wait()  # Waiting for arming the vehicle
        print('Armed!')

    def disarm_vehicle(self):
        # Disarming the vehicle
        self.master.mav.command_long_send(
            self.master.target_system,
            self.master.target_component,
            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
            0,
            0, 0, 0, 0, 0, 0, 0
        )
        print("Waiting for the vehicle to disarm")
        self.master.motors_disarmed_wait()  # Waiting for disarming the vehicle
        print('Disarmed!')

    def print_motor_outputs(self):
        print(self.master.recv_match(type='SERVO_OUTPUT_RAW', blocking=True))

    def go_to_vehicle_raw(self, x=0, y=0, z=500, r=0, buttons=0, s=0, t=0, see_motor_output=0):
        # Manual controls
        self.master.mav.manual_control_send(
            self.master.target_system,
            x,  # x is straight 2000 is full front -2000 full back
            y,  # y is sideways 2000 is full right -2000 full left
            z,  # z is up 1500 is full up -500 full down (pwm is 1100 up 1900 down all z axis motors)
            r,
            buttons,
            s,
            t
        )
        if see_motor_output:
            self.print_motor_outputs()

    def set_servo(self, servo_pin, pwm_value, see_motor_output=0):
        self.master.mav.command_long_send(
            self.master.target_system,
            self.master.target_component,
            mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
            0,
            servo_pin,
            pwm_value,
            0, 0, 0, 0, 0
        )
        if see_motor_output:
            self.print_motor_outputs()

    def set_mode(self, mode_name):
        # (example: 'STABILIZE', 'MANUAL', 'DEPTH_HOLD')

        mode_id = self.master.mode_mapping()[mode_name]

        self.master.mav.set_mode_send(
            self.master.target_system,
            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
            mode_id
        )
        print("Aracın modu " + mode_name + " olarak değiştirildi.")