Skip to content

Commit

Permalink
Added RC setpoint control in H-H mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@softsr
softsr committed Sep 15, 2013
1 parent 03234de commit 4775fc5
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 32 deletions.
67 changes: 36 additions & 31 deletions conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
View file
Expand Up @@ -25,7 +25,6 @@

<define name="NO_RC_THRUST_LIMIT"/>
<define name="USE_RC_FP_BLOCK_SWITCHING"/>
<define name="USE_ATTITUDE_REF" value="0"/>
</firmware>

<modules main_freq="512">
Expand Down Expand Up @@ -101,11 +100,14 @@
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>

<define name="GYRO_AVG_FILTER" value="3"/>
</section>

<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_UPDATE_NORM_HEURISTIC" value="1"/>
<define name="MAG_CORRECT_FREQUENCY" value="75"/>
<define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/>
<define name="H_Z" value="0.933303"/>
Expand Down Expand Up @@ -145,35 +147,35 @@
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="SP_MAX_R" value="50." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>

<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.87"/>
<define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>

<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.87"/>
<define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>

<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/>
<define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>

<!-- feedback -->
<define name="PHI_PGAIN" value="850"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="100"/>
<define name="PHI_PGAIN" value="1700"/>
<define name="PHI_DGAIN" value="500"/>
<define name="PHI_IGAIN" value="250"/>

<define name="THETA_PGAIN" value="850"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="1700"/>
<define name="THETA_DGAIN" value="500"/>
<define name="THETA_IGAIN" value="250"/>

<define name="PSI_PGAIN" value="4000"/>
<define name="PSI_DGAIN" value="1500"/>
Expand All @@ -182,7 +184,7 @@
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="2000"/>
<define name="PSI_DDGAIN" value="5000"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
Expand All @@ -191,13 +193,14 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration -->
<define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration -->
<define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed -->
<define name="REF_MAX_ZD" value="3."/> <!-- max climb speed -->
<define name="HOVER_KP" value="120"/>
<define name="HOVER_KD" value="80"/>
<define name="REF_MIN_ZDD" value="-2.5"/> <!-- max climb acceleration -->
<define name="REF_MAX_ZDD" value=" 2.5"/> <!-- max descent acceleration -->
<define name="REF_MIN_ZD" value="-3."/> <!-- max climb speed -->
<define name="REF_MAX_ZD" value="3."/> <!-- max descent speed -->
<define name="HOVER_KP" value="130"/>
<define name="HOVER_KD" value="120"/>
<define name="HOVER_KI" value="20"/>
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.35"/> <!-- initial takeoff throttle value -->
<define name="RC_CLIMB_COEF" value ="500"/>
<define name="RC_DESCENT_COEF" value ="200"/>
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
Expand All @@ -206,12 +209,14 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="USE_REF" value="1"/>
<define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
<define name="APPROX_FORCE_BY_THRUST" value="1"/>
<define name="MAX_BANK" value="27" unit="deg"/>
<define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
<define name="REF_OMEGA" value="RadOfDeg(45.)"/>
<define name="REF_ZETA" value="0.95"/>
<define name="REF_MAX_ACCEL" value="5."/> <!-- max reference horizontal acceleration in m/s2 -->
<define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
<define name="RC_SPEED_DEAD_BAND" value="200000"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="120"/>
<define name="DGAIN" value="220"/>
<define name="IGAIN" value="10"/>
<define name="AGAIN" value="10"/>
</section>
Expand All @@ -229,8 +234,8 @@
</section>

<section name="BAT">
<define name="CRITIC_BAT_LEVEL" value="13.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="13.2" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="14.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="14.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
</section>

Expand Down
35 changes: 34 additions & 1 deletion sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
View file
Expand Up @@ -71,7 +71,9 @@ struct Int32Vect2 guidance_h_pos_sp;
struct Int32Vect2 guidance_h_pos_ref;
struct Int32Vect2 guidance_h_speed_ref;
struct Int32Vect2 guidance_h_accel_ref;

#ifdef GUIDANCE_H_USE_SPEED_REF
struct Int32Vect2 guidance_h_speed_sp;
#endif
struct Int32Vect2 guidance_h_pos_err;
struct Int32Vect2 guidance_h_speed_err;
struct Int32Vect2 guidance_h_pos_err_sum;
Expand Down Expand Up @@ -211,6 +213,31 @@ void guidance_h_read_rc(bool_t in_flight) {

case GUIDANCE_H_MODE_HOVER:
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight);
#ifdef GUIDANCE_H_USE_SPEED_REF
if(in_flight) {
int32_t psi, s_psi, c_psi, rc_norm, max_pprz;
int64_t rc_x, rc_y;
int64_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED);
rc_x = (int64_t)radio_control.values[RADIO_PITCH];
rc_y = (int64_t)radio_control.values[RADIO_ROLL];
DeadBand(rc_x, MAX_PPRZ/20);
DeadBand(rc_y, MAX_PPRZ/20);
rc_norm = sqrt(pow(rc_x, 2) + pow(rc_y, 2));
rc_x = abs(rc_x);
rc_y = abs(rc_y);
max_pprz = rc_norm * MAX_PPRZ / Max(rc_x, rc_y);
rc_x = rc_x * max_speed / max_pprz;
rc_y = -rc_y * max_speed / max_pprz;
/* Rotate to body frame */
psi = stateGetNedToBodyEulers_i()->psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
guidance_h_speed_sp.x = (int32_t)(((int64_t)-c_psi * rc_x + (int64_t)s_psi * rc_y) / (1 << INT32_TRIG_FRAC));
guidance_h_speed_sp.y = (int32_t)(((int64_t)-s_psi * rc_x - (int64_t)c_psi * rc_y) / (1 << INT32_TRIG_FRAC));
}
else
stabilization_attitude_enter();
#endif
break;

case GUIDANCE_H_MODE_NAV:
Expand Down Expand Up @@ -304,12 +331,18 @@ void guidance_h_run(bool_t in_flight) {
static void guidance_h_update_reference(void) {
/* compute reference even if usage temporarily disabled via guidance_h_use_ref */
#if GUIDANCE_H_USE_REF
#if GUIDANCE_H_USE_SPEED_REF
if(guidance_h_mode == GUIDANCE_H_MODE_HOVER)
gh_update_ref_from_speed_sp(guidance_h_speed_sp);
else
#endif
gh_update_ref_from_pos_sp(guidance_h_pos_sp);
#endif

/* either use the reference or simply copy the pos setpoint */
if (guidance_h_use_ref) {
/* convert our reference to generic representation */
VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only
INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_LSHIFT(guidance_h_speed_ref, gh_speed_ref, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
INT32_VECT2_LSHIFT(guidance_h_accel_ref, gh_accel_ref, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC));
Expand Down

0 comments on commit 4775fc5

Please sign in to comment.