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

Merge pull request #1878 from paparazzi/obc_throttle_curves

Browse Source 
[rpm-control] throttle curves with rpm control
This commit is contained in:
Freek van Tienen
2018-06-21 16:27:58 +02:00
committed by GitHub
parent b8cdc09194 285a72c931
commit 477558a38f
18 changed files with 279 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/airframe.dtd
+1
View File
@@ -77,6 +77,7 @@ max CDATA #REQUIRED>
<!ATTLIST curve <!ATTLIST curve
throttle CDATA #REQUIRED throttle CDATA #REQUIRED
rpm CDATA #IMPLIED
collective CDATA #REQUIRED> collective CDATA #REQUIRED>
<!ATTLIST axis <!ATTLIST axis
conf/airframes/tudelft/bs_helidd_indi.xml
-1
View File
@@ -79,7 +79,6 @@
</section> </section>
<command_laws> <command_laws>
<call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
<call fun="swashplate_mixing_run(values)"/> <call fun="swashplate_mixing_run(values)"/>
<set servo="THROTTLE" value="throttle_curve.throttle"/> <set servo="THROTTLE" value="throttle_curve.throttle"/>
<set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/> <set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/>
conf/airframes/tudelft/bs_helidd_pid.xml
-1
View File
@@ -78,7 +78,6 @@
</section> </section>
<command_laws> <command_laws>
<call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
<call fun="swashplate_mixing_run(values)"/> <call fun="swashplate_mixing_run(values)"/>
<set servo="THROTTLE" value="throttle_curve.throttle"/> <set servo="THROTTLE" value="throttle_curve.throttle"/>
<set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/> <set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/>
conf/airframes/tudelft/heli450.xml
-1
View File
@@ -63,7 +63,6 @@
</section> </section>
<command_laws> <command_laws>
<call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
<call fun="swashplate_mixing_run(values)"/> <call fun="swashplate_mixing_run(values)"/>
<set servo="THROTTLE" value="throttle_curve.throttle"/> <set servo="THROTTLE" value="throttle_curve.throttle"/>
<set servo="BACK" value="swashplate_mixing.commands[SW_BACK]"/> <set servo="BACK" value="swashplate_mixing.commands[SW_BACK]"/>
conf/airframes/tudelft/heliGeniusDD.xml
-1
View File
@@ -78,7 +78,6 @@
</section> </section>
<command_laws> <command_laws>
<call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
<call fun="swashplate_mixing_run(values)"/> <call fun="swashplate_mixing_run(values)"/>
<set servo="THROTTLE" value="throttle_curve.throttle"/> <set servo="THROTTLE" value="throttle_curve.throttle"/>
<set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/> <set servo="FRONT" value="swashplate_mixing.commands[SW_FRONT]"/>
conf/airframes/tudelft/logo600.xml
+4 -4
View File
@@ -41,6 +41,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
<module name="intermcu" type="uart"> <module name="intermcu" type="uart">
<define name="REMAP_UART3" value="TRUE"/> <define name="REMAP_UART3" value="TRUE"/>
</module> </module>
<module name="heli" type="throttle_curve"/>
</firmware> </firmware>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
@@ -54,7 +55,6 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
<module name="actuators" type="spektrum"> <module name="actuators" type="spektrum">
<configure name="ACTUATORS_SPEKTRUM_DEV2" value="UART6"/> <configure name="ACTUATORS_SPEKTRUM_DEV2" value="UART6"/>
</module> </module>
<module name="heli" type="throttle_curve"/>
<define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Switch to Failsafe or to Autopilot on RC loss? --> <define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Switch to Failsafe or to Autopilot on RC loss? -->
<!--define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_AUTO"/--><!-- Switch to AUTO2 with a working autopilot on RC loss? Warning: beware of fly-away: program a GeoFence. beware: AP can ARM the FBW: airframe is dangerous at all times.--> <!--define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_AUTO"/--><!-- Switch to AUTO2 with a working autopilot on RC loss? Warning: beware of fly-away: program a GeoFence. beware: AP can ARM the FBW: airframe is dangerous at all times.-->
<define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Behave as a model aircraft: failsafe on RC lost. --> <define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Behave as a model aircraft: failsafe on RC lost. -->
@@ -88,6 +88,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
<commands> <commands>
<axis name="THRUST" failsafe_value="0"/> <axis name="THRUST" failsafe_value="0"/>
<axis name="COLLECTIVE" failsafe_value="-9600"/>
<axis name="ROLL" failsafe_value="0"/> <axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/> <axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/> <axis name="YAW" failsafe_value="0"/>
@@ -111,14 +112,13 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
</heli_curves> </heli_curves>
<command_laws> <command_laws>
<call fun="throttle_curve_run(TRUE, values)"/>
<let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<set servo="THROTTLE" value="($th_hold? -9600 : throttle_curve.throttle)"/> <set servo="THROTTLE" value="($th_hold? -9600 : @THRUST)"/>
<set servo="ROLL" value="@ROLL"/> <set servo="ROLL" value="@ROLL"/>
<set servo="PITCH" value="@PITCH"/> <set servo="PITCH" value="@PITCH"/>
<set servo="YAW" value="@YAW"/> <set servo="YAW" value="@YAW"/>
<set servo="COLLECTIVE" value="throttle_curve.collective"/> <set servo="COLLECTIVE" value="@COLLECTIVE"/>
<set servo="TAIL_GAIN" value="0"/> <set servo="TAIL_GAIN" value="0"/>
<set servo="AUX2" value="0"/> <set servo="AUX2" value="0"/>
</command_laws> </command_laws>
conf/airframes/tudelft/outback.xml
+47 -26
View File
@@ -73,7 +73,6 @@
<module name="intermcu" type="uart"/> <module name="intermcu" type="uart"/>
<module name="telemetry" type="intermcu"/> <module name="telemetry" type="intermcu"/>
<module name="opa_controller"/> <module name="opa_controller"/>
<module name="heli_throttle_curve"/>
<module name="heli_swashplate_mixing"/> <module name="heli_swashplate_mixing"/>
<module name="gps" type="ublox"/> <module name="gps" type="ublox"/>
<module name="gps_ubx_ucenter"/> <module name="gps_ubx_ucenter"/>
@@ -116,57 +115,79 @@
<set command="ROLL" value="@ROLL"/> <set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/> <set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/> <set command="YAW" value="@YAW"/>
<set command="FMODE" value="@FMODE"/>
</rc_commands> </rc_commands>
<commands> <commands>
<axis name="THRUST" failsafe_value="0"/> <axis name="THRUST" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/> <axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/> <axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/> <axis name="YAW" failsafe_value="9600"/>
<axis name="FMODE" failsafe_value="-9600"/> <axis name="COLLECTIVE" failsafe_value="-9600"/>
<axis name="TH_HOLD" failsafe_value="-9600"/>
<axis name="MODE" failsafe_value="-9600"/>
</commands> </commands>
<section name="MIXING" prefix="SW_MIXING_"> <section name="MIXING" prefix="SW_MIXING_">
<define name="TYPE" value="HR120"/> <define name="TYPE" value="HR120"/>
<define name="TRIM_ROLL" value="0"/> <define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/> <define name="TRIM_PITCH" value="-682"/>
<define name="TRIM_COLL" value="0"/> <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> </section>
<command_laws> <command_laws>
<call fun="throttle_curve_run(fbw_motors_on, values)"/>
<call fun="swashplate_mixing_run(values)"/> <call fun="swashplate_mixing_run(values)"/>
<let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <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_BACK" value="swashplate_mixing.commands[SW_BACK]"/>
<set servo="SW_LEFTFRONT" value="swashplate_mixing.commands[SW_LEFTFRONT]"/> <set servo="SW_LEFTFRONT" value="swashplate_mixing.commands[SW_LEFTFRONT]"/>
<set servo="SW_RIGHTFRONT" value="swashplate_mixing.commands[SW_RIGHTFRONT]"/> <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_LEFT" value="(Or($th_hold, $forward_curve)? 0 : (-@YAW - @THRUST*0.25) )"/>
<set servo="TAIL_RIGHT" value="($th_hold? 0 : (-@YAW - throttle_curve.throttle*0.25) )"/> <set servo="TAIL_RIGHT" value="(Or($th_hold, $forward_curve)? 0 : (-@YAW - @THRUST*0.25) )"/>
<set servo="AIL_LEFTLOW" value="($man_mode? ( @PITCH - @ROLL) : 0)"/> <set servo="AIL_LEFTLOW" value=" 2*@PITCH + 3*@YAW"/>
<set servo="AIL_RIGHTLOW" value="($man_mode? (-@PITCH - @ROLL) : 0)"/> <set servo="AIL_RIGHTLOW" value="-2*@PITCH + 3*@YAW"/>
<set servo="AIL_LEFTUP" value="($man_mode? ( @PITCH - @ROLL) : 0)"/> <set servo="AIL_LEFTUP" value=" 2*@PITCH + 3*@YAW"/>
<set servo="AIL_RIGHTUP" value="($man_mode? (-@PITCH - @ROLL) : 0)"/> <set servo="AIL_RIGHTUP" value="-2*@PITCH + 3*@YAW"/>
</command_laws> </command_laws>
<!-- v3 Pitch RODS black laminated props with straight angle root and 25deg blade-grip handles: 52mm top links 65mm bottom links -->
<!-- Pitch RODS black laminated props: 52mm top links 65mm bottom links -->
<heli_curves> <heli_curves>
<curve throttle="0,7000,7000" collective="-7500,-5000,-2500"/> <curve throttle="0,6000,0,0,0" collective="-9600,-8150,-6700,-5250,-3800"/>
<curve throttle="7500,9000,9600" collective="-7500,-5200,-2000"/> <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="7500,7500,7500" collective="-7000,500,8000"/> <curve throttle="8600,8600,8600,8600,9600" collective="-9600,-7300,-5000,-2300,500"/>
</heli_curves> </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> <!--heli_curves>
<curve throttle="0,7000,7000" collective="0,2000,4000"/> <curve throttle="0,6492,6492" collective="-5000,-2500,0"/>
<curve throttle="5000,7000,9000" collective="0,2000,4000"/> <curve throttle="6492,6492,6492" collective="-5000,-2700,1500"/>
<curve throttle="7500,8500,9600" collective="0,2000,4000"/> <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,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--> </heli_curves-->
<section name="MISC"> <section name="MISC">
conf/airframes/tudelft/walkera_genius_v2.xml
-1
View File
@@ -165,7 +165,6 @@ Using Lisa/S V1.0 board file as it is software compatible.
<let var="hoverstick" value=".1*MAX_PPRZ"/> <let var="hoverstick" value=".1*MAX_PPRZ"/>
<let var="halfway" value="(@THRUST >= ($hoverstick) ? 1 : 0)"/> <let var="halfway" value="(@THRUST >= ($hoverstick) ? 1 : 0)"/>
<call fun="throttle_curve_run(autopilot_get_motors_on(), values)"/>
<call fun="swashplate_mixing_run(values)"/> <call fun="swashplate_mixing_run(values)"/>
<set servo="CIC_FRONT" value="-swashplate_mixing.commands[SW_FRONT]"/> <set servo="CIC_FRONT" value="-swashplate_mixing.commands[SW_FRONT]"/>
<set servo="CIC_LEFT" value="-swashplate_mixing.commands[SW_LEFTBACK]"/> <set servo="CIC_LEFT" value="-swashplate_mixing.commands[SW_LEFTBACK]"/>
conf/conf_tests.xml
+23 -1
View File
@@ -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" 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" gui_color="blue"
/> />
<aircraft
name="DelftaCopter"
ac_id="48"
airframe="airframes/tudelft/outback.xml"
radio="radios/dummy.xml"
telemetry="telemetry/tudelft/outback.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/heli_throttle_curve.xml modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="#ffffdffac31f"
/>
<aircraft <aircraft
name="Bixler_Mission" name="Bixler_Mission"
ac_id="46" ac_id="46"
@@ -42,7 +53,7 @@
settings="settings/fixedwing_basic.xml settings/nps.xml" 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" 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" gui_color="#8fe1fffff553"
/> />
<aircraft <aircraft
name="DualBoard_AP_FBW" name="DualBoard_AP_FBW"
ac_id="3" ac_id="3"
@@ -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" 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" gui_color="white"
/> />
<aircraft
name="Logo600"
ac_id="49"
airframe="airframes/tudelft/logo600.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/heli_throttle_curve.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
gui_color="#ffffdffac31f"
/>
<aircraft <aircraft
name="MentorEnergy" name="MentorEnergy"
ac_id="13" ac_id="13"
conf/modules/heli_swashplate_mixing.xml
-1
View File
@@ -4,7 +4,6 @@
<doc> <doc>
<description>Helicopter Swashplate Mixing</description> <description>Helicopter Swashplate Mixing</description>
</doc> </doc>
<depends>heli_throttle_curve.xml</depends>
<header> <header>
<file name="swashplate_mixing.h"/> <file name="swashplate_mixing.h"/>
</header> </header>
conf/modules/heli_throttle_curve.xml
+16 -1
View File
@@ -3,13 +3,28 @@
<module name="throttle_curve" dir="helicopter"> <module name="throttle_curve" dir="helicopter">
<doc> <doc>
<description>Throttle Curve Mixers</description> <description>Throttle Curve Mixers</description>
<define name="THROTTLE_CURVE_RPM_FB_P" value="0" description="The RPM controller feeadback P gain of the PI controller"/>
<define name="THROTTLE_CURVE_RPM_FB_I" value="0" description="The RPM controller feeadback I gain of the PI controller"/>
<define name="THROTTLE_CURVE_RPM_INC_LIMIT" value="512" description="Amount of RPM the controller can increase or decrease per second"/>
<define name="THROTTLE_CURVE_RPM_ID" value="ABI_BROADCAST" description="The ABI sender ID to listen for RPM messages"/>
<define name="THROTTLE_CURVE_RPM_ACT" value="0" description="The ABI RPM message motor index"/>
</doc> </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> <header>
<file name="throttle_curve.h"/> <file name="throttle_curve.h"/>
</header> </header>
<init fun="throttle_curve_init()"/> <init fun="throttle_curve_init()"/>
<makefile target="ap|fbw"> <makefile target="ap">
<file name="throttle_curve.c"/> <file name="throttle_curve.c"/>
<define name="USE_THROTTLE_CURVES" value="TRUE"/>
</makefile> </makefile>
</module> </module>
conf/modules/rpm_control.xml
+16
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>
conf/telemetry/tudelft/outback.xml
+1
View File
@@ -30,6 +30,7 @@
<message name="TEMP_ADC" period="3.0"/> <message name="TEMP_ADC" period="3.0"/>
<message name="FBW_STATUS" period="2.0"/> <message name="FBW_STATUS" period="2.0"/>
<message name="MOTOR" period="0.5"/> <message name="MOTOR" period="0.5"/>
<message name="THROTTLE_CURVE" period="0.5"/>
</mode> </mode>
<mode name="ppm"> <mode name="ppm">
conf/userconf/tudelft/conf.xml
+2 -2
View File
@@ -173,7 +173,7 @@
telemetry="telemetry/tudelft/outback.xml" telemetry="telemetry/tudelft/outback.xml"
flight_plan="flight_plans/rotorcraft_basic.xml" flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" settings_modules="modules/heli_throttle_curve.xml modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml modules/temp_adc.xml modules/air_data.xml modules/geo_mag.xml modules/opa_controller.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="#ffffdffac31f" gui_color="#ffffdffac31f"
release="dc8a90d35290784bcc817edff08d8da2df64bcef" release="dc8a90d35290784bcc817edff08d8da2df64bcef"
/> />
@@ -276,7 +276,7 @@
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml" flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml" settings_modules="modules/air_data.xml modules/geo_mag.xml modules/heli_throttle_curve.xml modules/guidance_indi.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
gui_color="#ffffdffac31f" gui_color="#ffffdffac31f"
release="0fba1ce80a46d8dce23c570e6547595c89988fad" release="0fba1ce80a46d8dce23c570e6547595c89988fad"
/> />
sw/airborne/firmwares/rotorcraft/main_ap.c
+5
View File
@@ -247,6 +247,11 @@ void main_periodic(void)
autopilot_periodic(); autopilot_periodic();
/* set actuators */ /* set actuators */
//actuators_set(autopilot_get_motors_on()); //actuators_set(autopilot_get_motors_on());
#if USE_THROTTLE_CURVES
throttle_curve_run(commands, autopilot_get_mode());
#endif
#ifndef INTER_MCU_AP #ifndef INTER_MCU_AP
SetActuatorsFromCommands(commands, autopilot_get_mode()); SetActuatorsFromCommands(commands, autopilot_get_mode());
#else #else
sw/airborne/modules/helicopter/throttle_curve.c
+133 -11
View File
@@ -26,59 +26,179 @@
#include "throttle_curve.h" #include "throttle_curve.h"
#include "subsystems/commands.h" #include "subsystems/commands.h"
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/abi.h"
/* The switching values for the Throttle Curve Mode switch */ /* The switching values for the Throttle Curve Mode switch */
#define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB) #define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB)
/* Default RPM feedback gains and limits */
#ifndef THROTTLE_CURVE_RPM_FB_P
#define THROTTLE_CURVE_RPM_FB_P 0.0
#endif
#ifndef THROTTLE_CURVE_RPM_FB_I
#define THROTTLE_CURVE_RPM_FB_I 0.0
#endif
#ifndef THROTTLE_CURVE_RPM_INC_LIMIT
#define THROTTLE_CURVE_RPM_INC_LIMIT 512
#endif
/* Register the RPM callback */
#ifndef THROTTLE_CURVE_RPM_ID
#define THROTTLE_CURVE_RPM_ID ABI_BROADCAST
#endif
#ifndef THROTTLE_CURVE_RPM_ACT
#define THROTTLE_CURVE_RPM_ACT 0
#endif
static abi_event rpm_ev;
static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
/* Initialize the throttle curves from the airframe file */ /* Initialize the throttle curves from the airframe file */
struct throttle_curve_t throttle_curve = { struct throttle_curve_t throttle_curve = {
.nb_curves = THROTTLE_CURVES_NB, .nb_curves = THROTTLE_CURVES_NB,
.curves = THROTTLE_CURVES .curves = THROTTLE_CURVES
}; };
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void throttle_curve_send_telem(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_THROTTLE_CURVE(trans, dev, AC_ID, &throttle_curve.mode, &throttle_curve.throttle,
&throttle_curve.collective,
&throttle_curve.rpm, &throttle_curve.rpm_meas, &throttle_curve.rpm_err_sum);
}
#endif
/** /**
* Initialize the default throttle curve values * Initialize the default throttle curve values
*/ */
void throttle_curve_init(void) void throttle_curve_init(void)
{ {
throttle_curve.mode = THROTTLE_CURVE_MODE_INIT; throttle_curve.mode = THROTTLE_CURVE_MODE_INIT;
throttle_curve.nav_mode = THROTTLE_CURVE_MODE_INIT;
throttle_curve.throttle = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].throttle[0]; throttle_curve.throttle = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].throttle[0];
throttle_curve.collective = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].collective[0]; throttle_curve.collective = throttle_curve.curves[THROTTLE_CURVE_MODE_INIT].collective[0];
throttle_curve.rpm_fb_p = THROTTLE_CURVE_RPM_FB_P;
throttle_curve.rpm_fb_i = THROTTLE_CURVE_RPM_FB_I;
throttle_curve.rpm_err_sum = 0;
throttle_curve.rpm_measured = false;
throttle_curve.throttle_trim = 0;
throttle_curve.coll_trim = 0;
AbiBindMsgRPM(THROTTLE_CURVE_RPM_ID, &rpm_ev, rpm_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_THROTTLE_CURVE, throttle_curve_send_telem);
#endif
}
/**
* RPM callback for RPM based control throttle curves
*/
static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t *rpm, uint8_t num_act)
{
if(num_act <= THROTTLE_CURVE_RPM_ACT)
return;
throttle_curve.rpm_meas = rpm[THROTTLE_CURVE_RPM_ACT];
throttle_curve.rpm_measured = true;
} }
/** /**
* Run the throttle curve and generate the output throttle and pitch * Run the throttle curve and generate the output throttle and pitch
* This depends on the FMODE(flight mode) and TRHUST command * This depends on the FMODE(flight mode) and TRHUST command
*/ */
void throttle_curve_run(bool motors_on, pprz_t in_cmd[]) void throttle_curve_run(pprz_t cmds[], uint8_t ap_mode)
{ {
// Calculate the mode value from the switch // Calculate the mode value from the switch
int8_t mode = ((float)(in_cmd[COMMAND_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL); if (ap_mode != AP_MODE_NAV) {
Bound(mode, 0, THROTTLE_CURVES_NB - 1); int8_t mode = ((float)(radio_control.values[RADIO_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
throttle_curve.mode = mode; Bound(mode, 0, THROTTLE_CURVES_NB - 1);
throttle_curve.mode = mode;
} else {
throttle_curve.mode = throttle_curve.nav_mode;
}
// Failsafe curve
if (ap_mode == AP_MODE_FAILSAFE) {
throttle_curve.mode = 0;
}
// Check if we have multiple points or a single point // Check if we have multiple points or a single point
struct curve_t curve = throttle_curve.curves[mode]; struct curve_t curve = throttle_curve.curves[throttle_curve.mode];
if (curve.nb_points == 1) { if (curve.nb_points == 1) {
throttle_curve.throttle = curve.throttle[0]; throttle_curve.throttle = curve.throttle[0];
throttle_curve.collective = curve.collective[0]; throttle_curve.collective = curve.collective[0];
throttle_curve.rpm = curve.rpm[0];
} else { } else {
// Calculate the left point on the curve we need to use // Calculate the left point on the curve we need to use
uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1)); uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1));
int8_t curve_p = ((float)in_cmd[COMMAND_THRUST] / curve_range); int8_t curve_p = ((float)cmds[COMMAND_THRUST] / curve_range);
Bound(curve_p, 0, curve.nb_points - 1); Bound(curve_p, 0, curve.nb_points - 1);
// Calculate the throttle and pitch value // Calculate the throttle, pitch and rpm value
uint16_t x = in_cmd[COMMAND_THRUST] - curve_p * curve_range; uint16_t x = cmds[COMMAND_THRUST] - curve_p * curve_range;
throttle_curve.throttle = curve.throttle[curve_p] throttle_curve.throttle = curve.throttle[curve_p]
+ ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range); + ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
throttle_curve.collective = curve.collective[curve_p] throttle_curve.collective = curve.collective[curve_p]
+ ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range); + ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range);
if (curve.rpm[0] != 0xFFFF) {
if (throttle_curve.rpm == 0xFFFF) {
throttle_curve.rpm = throttle_curve.rpm_meas;
}
uint16_t new_rpm = curve.rpm[curve_p]
+ ((curve.rpm[curve_p + 1] - curve.rpm[curve_p]) * x / curve_range);
int32_t rpm_diff = new_rpm - throttle_curve.rpm;
Bound(rpm_diff, -(THROTTLE_CURVE_RPM_INC_LIMIT/512), THROTTLE_CURVE_RPM_INC_LIMIT/512);
throttle_curve.rpm += rpm_diff;
} else {
throttle_curve.rpm = 0xFFFF;
}
} }
// Trim
int32_t trimmed_throttle = throttle_curve.throttle + throttle_curve.throttle_trim;
Bound(trimmed_throttle, 0, MAX_PPRZ);
throttle_curve.throttle = trimmed_throttle;
int32_t trimmed_collective = throttle_curve.collective + throttle_curve.coll_trim;
Bound(trimmed_collective, -MAX_PPRZ, MAX_PPRZ);
throttle_curve.collective = trimmed_collective;
// Update RPM feedback
if (curve.rpm[0] != 0xFFFF && throttle_curve.rpm_measured) {
// Calculate RPM error
int32_t rpm_err = (throttle_curve.rpm - throttle_curve.rpm_meas);
// Calculate integrated error
throttle_curve.rpm_err_sum += rpm_err * throttle_curve.rpm_fb_i / 512.0f;
Bound(throttle_curve.rpm_err_sum, -throttle_curve.throttle, (MAX_PPRZ - throttle_curve.throttle));
// Calculate feedback command
int32_t rpm_feedback = rpm_err * throttle_curve.rpm_fb_p + throttle_curve.rpm_err_sum;
Bound(rpm_feedback, -MAX_PPRZ, MAX_PPRZ);
// Apply feedback command
int32_t new_throttle = throttle_curve.throttle + rpm_feedback;
Bound(new_throttle, 0, MAX_PPRZ);
throttle_curve.throttle = new_throttle;
throttle_curve.rpm_measured = false;
} else if (curve.rpm[0] == 0xFFFF) {
throttle_curve.rpm_err_sum = 0;
}
// Set the commands
cmds[COMMAND_THRUST] = throttle_curve.throttle; //Reuse for now
cmds[COMMAND_COLLECTIVE] = throttle_curve.collective;
// Only set throttle if motors are on // Only set throttle if motors are on
if (!motors_on) { if (!autopilot_get_motors_on()) {
throttle_curve.throttle = 0; cmds[COMMAND_THRUST] = 0;
throttle_curve.rpm_err_sum = 0;
} }
} }
@@ -87,5 +207,7 @@ void throttle_curve_run(bool motors_on, pprz_t in_cmd[])
*/ */
void nav_throttle_curve_set(uint8_t mode) void nav_throttle_curve_set(uint8_t mode)
{ {
commands[COMMAND_FMODE] = mode * THROTTLE_CURVE_SWITCH_VAL - MAX_PPRZ; int16_t new_mode = mode;
Bound(new_mode, 0, THROTTLE_CURVES_NB - 1);
throttle_curve.nav_mode = new_mode;
} }
sw/airborne/modules/helicopter/throttle_curve.h
+13 -1
View File
@@ -35,23 +35,35 @@
struct curve_t { struct curve_t {
uint8_t nb_points; ///< The number of points in the curve uint8_t nb_points; ///< The number of points in the curve
uint16_t throttle[THROTTLE_POINTS_NB]; ///< Throttle points in the curve uint16_t throttle[THROTTLE_POINTS_NB]; ///< Throttle points in the curve
uint16_t rpm[THROTTLE_POINTS_NB]; ///< RPM points in the curve
int16_t collective[THROTTLE_POINTS_NB]; ///< The collective points in the curve int16_t collective[THROTTLE_POINTS_NB]; ///< The collective points in the curve
}; };
/* Main throttle curve structure */ /* Main throttle curve structure */
struct throttle_curve_t { struct throttle_curve_t {
uint8_t mode; ///< Flight mode uint8_t mode; ///< Flight mode
uint8_t nav_mode; ///< Nav Flight mode
uint8_t nb_curves; ///< The number of throttle/pitch curves uint8_t nb_curves; ///< The number of throttle/pitch curves
struct curve_t curves[THROTTLE_CURVES_NB]; ///< Throttle/pitch curves struct curve_t curves[THROTTLE_CURVES_NB]; ///< Throttle/pitch curves
uint16_t throttle; ///< Output thrust(throttle) of the throttle curve uint16_t throttle; ///< Output thrust(throttle) of the throttle curve
int16_t collective; ///< Output collective of the throttle curve int16_t collective; ///< Output collective of the throttle curve
uint16_t rpm; ///< Output RPM of the throttle curve
uint16_t rpm_meas; ///< RPM measured
bool rpm_measured; ///< Whenever the RPM is measured
float rpm_err_sum; ///< Summed RPM error
float rpm_fb_p; ///< RPM feedback p gain
float rpm_fb_i;
int32_t throttle_trim; ///< RPM feedback i gain
int32_t coll_trim; ///< Collective trim
}; };
extern struct throttle_curve_t throttle_curve; extern struct throttle_curve_t throttle_curve;
/* External functions */ /* External functions */
extern void throttle_curve_init(void); extern void throttle_curve_init(void);
void throttle_curve_run(bool motors_on, pprz_t in_cmd[]); void throttle_curve_run(pprz_t in_cmd[], uint8_t autopilot_mode);
void nav_throttle_curve_set(uint8_t mode); void nav_throttle_curve_set(uint8_t mode);
#endif #endif
sw/tools/generators/gen_airframe.ml
+18 -4
View File
@@ -289,14 +289,28 @@ let parse_command = fun command no ->
let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
{ failsafe_value = failsafe_value; foo = 0} { failsafe_value = failsafe_value; foo = 0}
let parse_heli_curves = fun heli_surves -> let parse_heli_curves = fun heli_curves ->
let a = fun s -> ExtXml.attrib heli_surves s in let a = fun s -> ExtXml.attrib heli_curves s in
match Xml.tag heli_surves with match Xml.tag heli_curves with
"curve" -> "curve" ->
let throttle = a "throttle" in let throttle = a "throttle" in
let rpm = ExtXml.attrib_or_default heli_curves "rpm" "" in
let collective = a "collective" in let collective = a "collective" in
printf " {.nb_points = %i, \\\n" (List.length (Str.split (Str.regexp ",") throttle)); let nb_throttle = List.length (Str.split (Str.regexp ",") throttle) in
let nb_rpm = List.length (Str.split (Str.regexp ",") rpm) in
let nb_collective = List.length (Str.split (Str.regexp ",") collective) in
if nb_throttle < 1 then
failwith (Printf.sprintf "Need at least one value in throttle curve for a throttle ('%s', '%s')" throttle collective);
if nb_throttle <> nb_collective then
failwith (Printf.sprintf "Amount of throttle points not the same as collective in throttle curve ('%s', '%s')" throttle collective);
if nb_throttle <> nb_rpm && nb_rpm <> 0 then
failwith (Printf.sprintf "Amount of throttle points not the same as rpm in throttle curve ('%s', '%s', '%s')" throttle collective rpm);
printf " {.nb_points = %i, \\\n" nb_throttle;
printf " .throttle = {%s}, \\\n" throttle; printf " .throttle = {%s}, \\\n" throttle;
if nb_rpm <> 0 then
printf " .rpm = {%s}, \\\n" rpm
else
printf " .rpm = {0xFFFF}, \\\n";
printf " .collective = {%s}}, \\\n" collective printf " .collective = {%s}}, \\\n" collective
| _ -> xml_error "mixer" | _ -> xml_error "mixer"