Skip to content

Commit

Permalink
[rpm_control] throttle curves with RPM control
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter @fvantienen
dewagter authored and fvantienen committed Jun 21, 2018
1 parent b8cdc09 commit 8ffa794
Show file tree
Hide file tree
Showing 11 changed files with 258 additions and 45 deletions.
1 change: 1 addition & 0 deletions conf/airframes/airframe.dtd
View file
Expand Up @@ -77,6 +77,7 @@ max CDATA #REQUIRED>

<!ATTLIST curve
throttle CDATA #REQUIRED
rpm CDATA #IMPLIED
collective CDATA #REQUIRED>

<!ATTLIST axis
Expand Down
73 changes: 47 additions & 26 deletions conf/airframes/tudelft/outback.xml
View file
Expand Up @@ -73,7 +73,6 @@
<module name="intermcu" type="uart"/>
<module name="telemetry" type="intermcu"/>
<module name="opa_controller"/>
<module name="heli_throttle_curve"/>
<module name="heli_swashplate_mixing"/>
<module name="gps" type="ublox"/>
<module name="gps_ubx_ucenter"/>
Expand Down Expand Up @@ -116,57 +115,79 @@
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
<set command="FMODE" value="@FMODE"/>
</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="-9600"/>
<axis name="TH_HOLD" failsafe_value="-9600"/>
<axis name="MODE" failsafe_value="-9600"/>
<axis name="YAW" failsafe_value="9600"/>
<axis name="COLLECTIVE" failsafe_value="-9600"/>
</commands>

<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"/>
<define name="TRIM_PITCH" value="-682"/>
<define name="TRIM_COLL" value="0"/>
</section>

<section name="THROTTLE_CURVE" prefix="THROTTLE_CURVE_">
<define name="RPM_FB_P" value="5.0"/>
<define name="RPM_FB_I" value="5.0"/>
</section>

<command_laws>
<call fun="throttle_curve_run(fbw_motors_on, values)"/>

<call fun="swashplate_mixing_run(values)"/>
<let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let var="man_mode" value="LessThan(RadioControlValues(RADIO_MODE), -4800)"/>
<let var="forward_curve" value="INTERMCU_GET_CMD_STATUS(INTERMCU_CMD_TIPPROPS)"/>
<call fun="INTERMCU_CLR_CMD_STATUS(INTERMCU_CMD_TIPPROPS)"/>

<set servo="THROTTLE" value="($th_hold? -9600 : throttle_curve.throttle)"/>
<set servo="THROTTLE" value="($th_hold? -9600 : @THRUST)"/>
<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="($th_hold? 0 : (-@YAW - throttle_curve.throttle*0.25) )"/>
<set servo="TAIL_RIGHT" value="($th_hold? 0 : (-@YAW - throttle_curve.throttle*0.25) )"/>
<set servo="AIL_LEFTLOW" value="($man_mode? ( @PITCH - @ROLL) : 0)"/>
<set servo="AIL_RIGHTLOW" value="($man_mode? (-@PITCH - @ROLL) : 0)"/>
<set servo="AIL_LEFTUP" value="($man_mode? ( @PITCH - @ROLL) : 0)"/>
<set servo="AIL_RIGHTUP" value="($man_mode? (-@PITCH - @ROLL) : 0)"/>
<set servo="TAIL_LEFT" value="(Or($th_hold, $forward_curve)? 0 : (-@YAW - @THRUST*0.25) )"/>
<set servo="TAIL_RIGHT" value="(Or($th_hold, $forward_curve)? 0 : (-@YAW - @THRUST*0.25) )"/>
<set servo="AIL_LEFTLOW" value=" 2*@PITCH + 3*@YAW"/>
<set servo="AIL_RIGHTLOW" value="-2*@PITCH + 3*@YAW"/>
<set servo="AIL_LEFTUP" value=" 2*@PITCH + 3*@YAW"/>
<set servo="AIL_RIGHTUP" value="-2*@PITCH + 3*@YAW"/>
</command_laws>


<!-- Pitch RODS black laminated props: 52mm top links 65mm bottom links -->
<!-- v3 Pitch RODS black laminated props with straight angle root and 25deg blade-grip handles: 52mm top links 65mm bottom links -->
<heli_curves>
<curve throttle="0,7000,7000" collective="-7500,-5000,-2500"/>
<curve throttle="7500,9000,9600" collective="-7500,-5200,-2000"/>
<curve throttle="7500,7500,7500" collective="-7000,500,8000"/>
<curve throttle="0,6000,0,0,0" collective="-9600,-8150,-6700,-5250,-3800"/>
<curve throttle="5000,6500,8000" rpm="1650,1650,1650" collective="-9600,-6500,-2300"/><!-- 1 blue bat: -5650 collective with 8000 throttle FF with 1650 RPM -->
<curve throttle="8600,8600,8600,8600,9600" collective="-9600,-7300,-5000,-2300,500"/>
</heli_curves>

<!-- Pitch RODS white symmetric props: 56mm top links 65mm bottom links -->
<!-- RPM to PPRZ calculation (governer low, fixed RPM) Castle Creations BEC-->
<!-- RPM * 0.000214141414 + 1.2841 = ms -->
<!-- 1500 RPM: 1.602ms 1.602 - 1.1 = 0.502ms 9600 / 800 * 502 = 6024 PPRZ -->
<!-- 1650 RPM: 1.641ms 1.641 - 1.1 = 0.541ms 9600 / 800 * 541 = 6492 PPRZ -->
<!-- 1800 RPM: 1.672ms 1.672 - 1.1 = 0.572ms 9600 / 800 * 572 = 6864 PPRZ -->

<!-- v2 Pitch RODS black laminated props with straight angle root and straight blade-grip handles: 63mm top links 65mm bottom links -->
<!--heli_curves>
<curve throttle="0,6492,6492" collective="-5000,-2500,0"/>
<curve throttle="6492,6492,6492" collective="-5000,-2700,1500"/>
<curve throttle="6492,6492,6024" collective="-5000,-2700,3500"/>
</heli_curves-->

<!-- v1 Pitch RODS black laminated props with twisted angle root and straight blade-grip handles: 52mm top links 65mm bottom links -->
<!--heli_curves>
<curve throttle="0,6492,6492" collective="-7500,-5000,-2500"/>
<curve throttle="6492,6492,6492" collective="-7500,-5200,-1000"/>
<curve throttle="6492,6492,6024" collective="-7500,-5200,2000"/>
</heli_curves-->

<!-- v0 Pitch RODS white symmetric props: 56mm top links 65mm bottom links -->
<!--heli_curves>
<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"/>
<curve throttle="0,6024,6024" collective="0,2000,4000"/>
<curve throttle="6024,6024,6024" collective="0,2000,4000"/>
<curve throttle="6492,6492,6492" collective="0,2000,4000"/>
</heli_curves-->

<section name="MISC">
Expand Down
24 changes: 23 additions & 1 deletion conf/conf_tests.xml
View file
Expand Up @@ -32,6 +32,17 @@
settings_modules="modules/nav_basic_fw.xml modules/gps.xml modules/ahrs_float_dcm.xml modules/imu_common.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml"
gui_color="blue"
/>
<aircraft
name="DelftaCopter"
ac_id="45"
airframe="airframes/TUDELFT/tudelft_outback.xml"
radio="radios/dummy.xml"
telemetry="telemetry/TUDELFT/tudelft_outback.xml"
flight_plan="flight_plans/TUDELFT/tudelft_delft_outback.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/imu_common.xml modules/gps.xml modules/ahrs_int_cmpl_quat.xml modules/opa_controller.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml modules/heli_throttle_curve.xml"
gui_color="#ffffdffac31f"
/>
<aircraft
name="Bixler_Mission"
ac_id="46"
Expand All @@ -42,7 +53,7 @@
settings="settings/fixedwing_basic.xml settings/nps.xml"
settings_modules="modules/nav_basic_fw.xml modules/gps.xml modules/ahrs_float_dcm.xml modules/imu_common.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml"
gui_color="#8fe1fffff553"
/>
/>
<aircraft
name="DualBoard_AP_FBW"
ac_id="3"
Expand Down Expand Up @@ -175,6 +186,17 @@
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="white"
/>
<aircraft
name="Logo600"
ac_id="44"
airframe="airframes/TUDELFT/tudelft_logo600.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/imu_common.xml modules/gps.xml modules/ahrs_int_cmpl_quat.xml modules/heli_throttle_curve.xml modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="#ffffdffac31f"
/>
<aircraft
name="MentorEnergy"
ac_id="13"
Expand Down
1 change: 0 additions & 1 deletion conf/modules/heli_swashplate_mixing.xml
View file
Expand Up @@ -4,7 +4,6 @@
<doc>
<description>Helicopter Swashplate Mixing</description>
</doc>
<depends>heli_throttle_curve.xml</depends>
<header>
<file name="swashplate_mixing.h"/>
</header>
Expand Down
12 changes: 11 additions & 1 deletion conf/modules/heli_throttle_curve.xml
View file
Expand Up @@ -4,12 +4,22 @@
<doc>
<description>Throttle Curve Mixers</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="ThrottleCurve">
<dl_setting var="throttle_curve.rpm_fb_p" min="0.00" step="0.005" max="100" shortname="rpm_fb_p" param="THROTTLE_CURVE_RPM_FB_P" />
<dl_setting var="throttle_curve.rpm_fb_i" min="0.00" step="0.005" max="100" shortname="rpm_fb_i" param="THROTTLE_CURVE_RPM_FB_I" />
<dl_setting var="throttle_curve.throttle_trim" min="-9600" step="0" max="9600" shortname="throttle_trim" />
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="throttle_curve.h"/>
</header>
<init fun="throttle_curve_init()"/>
<makefile target="ap|fbw">
<makefile target="ap">
<file name="throttle_curve.c"/>
<define name="USE_THROTTLE_CURVES" value="TRUE"/>
</makefile>
</module>

16 changes: 16 additions & 0 deletions conf/modules/rpm_control.xml
View file
@@ -0,0 +1,16 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="rpm_control" dir="ctrl">
<doc>
<description>RPM controller with feedback</description>
</doc>
<header>
<file name="rpm_control.h"/>
</header>
<init fun="rpm_control_init()"/>
<periodic fun="rpm_control_periodic()" freq="512" autorun="TRUE"/>
<makefile>
<file name="rpm_control.c"/>
</makefile>
</module>

1 change: 1 addition & 0 deletions conf/telemetry/tudelft/outback.xml
View file
Expand Up @@ -30,6 +30,7 @@
<message name="TEMP_ADC" period="3.0"/>
<message name="FBW_STATUS" period="2.0"/>
<message name="MOTOR" period="0.5"/>
<message name="THROTTLE_CURVE" period="0.5"/>
</mode>

<mode name="ppm">
Expand Down
5 changes: 5 additions & 0 deletions sw/airborne/firmwares/rotorcraft/main_ap.c
View file
Expand Up @@ -247,6 +247,11 @@ void main_periodic(void)
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_get_motors_on());

#if USE_THROTTLE_CURVES
throttle_curve_run(commands, autopilot_mode);
#endif

#ifndef INTER_MCU_AP
SetActuatorsFromCommands(commands, autopilot_get_mode());
#else
Expand Down

0 comments on commit 8ffa794

Please sign in to comment.