Fly-Position/hover.py at main · z3niith/Fly-Position · 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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
import time
import threading
import logging

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.high_level_commander import HighLevelCommander
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.log import LogConfig

# Global state variables
current_x = 0.0
current_y = 0.0
current_z = 0.0

#--------------------------------------------------------------------
# PID controller class with deadzone and output cap
#--------------------------------------------------------------------
class PIDController:
    def __init__(self, kp, ki, kd, setpoint=0.0, deadzone=0.02, output_limit=0.03):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.setpoint = setpoint
        self.integral = 0.0
        self.last_error = 0.0
        self.deadzone = deadzone
        self.output_limit = output_limit

    def update(self, measurement, dt):
        error = self.setpoint - measurement

        if abs(error) < self.deadzone:
            return 0.0

        self.integral += error * dt
        derivative = (error - self.last_error) / dt if dt > 0 else 0.0
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.last_error = error

        return max(min(output, self.output_limit), -self.output_limit)


def log_callback(timestamp, data, logconf):
    global current_x, current_y, current_z
    current_x = data.get('stateEstimate.x', current_x)
    current_y = data.get('stateEstimate.y', current_y)
    current_z = data.get('stateEstimate.z', current_z)

def log_thread_func(scf, log_conf):
    with SyncLogger(scf, log_conf) as logger:
        for log_entry in logger:
            timestamp, data, logconf = log_entry
            log_callback(timestamp, data, logconf)

#--------------------------------------------------------------------
def main():
    logging.basicConfig(level=logging.INFO)
    cflib.crtp.init_drivers()
    URI = 'radio://0/80/2M'

    target_altitude = 1.0
    takeoff_duration = 2.0
    hover_duration = 6.0
    go_to_duration = 0.2

    # Tuned PID parameters for stability
    pid_x = PIDController(kp=0.05, ki=0.002, kd=0.02)
    pid_y = PIDController(kp=0.05, ki=0.002, kd=0.02)

    with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
        log_conf = LogConfig(name='StateEstimate', period_in_ms=50)
        log_conf.add_variable('stateEstimate.x', 'float')
        log_conf.add_variable('stateEstimate.y', 'float')
        log_conf.add_variable('stateEstimate.z', 'float')
        log_thread = threading.Thread(target=log_thread_func, args=(scf, log_conf))
        log_thread.daemon = True
        log_thread.start()

        commander = HighLevelCommander(scf.cf)

        print(f"Taking off to {target_altitude} m...")
        commander.takeoff(target_altitude, takeoff_duration)
        time.sleep(takeoff_duration + 1.0)

        setpoint_x = current_x
        setpoint_y = current_y
        pid_x.setpoint = setpoint_x
        pid_y.setpoint = setpoint_y
        print(f"Holding position at: x = {setpoint_x:.2f}, y = {setpoint_y:.2f}")

        hover_start = time.time()
        last_time = hover_start
        while time.time() - hover_start < hover_duration:
            now = time.time()
            dt = now - last_time
            last_time = now

            corr_x = pid_x.update(current_x, dt)
            corr_y = pid_y.update(current_y, dt)
            new_x = setpoint_x + corr_x
            new_y = setpoint_y + corr_y

            commander.go_to(new_x, new_y, target_altitude, yaw=0.0, duration_s=go_to_duration, relative=False)

            print(f"[{time.time()-hover_start:4.1f}s] Pos: x={current_x:.2f}, y={current_y:.2f} | "
                  f"Target: x={new_x:.2f}, y={new_y:.2f} | PID: dx={corr_x:.3f}, dy={corr_y:.3f}")

            time.sleep(0.05)

        print("Hover complete. Landing...")
        commander.land(0.0, 2.0)
        time.sleep(3.0)
        print("Landed.")

if __name__ == '__main__':
    main()