-
Notifications
You must be signed in to change notification settings - Fork 2
/
position_controller.py
122 lines (107 loc) · 4.22 KB
/
position_controller.py
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
119
120
121
122
from models import SensorModel
from pid import PID
import time
class MetaPid():
def __init__(self, **kwargs):
self.timestamp = time.time()
self.P = kwargs.get('P', None)
self.I = kwargs.get('I', None)
self.D = kwargs.get('D', None)
self.maximum_thrust = kwargs.get('maximum_thrust', None)
self.minimum_thrust = kwargs.get('minimum_thrust', None)
class PIDlog():
def __init__(self, **kwargs):
self.timestamp = time.time()
self.correction = kwargs.get('corretion', None)
self.target = kwargs.get('target', None)
self.altitude = kwargs.get('altitude', None)
self.altitude_raw = kwargs.get('altitude_raw', None)
self.thrust = kwargs.get('thrust', None)
class PositionController():
def __init__(self, autopilot, state_estimation, **kwargs):
self.sm = SensorModel()
self.heading = None
self.targets = {}
self.autopilot = autopilot
self.state_estimation = state_estimation
self.maximum_thrust = 1850
self.minimum_thrust = 1350
# self.altitude_pid = kwargs.get('altitude_pid')
p = 25
i = 0.6
d = 0
max_t = 50
min_t = -5
self.altitude_pid = PID(
P=p,
I=i,
D=d,
Derivator=0,
Integrator=0,
Integrator_max=25,
Integrator_min=-25,
maximum_thrust=max_t,
minimum_thrust=min_t,
)
self.heading_pid = PID(
P=1,
I=0,
D=0,
Derivator=0,
Integrator=0,
Integrator_max=25,
Integrator_min=-25,
maximum_thrust=25,
minimum_thrust=-25,
)
def headingHold(self):
if not self.targets.get('heading'):
#self.targets['heading'] = self.state_estimation.getHeading()
self.targets['heading'] = self.autopilot.heading
self.heading_pid.setPoint(self.targets.get('heading'))
thrust_correction = self.altitude_pid.update(self.autopilot.heading)
# thrust_correction = self.althold_pid.constraint(thrust_correction)
self.autopilot.yaw = self.autopilot.yaw + thrust_correction
def holdAltitude(self):
if not self.targets.get('altitude'):
self.targets['altitude'] = self.state_estimation.getAltitude()
self.altitude_pid.setPoint(self.targets.get('altitude'))
self.autopilot.meta_pid = MetaPid(
P=self.altitude_pid.Kp,
I=self.altitude_pid.Ki,
D=self.altitude_pid.Kd,
maximum_thrust=self.altitude_pid.maximum_thrust,
minimum_thrust=self.altitude_pid.minimum_thrust,
)
altitude = self.state_estimation.getAltitude()
thrust_correction = self.altitude_pid.update(altitude)
thrust_correction = self.altitude_pid.constraint(thrust_correction)
thrust = self.autopilot.throttle + thrust_correction
print 'target: %f altitude: %f corretion: %d current: %d new thrust: %d ' % (self.altitude_pid.set_point, self.state_estimation.getAltitude(), thrust_correction, thrust, self.autopilot.throttle)
self.autopilot.throttle = self.constraint(thrust)
self.autopilot.pid_log(
PIDlog(
corretion=thrust_correction,
altitude=altitude,
# altitude_raw=self.autopilot.altitude_barometer,
altitude_raw=self.autopilot.altitude_barometer,
target=self.altitude_pid.set_point,
thrust=self.autopilot.throttle,
error=self.altitude_pid.error,
)
)
# print 'target: %f altitude: %f' % (self.altitude_pid.set_point,
# self.state_estimation.getAltitude())
def reset_targets(self):
self.targets.clear()
def set_target_altitude(self, altitude):
self.targets['altitude'] = altitude
def constraint(self, value):
if value > self.maximum_thrust:
return self.maximum_thrust
elif value < self.minimum_thrust:
return self.minimum_thrust
else:
return int(round(value))
#pc = PositionController(s)
# pc.headingHold()