Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 19:17:28 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[rotorcraft] ref_euler_int: remove obsolete include

Browse Source
This commit is contained in:
Felix Ruess
2015-10-12 18:14:47 +02:00
parent de2d42bf82
commit 14e4d182be
2 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
-1
View File
@@ -25,7 +25,6 @@
*/ */
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h"
#include "generated/airframe.h" #include "generated/airframe.h"
sw/misc/attitude_reference/control.py
+8 -3
View File
@@ -122,8 +122,7 @@ class att_ref_sat_naive(att_ref_default):
for p in ['sat_vel', 'sat_accel']: for p in ['sat_vel', 'sat_accel']:
self.ensure_vect(p) self.ensure_vect(p)
def update_quat(self, setpoint, dt): def saturate_naive(self):
super(att_ref_sat_naive, self).update_quat(setpoint, dt)
self.accel = np.clip(self.accel, -self.sat_accel, self.sat_accel) self.accel = np.clip(self.accel, -self.sat_accel, self.sat_accel)
for i in range(0, 3): for i in range(0, 3):
if self.vel[i] >= self.sat_vel[i]: if self.vel[i] >= self.sat_vel[i]:
@@ -134,6 +133,10 @@ class att_ref_sat_naive(att_ref_default):
self.vel[i] = -self.sat_vel[i] self.vel[i] = -self.sat_vel[i]
if self.accel[i] < 0: if self.accel[i] < 0:
self.accel[i] = 0 self.accel[i] = 0
def update_quat(self, setpoint, dt):
super(att_ref_sat_naive, self).update_quat(setpoint, dt)
self.saturate_naive()
return self.quat, self.vel, self.accel return self.quat, self.vel, self.accel
@@ -164,9 +167,11 @@ class att_ref_sat_nested(att_ref_sat_naive):
# self.K = pc.butterworth(2, self.omega[0]) # self.K = pc.butterworth(2, self.omega[0])
omega, xi = self.omega[0], self.xi[0] omega, xi = self.omega[0], self.xi[0]
omega_d = omega * math.sqrt(1 - xi ** 2) omega_d = omega * math.sqrt(1 - xi ** 2)
LOG.debug('omega_d: {}'.format(omega_d))
LOG.debug('-omega*xi: {}'.format(-omega * xi))
coefs = -np.poly([complex(-omega * xi, omega_d), complex(-omega * xi, -omega_d)])[::-1] coefs = -np.poly([complex(-omega * xi, omega_d), complex(-omega * xi, -omega_d)])[::-1]
self.K = np.real(coefs[:-1]) self.K = np.real(coefs[:-1])
LOG.debug('sat_nested.__init__ omega: {} K:{} poles:{} '.format(self.omega[0], self.K, pc.poles(self.K))) LOG.debug('sat_nested.__init__ omega: {} coefs:{} K:{} poles:{} '.format(self.omega[0], coefs, self.K, pc.poles(self.K)))
self.sats = np.array([self.sat_vel, self.sat_accel]) self.sats = np.array([self.sat_vel, self.sat_accel])
LOG.debug(' sats:\n{}'.format(self.sats)) LOG.debug(' sats:\n{}'.format(self.sats))
self.M = np.array(self.sats) self.M = np.array(self.sats)