Skip to content

Commit

Permalink
[conf] Update Outback airframe and Gyro filtering
Browse files Browse the repository at this point in the history
  • Loading branch information
@fvantienen
fvantienen committed May 18, 2016
1 parent b709198 commit eea3552
Show file tree
Hide file tree
Showing 7 changed files with 118 additions and 68 deletions.
4 changes: 2 additions & 2 deletions conf/airframes/TUDELFT/tudelft_conf.xml
View file
Expand Up @@ -214,8 +214,8 @@
airframe="airframes/TUDELFT/tudelft_outback.xml"
radio="radios/dummy.xml"
telemetry="telemetry/rotorcraft_with_logger.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml"
flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml"
settings_modules="modules/opa_controller.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml"
gui_color="#ffffdffac31f"
/>
Expand Down
118 changes: 75 additions & 43 deletions conf/airframes/TUDELFT/tudelft_outback.xml
View file
Expand Up @@ -23,6 +23,7 @@
<module name="imu" type="mpu6000"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="int_quat"/>
<module name="stabilization" type="rate"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins" type="hff"/>
<module name="guidance" type="indi"/>
Expand All @@ -38,6 +39,8 @@
<define name="SDLOGGER_DIRECT_CONTROL_SWITCH" value="AUX3"/>
</module>
<module name="gps_ubx_ucenter"/>
<module name="heli_throttle_curve"/>
<define name="ROTORCRAFT_IS_HELI" value="TRUE" />
</target>

<!-- FBW (Flight by Wire) part of the board -->
Expand Down Expand Up @@ -70,35 +73,44 @@

<servos driver="Pwm">
<servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1900"/>
<servo name="SW_BACK" no="1" min="1900" neutral="1500" max="1100"/>
<servo name="SW_BACK" no="1" min="1950" neutral="1550" max="1150"/>
<servo name="SW_LEFTFRONT" no="2" min="1100" neutral="1500" max="1900"/>
<servo name="SW_RIGHTFRONT" no="3" min="1900" neutral="1500" max="1100"/>
<servo name="SW_RIGHTFRONT" no="3" min="1880" neutral="1480" max="1080"/>
<servo name="TAIL_LEFT" no="4" min="1100" neutral="1500" max="1900"/>
<servo name="TAIL_RIGHT" no="5" min="1100" neutral="1500" max="1900"/>
<servo name="AIL_LEFTFRONT" no="6" min="1100" neutral="1500" max="1900"/>
<servo name="AIL_LEFTBACK" no="7" min="1100" neutral="1500" max="1900"/>
<servo name="AIL_RIGHTFRONT" no="8" min="1100" neutral="1500" max="1900"/>
<servo name="AIL_RIGHTBACK" no="9" min="1100" neutral="1500" max="1900"/>
<servo name="AIL_LEFTLOW" no="6" min="1100" neutral="1475" max="1900"/>
<servo name="AIL_RIGHTLOW" no="7" min="1100" neutral="1520" max="1900"/>
<servo name="AIL_LEFTUP" no="8" min="1100" neutral="1530" max="1900"/>
<servo name="AIL_RIGHTUP" no="9" min="1100" neutral="1565" max="1900"/>
</servos>

<section name="RADIO">
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
<define name="RADIO_FMODE" value="RADIO_AUX2"/>
</section>

<rc_commands>
<set command="THRUST" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
<set command="FMODE" value="@AUX2"/>
<set command="THRUST" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
<set command="MODE" value="@MODE"/>
<set command="FMODE" value="@FMODE"/>
<set command="TH_HOLD" value="@TH_HOLD"/>
</rc_commands>

<commands>
<axis name="THRUST" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="FMODE" failsafe_value="0"/>
<axis name="FMODE" failsafe_value="-9600"/>
<axis name="MODE" failsafe_value="-9600"/>
<axis name="TH_HOLD" failsafe_value="-9600"/>
</commands>

<section name="MIXING" prefix="SW_MIXING_">
<define name="TYPE" value="HR120"/>
<section name="MIXING" prefix="SW_MIXING_">
<define name="TYPE" value="HR120"/>
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_COLL" value="0"/>
Expand All @@ -107,26 +119,40 @@
<command_laws>
<call fun="throttle_curve_run(autopilot_motors_on, values)"/>
<call fun="swashplate_mixing_run(values)"/>
<set servo="THROTTLE" value="throttle_curve.throttle"/>
<set servo="THROTTLE" value="(((@TH_HOLD) > 4800)? throttle_curve.throttle :-9600)"/>
<set servo="SW_BACK" value="swashplate_mixing.commands[SW_BACK]"/>
<set servo="SW_LEFTFRONT" value="swashplate_mixing.commands[SW_LEFTFRONT]"/>
<set servo="SW_RIGHTFRONT" value="swashplate_mixing.commands[SW_RIGHTFRONT]"/>
<set servo="TAIL_LEFT" value="@YAW"/>
<set servo="TAIL_RIGHT" value="-@YAW"/>
<set servo="AIL_LEFTFRONT" value="@YAW"/>
<set servo="AIL_LEFTBACK" value="@YAW"/>
<set servo="AIL_RIGHTFRONT" value="@YAW"/>
<set servo="AIL_RIGHTBACK" value="@YAW"/>
<set servo="TAIL_LEFT" value="(((@TH_HOLD) > 4800)? (-@YAW - throttle_curve.throttle*0.5) : 0)"/>
<set servo="TAIL_RIGHT" value="(((@TH_HOLD) > 4800)? (-@YAW - throttle_curve.throttle*0.5) : 0)"/>
<set servo="AIL_LEFTLOW" value="((-4800 > @MODE)? (@PITCH - @ROLL) : 0)"/>
<set servo="AIL_RIGHTLOW" value="((-4800 > @MODE)? (-@PITCH - @ROLL) : 0)"/>
<set servo="AIL_LEFTUP" value="((-4800 > @MODE)? (@PITCH - @ROLL) : 0)"/>
<set servo="AIL_RIGHTUP" value="((-4800 > @MODE)? (-@PITCH - @ROLL) : 0)"/>
</command_laws>


<!-- Pitch RODS black laminated props: 52mm top links 65mm bottom links
<heli_curves>
<curve throttle="0,7000,7000" collective="-7500,-5000,-2500"/>
<curve throttle="7500,8500,9600" collective="-7500,-4500,-1500"/>
<curve throttle="7500,7500,7500" collective="-7000,500,8000"/>
</heli_curves>
-->

<!-- Pitch RODS white symmetric props: 56mm top links 65mm bottom links -->

<heli_curves>
<curve throttle="0,4800,7200,8400,9600" collective="-1000,750,2500,4250,6000"/>
<curve throttle="9600,7200,9600" collective="-7500,0,7500"/>
<curve throttle="0,7000,7000" collective="0,2000,4000"/>
<curve throttle="5000,7000,9000" collective="0,2000,4000"/>
<curve throttle="7500,8500,9600" collective="0,2000,4000"/>
</heli_curves>

<section name="MISC">
<define name="NAV_CLIMB_VSPEED" value="2.5"/>
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_DESCEND_VSPEED" value="-0.7"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>

Expand All @@ -145,7 +171,7 @@

<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="BODY_TO_IMU_PSI" value="180." unit="deg"/>

<!-- Rotate magneto compared to imu -90 degress -->
<define name="TO_MAG_PHI" value="0." unit="deg"/>
Expand All @@ -159,6 +185,12 @@
</section>

<section name="AHRS" prefix="AHRS_">
<!--This airframe vibrates a lot, which causes accel measurements in excess of 1g continuously-->
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
<define name="PROPAGATE_LOW_PASS_RATES" value="TRUE"/>
<define name="PROPAGATE_LOW_PASS_RATES_MUL" value="19"/>
<define name="PROPAGATE_LOW_PASS_RATES_DIV" value="20"/>

<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
Expand All @@ -177,7 +209,7 @@
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="GAIN_R" value="700"/>

<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
Expand Down Expand Up @@ -206,14 +238,14 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>

<!-- feedback -->
<define name="PHI_PGAIN" value="500"/>
<define name="PHI_DGAIN" value="100"/>
<define name="PHI_PGAIN" value="600"/>
<define name="PHI_DGAIN" value="260"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="500"/>
<define name="THETA_DGAIN" value="100"/>
<define name="THETA_PGAIN" value="600"/>
<define name="THETA_DGAIN" value="260"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="200"/>
<define name="PSI_DGAIN" value="0"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="460"/>
<define name="PSI_IGAIN" value="0"/>

<!-- feedforward -->
Expand All @@ -223,26 +255,26 @@
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
<define name="HOVER_KP" value="75"/>
<define name="HOVER_KD" value="40"/>
<define name="HOVER_KI" value="6"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="35" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
<define name="PGAIN" value="20"/>
<define name="DGAIN" value="40"/>
<define name="AGAIN" value="30"/>
<define name="IGAIN" value="10"/>
</section>

<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_RATE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_AUTO1" value="AP_MODE_RATE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
</section>

</airframe>
38 changes: 21 additions & 17 deletions conf/flight_plans/TUDELFT/tudelft_delft_basic.xml
View file
@@ -1,6 +1,6 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="4" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="60" name="Delft Basic" security_height="2">
<flight_plan alt="5" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="60" name="Delft Basic" security_height="2">
<header>
#include "autopilot.h"
</header>
Expand All @@ -12,9 +12,12 @@
<waypoint name="p2" x="-0.6" y="21.6"/>
<waypoint name="p3" x="22.2" y="-26.5"/>
<waypoint name="p4" x="4.9" y="-34.4"/>
<waypoint name="CAM" x="14.2" y="-29.4"/>
<waypoint name="TD" x="5.6" y="-10.9"/>
</waypoints>
<modules>
<!-- extra navigation routines -->
<module name="nav" type="heli_spinup"/>
</modules>
<blocks>
<block name="Wait GPS">
<call fun="NavKillThrottle()"/>
Expand All @@ -23,19 +26,30 @@
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
<call fun="nav_set_heading_current()"/>
</block>
<block name="Holding point">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<manual pitch="0" roll="0" yaw="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<call fun="nav_throttle_curve_set(0)" loop="false"/>
<call fun="nav_heli_spinup_setup(7, 0.15)" loop="false"/>
<call fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<call fun="nav_heli_spinup_run()"/>
<call fun="nav_throttle_curve_set(1)" loop="false"/>
<manual pitch="0" roll="0" yaw="0" throttle="0.15" until="stage_time>7" vmode="throttle"/>
<call fun="nav_throttle_curve_set(2)" loop="false"/>
<manual pitch="0" roll="0" yaw="0" throttle="0.15" until="stage_time>3" vmode="throttle"/>
</block>
<block name="Hold Attitude">
<manual pitch="0" roll="0" yaw="0" throttle="0.15" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<call fun="nav_set_heading_current()"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
<stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
Expand All @@ -49,7 +63,7 @@
</block>
<block name="line_p1_p2">
<go from="p1" hmode="route" wp="p2"/>
<stay until="stage_time>10" wp="p2"/>
<stay wp="p2" until="stage_time>10"/>
<go from="p2" hmode="route" wp="p1"/>
<deroute block="stay_p1"/>
</block>
Expand All @@ -59,16 +73,6 @@
<go from="p4" hmode="route" wp="p1"/>
<deroute block="stay_p1"/>
</block>
<block name="test yaw">
<go wp="p1"/>
<for from="1" to="16" var="i">
<heading alt="WaypointAlt(WP_p1)" course="90 * $i" until="stage_time > 3"/>
</for>
<deroute block="Standby"/>
</block>
<block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
<circle radius="nav_radius" wp="CAM"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call fun="NavSetWaypointHere(WP_TD)"/>
</block>
Expand All @@ -82,7 +86,7 @@
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
</blocks>
</flight_plan>
4 changes: 2 additions & 2 deletions conf/flight_plans/TUDELFT/tudelft_delft_outback.xml
View file
Expand Up @@ -44,7 +44,7 @@
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
<stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
Expand All @@ -59,7 +59,7 @@
</block>
<block name="line_p1_p2">
<go from="p1" hmode="route" wp="p2"/>
<stay until="stage_time>10" wp="p2"/>
<stay wp="p2" until="stage_time>10"/>
<go from="p2" hmode="route" wp="p1"/>
<deroute block="stay_p1"/>
</block>
Expand Down
5 changes: 3 additions & 2 deletions conf/telemetry/rotorcraft_with_logger.xml
View file
Expand Up @@ -29,6 +29,7 @@
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="TEMP_ADC" period="3.0"/>
<message name="FBW_STATUS" period="2.0"/>
<message name="MOTOR" period="0.5"/>
</mode>

<mode name="ppm">
Expand Down Expand Up @@ -151,10 +152,10 @@
<mode name="default">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".002"/>
<message name="IMU_ACCEL_SCALED" period=".002"/>
<!--message name="IMU_ACCEL_SCALED" period=".002"/-->
<message name="IMU_MAG_SCALED" period=".1"/>
<message name="ROTORCRAFT_CMD" period=".002"/>
</mode>
</process>

Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/modules/nav/nav_heli_spinup.h
View file
Expand Up @@ -27,6 +27,8 @@
#define NAV_HELI_SPINUP_H

#include "std.h"
#include "paparazzi.h"
#include "modules/helicopter/throttle_curve.h"

struct nav_heli_spinup_t {
uint16_t duration; ///< The duration in seconds to reach the final throttle
Expand Down
15 changes: 13 additions & 2 deletions sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
View file
Expand Up @@ -100,6 +100,17 @@ PRINT_CONFIG_VAR(AHRS_MAG_ZETA)
#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0
#endif

/** Default Rate filter Low pass */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
#ifndef AHRS_PROPAGATE_LOW_PASS_RATES_MUL
#define AHRS_PROPAGATE_LOW_PASS_RATES_MUL 2
#endif

#ifndef AHRS_PROPAGATE_LOW_PASS_RATES_DIV
#define AHRS_PROPAGATE_LOW_PASS_RATES_DIV 3
#endif
#endif

struct AhrsIntCmplQuat ahrs_icq;

static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt);
Expand Down Expand Up @@ -187,9 +198,9 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)

/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 2);
RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
RATES_ADD(ahrs_icq.imu_rate, omega);
RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 3);
RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
#else
RATES_COPY(ahrs_icq.imu_rate, omega);
#endif
Expand Down

0 comments on commit eea3552

Please sign in to comment.