Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[INDI] support for servos (#2005)

Browse Source 
Now you can add dynamics for servos next to the first order dynamics of
motors. Also, you can give a rate limit in PPRZ/loop units
This commit is contained in:
Ewoud Smeur
2017-02-09 10:39:26 +01:00
committed by Felix Ruess
parent 8340f82d02
commit 12d6c35c25
2 changed files with 37 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
+1 -1
View File
@@ -150,7 +150,7 @@
<define name="FILT_CUTOFF" value="5.0"/> <define name="FILT_CUTOFF" value="5.0"/>
<!-- first order actuator dynamics --> <!-- first order actuator dynamics -->
<define name="ACT_DYN" value="0.1"/> <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<!-- Adaptive Learning Rate --> <!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/> <define name="USE_ADAPTIVE" value="TRUE"/>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+36 -3
View File
@@ -75,6 +75,18 @@ bool indi_use_adaptive = true;
bool indi_use_adaptive = false; bool indi_use_adaptive = false;
#endif #endif
#ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
#endif
#ifdef STABILIZATION_INDI_ACT_IS_SERVO
bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
#else
bool act_is_servo[INDI_NUM_ACT] = {0};
#endif
float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
// variables needed for control // variables needed for control
float actuator_state_filt_vect[INDI_NUM_ACT]; float actuator_state_filt_vect[INDI_NUM_ACT];
struct FloatRates angular_accel_ref = {0., 0., 0.}; struct FloatRates angular_accel_ref = {0., 0., 0.};
@@ -334,7 +346,11 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
// Bound the inputs to the actuators // Bound the inputs to the actuators
for(i=0; i<INDI_NUM_ACT; i++) { for(i=0; i<INDI_NUM_ACT; i++) {
Bound(indi_u[i], 0, MAX_PPRZ); if(act_is_servo[i]) {
BoundAbs(indi_u[i], MAX_PPRZ);
} else {
Bound(indi_u[i], 0, MAX_PPRZ);
}
} }
// Propagate actuator filters // Propagate actuator filters
@@ -353,6 +369,7 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
//Don't increment if thrust is off //Don't increment if thrust is off
if(!in_flight) { if(!in_flight) {
float_vect_zero(indi_u, INDI_NUM_ACT); float_vect_zero(indi_u, INDI_NUM_ACT);
float_vect_zero(actuator_state_filt_vect, INDI_NUM_ACT);
} }
else if(indi_use_adaptive) { else if(indi_use_adaptive) {
lms_estimation(); lms_estimation();
@@ -426,7 +443,9 @@ void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinat
/** /**
* Function that tries to get actuator feedback. * Function that tries to get actuator feedback.
*
* If this is not available it will use a first order filter to approximate the actuator state. * If this is not available it will use a first order filter to approximate the actuator state.
* It is also possible to model rate limits (unit: PPRZ/loop cycle)
*/ */
void get_actuator_state(void) { void get_actuator_state(void) {
#if INDI_RPM_FEEDBACK #if INDI_RPM_FEEDBACK
@@ -434,10 +453,22 @@ void get_actuator_state(void) {
#else #else
//actuator dynamics //actuator dynamics
int8_t i; int8_t i;
float UNUSED prev_actuator_state;
for(i=0; i<INDI_NUM_ACT; i++) { for(i=0; i<INDI_NUM_ACT; i++) {
prev_actuator_state = actuator_state[i];
actuator_state[i] = actuator_state[i] actuator_state[i] = actuator_state[i]
+ STABILIZATION_INDI_ACT_DYN*( indi_u[i] - actuator_state[i]); + act_dyn[i]*( indi_u[i] - actuator_state[i]);
#ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
if((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
actuator_state[i] = prev_actuator_state + act_rate_limit[i];
} else if((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
actuator_state[i] = prev_actuator_state - act_rate_limit[i];
}
#endif
} }
#endif #endif
} }
@@ -592,13 +623,15 @@ void calc_g1g2_pseudo_inv(void) {
} }
} }
static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t *rpm, uint8_t num_act) static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
{ {
#if INDI_RPM_FEEDBACK
int8_t i; int8_t i;
for(i=0; i<num_act; i++) { for(i=0; i<num_act; i++) {
act_obs[i] = (rpm[i] - get_servo_min(i)); act_obs[i] = (rpm[i] - get_servo_min(i));
act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i)-get_servo_min(i))); act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i)-get_servo_min(i)));
} }
#endif
} }
/** /**