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

[rotorcraft] attitude_ref: more refactoring

Browse Source 
- shared ref saturation defaults in header file
- runtime adjustable saturation for ref_quat_int
- update python wrappers accordingly
This commit is contained in:
Felix Ruess
2015-10-12 15:35:28 +02:00
parent 572d71cc0d
commit de2d42bf82
11 changed files with 187 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/misc/attitude_reference/c_att_refs/generated/airframe.h
+2 -6
View File
@@ -20,16 +20,12 @@
#define STABILIZATION_ATTITUDE_REF_OMEGA_P 6.981317
#define STABILIZATION_ATTITUDE_REF_ZETA_P 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_ZETA_Q 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R 4.363323125
#define STABILIZATION_ATTITUDE_REF_ZETA_R 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
#endif
sw/misc/attitude_reference/c_att_refs/ref_quat_int.pxd
+14 -2
View File
@@ -4,16 +4,21 @@ from pprz_algebra_int_c cimport Int32Quat, Int32Eulers, Int32Rates
from pprz_algebra_float_c cimport FloatRates
cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h":
struct FloatRefModel:
struct IntRefModel:
FloatRates omega
FloatRates zeta
struct Int32RefSat:
Int32Rates max_rate
Int32Rates max_accel
struct AttRefQuatInt:
Int32Eulers euler
Int32Quat quat
Int32Rates rate
Int32Rates accel
FloatRefModel model
IntRefModel model
Int32RefSat saturation
void attitude_ref_quat_int_init(AttRefQuatInt *ref)
void attitude_ref_quat_int_enter(AttRefQuatInt *ref, int32_t psi)
@@ -21,3 +26,10 @@ cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h":
void attitude_ref_quat_int_set_omega(AttRefQuatInt *ref, FloatRates *omega)
void attitude_ref_quat_int_set_zeta(AttRefQuatInt *ref, FloatRates *zeta)
void attitude_ref_quat_int_set_max_p(AttRefQuatInt *ref, float max_p);
void attitude_ref_quat_int_set_max_q(AttRefQuatInt *ref, float max_q);
void attitude_ref_quat_int_set_max_r(AttRefQuatInt *ref, float max_r);
void attitude_ref_quat_int_set_max_pdot(AttRefQuatInt *ref, float max_pdot);
void attitude_ref_quat_int_set_max_qdot(AttRefQuatInt *ref, float max_qdot);
void attitude_ref_quat_int_set_max_rdot(AttRefQuatInt *ref, float max_rdot);
sw/misc/attitude_reference/c_att_refs/ref_quat_int.pyx
+22
View File
@@ -97,3 +97,25 @@ cdef class RefQuatInt:
self.set_omega(omega * np.ones(3))
else:
self.set_omega(omega)
property sat_vel:
def __get__(self):
max_rate = [self.ref.saturation.max_rate.p, self.ref.saturation.max_rate.q, self.ref.saturation.max_rate.r]
return np.array(max_rate, dtype='d') / (1 << REF_RATE_FRAC)
def __set__(self, vel):
if type(vel) == float:
vel = vel * np.ones(3)
ref_quat_int.attitude_ref_quat_int_set_max_p(&self.ref, vel[0])
ref_quat_int.attitude_ref_quat_int_set_max_q(&self.ref, vel[1])
ref_quat_int.attitude_ref_quat_int_set_max_r(&self.ref, vel[2])
property sat_accel:
def __get__(self):
max_accel = [self.ref.saturation.max_accel.p, self.ref.saturation.max_accel.q, self.ref.saturation.max_accel.r]
return np.array(max_accel, dtype='d') / (1 << REF_ACCEL_FRAC)
def __set__(self, accel):
if type(accel) == float:
accel = accel * np.ones(3)
ref_quat_int.attitude_ref_quat_int_set_max_pdot(&self.ref, accel[0])
ref_quat_int.attitude_ref_quat_int_set_max_qdot(&self.ref, accel[1])
ref_quat_int.attitude_ref_quat_int_set_max_rdot(&self.ref, accel[2])
sw/misc/attitude_reference/test_att_ref.py
-1
View File
@@ -87,7 +87,6 @@ sp[:, 0] = pu.rad_of_deg(45.) * scipy.signal.square(math.pi / 2 * time + math.pi
args = {'omega': 10., 'xi': 0.7, 'sat_vel': pu.rad_of_deg(150.), 'sat_accel': pu.rad_of_deg(1800),
'sat_jerk': pu.rad_of_deg(27000)}
rs = [ctl.att_ref_sat_naive(**args), ctl.att_ref_sat_nested(**args), ctl.att_ref_sat_nested2(**args)]
# beware that the saturation parameters of IntNative can only be set/defined at compile time!!
# rs.append(ctl.AttRefIntNative(**args))
rs.append(ctl.AttRefFloatNative(**args))