mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-13 11:29:12 +08:00
Total Energy Control for Fixedwing
This commit is contained in:
Christophe De Wagter
committed by
Felix Ruess
Felix Ruess
parent
8a531fcfd9
commit
b845da2bb5
@@ -4,3 +4,6 @@
|
||||
|
||||
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/guidance_v.c
|
||||
|
||||
$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\"
|
||||
|
||||
|
||||
@@ -5,3 +5,5 @@
|
||||
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v.c
|
||||
|
||||
$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\"
|
||||
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
|
||||
# Standard fixed wing control loops
|
||||
|
||||
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/energy_ctrl.c
|
||||
|
||||
$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/energy_ctrl.h\"
|
||||
|
||||
@@ -4,3 +4,6 @@
|
||||
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v_n.c
|
||||
|
||||
$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\"
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,189 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="75" ground_alt="0" lat0="43.4622" lon0="1.2729" max_dist_from_home="1500" name="Versatile" qfu="270" security_height="25">
|
||||
<header>
|
||||
#include "firmwares/fixedwing/guidance/energy_ctrl.h"
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint name="STDBY" x="20" y="80"/>
|
||||
<waypoint name="1" x="44.8" y="102.2"/>
|
||||
<waypoint name="2" x="-63.5" y="122.9"/>
|
||||
<waypoint name="MOB" x="-11.5" y="-21.2"/>
|
||||
<waypoint name="S1" x="-151.6" y="80.4"/>
|
||||
<waypoint name="S2" x="180.1" y="214.9"/>
|
||||
<waypoint alt="30" name="AF" x="200" y="-10"/>
|
||||
<waypoint alt="0" name="TD" x="80.0" y="20.0"/>
|
||||
<waypoint name="BASELEG" x="26.9" y="-23.0"/>
|
||||
<waypoint name="_1" x="-100" y="0"/>
|
||||
<waypoint name="_2" x="-100" y="200"/>
|
||||
<waypoint name="_3" x="100" y="200"/>
|
||||
<waypoint name="_4" x="100" y="0"/>
|
||||
<waypoint name="CLIMB" x="-122.5" y="35.4"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector name="Square">
|
||||
<corner name="_1"/>
|
||||
<corner name="_2"/>
|
||||
<corner name="_3"/>
|
||||
<corner name="_4"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<exceptions/>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<set value="1" var="kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<call fun="NavSetGroundReferenceHere()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<set value="1" var="kill_throttle"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_icon="takeoff.png" strip_button="Takeoff (wp CLIMB)">
|
||||
<exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
|
||||
<set value="0" var="kill_throttle"/>
|
||||
<set value="CLIMB_AIRSPEED" var="v_ctl_auto_airspeed_setpoint" />
|
||||
<go wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<set value="NOMINAL_AIRSPEED" var="v_ctl_auto_airspeed_setpoint" />
|
||||
<circle radius="nav_radius" wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Figure 8 around wp 1" strip_button="Figure 8 (wp 1-2)" strip_icon="eight.png">
|
||||
<eight center="1" radius="nav_radius" turn_around="2"/>
|
||||
</block>
|
||||
<block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png">
|
||||
<set value="RACE_AIRSPEED" var="v_ctl_auto_airspeed_setpoint" />
|
||||
<oval p1="1" p2="2" radius="nav_radius"/>
|
||||
</block>
|
||||
<block name="MOB" strip_button="Turn around here" strip_icon="mob.png">
|
||||
<call fun="NavSetWaypointHere(WP_MOB)"/>
|
||||
<circle radius="100" wp="MOB"/>
|
||||
</block>
|
||||
<block name="Auto pitch (circle wp 1)">
|
||||
<circle pitch="auto" radius="75" throttle="0.7" wp="1"/>
|
||||
</block>
|
||||
<block name="Climb 75% throttle">
|
||||
<circle pitch="10" radius="50+(estimator_z-ground_alt)/2" throttle="0.75" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="throttle" wp="1"/>
|
||||
</block>
|
||||
<block name="Climb 0m/s">
|
||||
<circle climb="0" radius="nav_radius" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
|
||||
</block>
|
||||
<block name="Climb 1m/s">
|
||||
<circle climb="1" pitch="5" radius="50+(estimator_z-ground_alt)/2" until="10 > PowerVoltage()" vmode="climb" wp="1"/>
|
||||
</block>
|
||||
<block name="Climb nav_climb m/s">
|
||||
<circle climb="nav_climb" radius="nav_radius" until="(10 > PowerVoltage()) || (estimator_z > ground_alt+ 1350)" vmode="climb" wp="1"/>
|
||||
</block>
|
||||
|
||||
<block name="Circle 0% throttle">
|
||||
<circle pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle" wp="1"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="Oval 0% throttle">
|
||||
<oval p1="1" p2="2" pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
|
||||
<block name="Route 1-2">
|
||||
<go approaching_time="0" from="1" hmode="route" wp="2"/>
|
||||
</block>
|
||||
<block name="Stack wp 2">
|
||||
<circle radius="75" wp="2"/>
|
||||
</block>
|
||||
<block name="Route 2-1">
|
||||
<go approaching_time="0" from="2" hmode="route" wp="1"/>
|
||||
</block>
|
||||
<block name="Stack wp 1">
|
||||
<circle radius="75" wp="1"/>
|
||||
</block>
|
||||
<block name="Glide 1-2">
|
||||
<go from="1" hmode="route" vmode="glide" wp="2"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
<block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
|
||||
<survey_rectangle grid="150" wp1="S1" wp2="S2"/>
|
||||
</block>
|
||||
<block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png">
|
||||
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="Constant Slope Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
<call fun="nav_compute_final_from_glide(WP_AF, WP_TD, 10.)"/>
|
||||
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="Constant Slope Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png">
|
||||
<call fun="nav_compute_final_from_glide(WP_AF, WP_TD, 10.)"/>
|
||||
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<set value="NOMINAL_AIRSPEED" var="v_ctl_auto_airspeed_setpoint" />
|
||||
<call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
|
||||
<set value="GLIDE_AIRSPEED" var="v_ctl_auto_airspeed_setpoint" />
|
||||
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(estimator_z - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
|
||||
</block>
|
||||
<block name="final">
|
||||
<go from="AF" hmode="route" vmode="glide" wp="TD" approaching_time="5" />
|
||||
<set value="(GLIDE_AIRSPEED*10.0/12.0)" var="v_ctl_auto_airspeed_setpoint" />
|
||||
</block>
|
||||
<block name="flare">
|
||||
<go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="glide" wp="TD" approaching_time="0" />
|
||||
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle" />
|
||||
</block>
|
||||
<block name="Steps roll -10, +10">
|
||||
<while cond="TRUE">
|
||||
<attitude alt="250" roll="10.0" until=" stage_time > 6" vmode="alt"/>
|
||||
<attitude alt="250" roll="-10.0" until="stage_time > 6" vmode="alt"/>
|
||||
</while>
|
||||
</block>
|
||||
<block name="Steps roll -20, +20">
|
||||
<while cond="TRUE">
|
||||
<attitude alt="250" roll="20.0" until=" stage_time > 3" vmode="alt"/>
|
||||
<attitude alt="250" roll="-20.0" until="stage_time > 3" vmode="alt"/>
|
||||
</while>
|
||||
</block>
|
||||
<block name="Steps pitch -10, +10">
|
||||
<while cond="TRUE">
|
||||
<attitude alt="250" pitch="10" roll="0.0" until=" stage_time > 2" vmode="alt"/>
|
||||
<attitude alt="250" pitch="-10" roll="0.0" until=" stage_time > 2" vmode="alt"/>
|
||||
</while>
|
||||
</block>
|
||||
<block name="Heading 30">
|
||||
<heading alt="ground_alt+50" course="30" until="FALSE"/>
|
||||
</block>
|
||||
<block name="For loop (circles wp 1)">
|
||||
<for from="0" to="3" var="i">
|
||||
<circle radius="DEFAULT_CIRCLE_RADIUS+ $i*10" wp="1" until="NavCircleCount() > 1"/>
|
||||
</for>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
<block name="Test datalink (go to wp 2)">
|
||||
<exception cond="datalink_time > 22" deroute="Standby"/>
|
||||
<go from="STDBY" hmode="route" wp="2"/>
|
||||
<go from="2" hmode="route" wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Fly in Square">
|
||||
<exception cond="! InsideSquare(estimator_x, estimator_y)" deroute="Come back wp 1"/>
|
||||
<attitude alt="ground_alt+75" roll="0" vmode="alt"/>
|
||||
</block>
|
||||
<block name="Come back wp 1">
|
||||
<exception cond="InsideSquare(estimator_x, estimator_y)" deroute="Fly in Square"/>
|
||||
<go wp="1"/>
|
||||
<deroute block="Fly in Square"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -110,6 +110,7 @@
|
||||
<field name="y" type="float" format="%.0f" unit="m"/>
|
||||
<field name="altitude" type="float" format="%.0f" unit="m"/>
|
||||
<field name="climb" type="float" format="%.1f" unit="m/s"></field>
|
||||
<field name="airspeed" type="float" format="%.1f" unit="m/s"></field>
|
||||
</message>
|
||||
|
||||
<message name="GPS_SOL" id="17">
|
||||
|
||||
@@ -20,7 +20,7 @@
|
||||
<file name="airspeed_adc.h"/>
|
||||
</header>
|
||||
<init fun="airspeed_adc_init()"/>
|
||||
<periodic fun="airspeed_adc_update()" freq="10."/>
|
||||
<periodic fun="airspeed_adc_update()" />
|
||||
|
||||
<makefile>
|
||||
<file name="airspeed_adc.c"/>
|
||||
|
||||
@@ -0,0 +1,107 @@
|
||||
<!DOCTYPE settings SYSTEM "../settings.dtd">
|
||||
|
||||
<!-- A conf to use to tune a new A/C -->
|
||||
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="control">
|
||||
|
||||
<dl_settings NAME="trim">
|
||||
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="inter_mcu" param="COMMAND_ROLL_TRIM"/>
|
||||
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="attitude">
|
||||
<dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT"/>
|
||||
<dl_setting MAX="25000" MIN="0" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
|
||||
<dl_setting MAX="50000" MIN="0" STEP="10" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
|
||||
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_elevator_of_roll" shortname="elevator_of_roll" param="H_CTL_ELEVATOR_OF_ROLL"/>
|
||||
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
|
||||
|
||||
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll attitude pgain" param="H_CTL_ROLL_ATTITUDE_GAIN"/>
|
||||
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll rate gain" param="H_CTL_ROLL_RATE_GAIN"/>
|
||||
</dl_settings>
|
||||
|
||||
|
||||
<dl_settings name="climb_accel">
|
||||
<dl_setting MAX="2.0" MIN="0" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" module="guidance/energy_ctrl" param="V_CTL_ALTITUDE_PGAIN"/>
|
||||
<dl_setting MAX="1.0" MIN="0" STEP="0.01" VAR="v_ctl_airspeed_pgain" shortname="speed_pgain" param="V_CTL_AIRSPEED_PGAIN"/>
|
||||
<dl_setting MAX="10.0" MIN="-10" STEP="0.5" VAR="v_ctl_max_climb" shortname="max_climb" />
|
||||
<dl_setting MAX="2.0" MIN="-2" STEP="0.05" VAR="v_ctl_max_acceleration" shortname="max_acc_g" />
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="energy_loop">
|
||||
<dl_setting MAX="45" MIN="8.0" STEP="0.5" VAR="v_ctl_auto_airspeed_setpoint" shortname="airspeed sp" />
|
||||
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_nominal_cruise_throttle" shortname="cruise throttle" />
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_nominal_cruise_pitch" shortname="cruise pitch" />
|
||||
<dl_setting MAX="1.0" MIN="0" STEP="0.01" VAR="v_ctl_desired_acceleration" shortname="acc_cmd" />
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
|
||||
|
||||
<dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_of_airspeed_pgain" shortname="P_th_air"/>
|
||||
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_of_airspeed_igain" shortname="I_th_air" />
|
||||
<dl_setting MAX="1." MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_of_airspeed_pgain" shortname="P_pitch_air" />
|
||||
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_of_airspeed_igain" shortname="I_pitch_air" />
|
||||
<dl_setting MAX="5." MIN="0" STEP="0.01" VAR="v_ctl_auto_pitch_of_airspeed_dgain" shortname="D_pitch_air" />
|
||||
|
||||
<dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_total_pgain" shortname="P_en_tot"/>
|
||||
<dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_total_igain" shortname="I_en_tot"/>
|
||||
<dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_diff_pgain" shortname="P_en_dis"/>
|
||||
<dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_diff_igain" shortname="I_en_dis"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<!--
|
||||
|
||||
<dl_settings NAME="agr">
|
||||
<dl_setting MAX="1.0" MIN="0." STEP="0.05" VAR="agr_climb_throttle" shortname="climb_throttle" param="AGR_CLIMB_THROTTLE"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.1" STEP="0.05" VAR="agr_climb_pitch" shortname="climb_pitch" param="AGR_CLIMB_PITCH"/>
|
||||
<dl_setting MAX="1.0" MIN="0." STEP="0.1" VAR="agr_climb_nav_ratio" shortname="climb_nav_ratio" param="AGR_CLIMB_NAV_RATIO"/>
|
||||
<dl_setting MAX="1.0" MIN="0." STEP="0.05" VAR="agr_descent_throttle" shortname="descent_throttle" param="AGR_DESCENT_THROTTLE"/>
|
||||
<dl_setting MAX="0.1" MIN="-0.5" STEP="0.05" VAR="agr_descent_pitch" shortname="descent_ptich" param="AGR_DESCENT_PITCH"/>
|
||||
<dl_setting MAX="1.0" MIN="0." STEP="0.1" VAR="agr_descent_nav_ratio" shortname="descent_nav_ratio" param="AGR_DESCENT_NAV_RATIO"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="auto_throttle">
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE">
|
||||
<strip_button name="Loiter" value="0.1" group="dash_loiter"/>
|
||||
<strip_button name="Cruise" value="0" group="dash_loiter"/>
|
||||
<strip_button name="Dash" value="1" group="dash_loiter"/>
|
||||
</dl_setting>
|
||||
<dl_setting MAX="0.05" MIN="0.00" STEP="0.005" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_pgain" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_igain" shortname="throttle_igain" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
|
||||
<dl_setting MAX="2" MIN="0.0" STEP="0.1" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_dgain"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
|
||||
<dl_setting MAX="10" MIN="-10" STEP="0.1" VAR="v_ctl_auto_throttle_pitch_of_vz_dgain" shortname="pitch_of_vz (d)"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="auto_pitch">
|
||||
<dl_setting MAX="0.1" MIN="0.01" STEP="0.01" VAR="v_ctl_auto_pitch_pgain" shortname="pgain" param="V_CTL_AUTO_PITCH_PGAIN"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_pitch_igain" shortname="igain" param="V_CTL_AUTO_PITCH_IGAIN"/>
|
||||
</dl_settings>
|
||||
|
||||
-->
|
||||
|
||||
<dl_settings name="nav">
|
||||
<dl_setting MAX="3" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain" param="H_CTL_COURSE_PGAIN"/>
|
||||
<dl_setting MAX="2" MIN="0" STEP="0.1" VAR="h_ctl_course_dgain" shortname="course dgain" param="H_CTL_COURSE_DGAIN"/>
|
||||
<dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor" param="H_CTL_COURSE_PRE_BANK_CORRECTION"/>
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim" shortname="glide pitch trim" param="NAV_GLIDE_PITCH_TRIM"/>
|
||||
<dl_setting MAX="1" MIN="0.02" STEP="0.01" VAR="h_ctl_roll_slew" shortname="roll slew"/>
|
||||
<dl_setting MAX="500" MIN="-500" STEP="5" VAR="nav_radius"/>
|
||||
<dl_setting MAX="359" MIN="0" STEP="5" VAR="nav_course"/>
|
||||
<dl_setting MAX="2" MIN="1" STEP="1" VAR="nav_mode"/>
|
||||
<dl_setting MAX="5" MIN="-5" STEP="0.5" VAR="nav_climb"/>
|
||||
<dl_setting MAX="15" MIN="-15" STEP="1" VAR="fp_pitch"/>
|
||||
<dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="subsystems/nav" handler="IncreaseShift" shortname="inc. shift"/>
|
||||
<dl_setting MAX="50" MIN="5" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
|
||||
<dl_setting MAX="0.2" MIN="0" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
|
||||
<dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -0,0 +1,20 @@
|
||||
<!DOCTYPE settings SYSTEM "../settings.dtd">
|
||||
|
||||
<!-- A conf to use to tune a new A/C -->
|
||||
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="performance">
|
||||
|
||||
<dl_settings NAME="estimates">
|
||||
<dl_setting MAX="1" MIN="-1" STEP="1" VAR="ac_char_climb_pitch" shortname="climb_pitch" module="guidance/energy_ctrl"/>
|
||||
<dl_setting MAX="1" MIN="-1" STEP="1" VAR="ac_char_climb_max" shortname="climb_max" />
|
||||
<dl_setting MAX="1" MIN="-1" STEP="1" VAR="ac_char_descend_pitch" shortname="descend_pitch" />
|
||||
<dl_setting MAX="1" MIN="-1" STEP="1" VAR="ac_char_descend_max" shortname="descend_max" />
|
||||
<dl_setting MAX="1" MIN="-1" STEP="1" VAR="ac_char_cruise_throttle" shortname="cruise_thr" />
|
||||
<dl_setting MAX="1" MIN="-1" STEP="1" VAR="ac_char_cruise_pitch" shortname="cruise_pitch" />
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -90,7 +90,12 @@
|
||||
})
|
||||
|
||||
|
||||
#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint);
|
||||
#ifndef USE_AIRSPEED
|
||||
#define PERIODIC_SEND_DESIRED(_trans, _dev) { float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);}
|
||||
#else
|
||||
#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);
|
||||
#endif
|
||||
|
||||
|
||||
#if USE_INFRARED
|
||||
#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &contrast); }
|
||||
|
||||
@@ -0,0 +1,318 @@
|
||||
/*
|
||||
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file firmwares/fixedwing/guidance/guidance_v.c
|
||||
* Vertical control using total energy control for fixed wing vehicles.
|
||||
*
|
||||
|
||||
|
||||
=================================================
|
||||
Energy:
|
||||
------
|
||||
E = mgh + 1/2mV^2
|
||||
Edot / V = (gamma + Vdot/g) * W
|
||||
|
||||
equilibrium
|
||||
|
||||
Vdot / g = Thrust/W - Drag/W - sin(gamma)
|
||||
with: Drag/Weight = (Cl/Cd)^-1
|
||||
|
||||
-glide angle: Vdot = 0, T=0 ==> gamma = Cd/Cl
|
||||
-level flight: Vdot = 0, gamma=0 ==> W/T = Cl/Cd
|
||||
=================================================
|
||||
|
||||
Strategy: thrust = path + acceleration[g] (total energy)
|
||||
pitch = path - acceleration[g] (energy balance)
|
||||
|
||||
Pseudo-Control Unit = dimensionless acceleration [g]
|
||||
|
||||
- pitch <-> pseudocontrol: sin(Theta) steers Vdot in [g]
|
||||
- throttle <-> pseudocontrol: motor characteristic as function of V x throttle steeds VDot
|
||||
|
||||
*/
|
||||
|
||||
#pragma message "CAUTION! Using TOTAL ENERGY CONTROLLER: Experimental!"
|
||||
|
||||
#include "firmwares/fixedwing/guidance/energy_ctrl.h"
|
||||
#include "estimator.h"
|
||||
#include "subsystems/nav.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/fixedwing/autopilot.h"
|
||||
#include "subsystems/ahrs.h"
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
/////// DEFAULT GUIDANCE_V NECESSITIES //////
|
||||
|
||||
/* mode */
|
||||
uint8_t v_ctl_mode = V_CTL_MODE_MANUAL;
|
||||
uint8_t v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE;
|
||||
uint8_t v_ctl_auto_throttle_submode = V_CTL_CLIMB_MODE_AUTO_THROTTLE;
|
||||
float v_ctl_auto_throttle_sum_err = 0;
|
||||
float v_ctl_auto_airspeed_controlled = 0;
|
||||
float v_ctl_auto_groundspeed_setpoint = 0;
|
||||
|
||||
#ifdef LOITER_TRIM
|
||||
#error "Energy Controller can not accep Loiter Trim"
|
||||
#endif
|
||||
//#ifdef V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE
|
||||
//#error
|
||||
|
||||
/////// ACTUALLY USED STUFF //////
|
||||
|
||||
/* outer loop */
|
||||
float v_ctl_altitude_setpoint;
|
||||
float v_ctl_altitude_pre_climb; ///< Path Angle
|
||||
float v_ctl_altitude_pgain;
|
||||
float v_ctl_airspeed_pgain;
|
||||
float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low
|
||||
|
||||
float v_ctl_auto_airspeed_setpoint; ///< in meters per second
|
||||
|
||||
float v_ctl_max_climb = 2;
|
||||
float v_ctl_max_acceleration = 0.5;
|
||||
|
||||
/* inner loop */
|
||||
float v_ctl_climb_setpoint;
|
||||
|
||||
/* "auto throttle" inner loop parameters */
|
||||
float v_ctl_desired_acceleration;
|
||||
|
||||
float v_ctl_auto_throttle_cruise_throttle;
|
||||
float v_ctl_auto_throttle_nominal_cruise_throttle;
|
||||
float v_ctl_auto_throttle_nominal_cruise_pitch;
|
||||
float v_ctl_auto_throttle_climb_throttle_increment;
|
||||
float v_ctl_auto_throttle_pitch_of_vz_pgain;
|
||||
|
||||
float v_ctl_auto_throttle_of_airspeed_pgain;
|
||||
float v_ctl_auto_throttle_of_airspeed_igain;
|
||||
float v_ctl_auto_pitch_of_airspeed_pgain;
|
||||
float v_ctl_auto_pitch_of_airspeed_igain;
|
||||
float v_ctl_auto_pitch_of_airspeed_dgain;
|
||||
|
||||
float v_ctl_energy_total_pgain;
|
||||
float v_ctl_energy_total_igain;
|
||||
|
||||
float v_ctl_energy_diff_pgain;
|
||||
float v_ctl_energy_diff_igain;
|
||||
|
||||
|
||||
pprz_t v_ctl_throttle_setpoint;
|
||||
pprz_t v_ctl_throttle_slewed;
|
||||
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
// Automatically found airplane characteristics
|
||||
|
||||
float ac_char_climb_pitch = 0.0f;
|
||||
float ac_char_climb_max = 0.0f;
|
||||
int ac_char_climb_count = 0;
|
||||
float ac_char_descend_pitch = 0.0f;
|
||||
float ac_char_descend_max = 0.0f;
|
||||
int ac_char_descend_count = 0;
|
||||
float ac_char_cruise_throttle = 0.0f;
|
||||
float ac_char_cruise_pitch = 0.0f;
|
||||
int ac_char_cruise_count = 0;
|
||||
|
||||
static void ac_char_average(float* last, float new, int count)
|
||||
{
|
||||
*last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count);
|
||||
}
|
||||
|
||||
static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
|
||||
{
|
||||
if ((accelerate > -0.02) && (accelerate < 0.02))
|
||||
{
|
||||
if (throttle >= 1.0f)
|
||||
{
|
||||
ac_char_climb_count++;
|
||||
ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count );
|
||||
ac_char_average(&ac_char_climb_max , estimator_z_dot, ac_char_climb_count );
|
||||
}
|
||||
else if (throttle <= 0.0f)
|
||||
{
|
||||
ac_char_descend_count++;
|
||||
ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count );
|
||||
ac_char_average(&ac_char_descend_max , estimator_z_dot , ac_char_descend_count );
|
||||
}
|
||||
else if ((climb > -0.125) && (climb < 0.125))
|
||||
{
|
||||
ac_char_cruise_count++;
|
||||
ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count );
|
||||
ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void v_ctl_init( void ) {
|
||||
/* mode */
|
||||
v_ctl_mode = V_CTL_MODE_MANUAL;
|
||||
|
||||
/* outer loop */
|
||||
v_ctl_altitude_setpoint = 0.;
|
||||
v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
|
||||
v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
|
||||
|
||||
/* inner loops */
|
||||
v_ctl_climb_setpoint = 0.;
|
||||
|
||||
/* "auto throttle" inner loop parameters */
|
||||
v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
|
||||
v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
|
||||
v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
|
||||
|
||||
v_ctl_throttle_setpoint = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* outer loop
|
||||
* \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
|
||||
*/
|
||||
|
||||
void v_ctl_altitude_loop( void )
|
||||
{
|
||||
// Imput Checks
|
||||
if (v_ctl_auto_airspeed_setpoint <= 0.0f) v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
|
||||
|
||||
// Altitude Controller
|
||||
v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
|
||||
float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ;
|
||||
BoundAbs(sp, v_ctl_max_climb);
|
||||
|
||||
float incr = sp - v_ctl_climb_setpoint;
|
||||
BoundAbs(incr, 2.0 / 4.0);
|
||||
v_ctl_climb_setpoint += incr;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* auto throttle inner loop
|
||||
* \brief
|
||||
*/
|
||||
|
||||
const float dt = 0.01f;
|
||||
|
||||
float lp_vdot[5];
|
||||
|
||||
static float low_pass_vdot(float v);
|
||||
static float low_pass_vdot(float v)
|
||||
{
|
||||
lp_vdot[4] += (v - lp_vdot[4]) / 3;
|
||||
lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3;
|
||||
lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3;
|
||||
lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3;
|
||||
lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3;
|
||||
|
||||
return lp_vdot[0];
|
||||
}
|
||||
|
||||
void v_ctl_climb_loop( void )
|
||||
{
|
||||
// Airspeed outerloop: positive means we need to accelerate
|
||||
float speed_error = v_ctl_auto_airspeed_setpoint - estimator_airspeed;
|
||||
|
||||
// Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
|
||||
v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
|
||||
BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration);
|
||||
|
||||
// Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
|
||||
#ifndef SITL
|
||||
struct FloatVect3 accel_float = {0,0,0};
|
||||
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
|
||||
float vdot = ( accel_float.x / 9.81f - sin(ahrs_float.ltp_to_imu_euler.theta) );
|
||||
#else
|
||||
float vdot = 0;
|
||||
#endif
|
||||
|
||||
// Acceleration Error: positive means UAV needs to accelerate: needs extra energy
|
||||
float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot );
|
||||
|
||||
// Flight Path Outerloop: positive means needs to climb more: needs extra energy
|
||||
float gamma_err = (v_ctl_climb_setpoint - estimator_z_dot) / v_ctl_auto_airspeed_setpoint;
|
||||
|
||||
// Total Energy Error: positive means energy should be added
|
||||
float en_tot_err = gamma_err + vdot_err;
|
||||
|
||||
// Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
|
||||
float en_dis_err = gamma_err - vdot_err;
|
||||
|
||||
// Auto Cruise Throttle
|
||||
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB))
|
||||
{
|
||||
v_ctl_auto_throttle_nominal_cruise_throttle +=
|
||||
v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt
|
||||
+ en_tot_err * v_ctl_energy_total_igain * dt;
|
||||
if (v_ctl_auto_throttle_nominal_cruise_throttle < 0.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 0.0f;
|
||||
else if (v_ctl_auto_throttle_nominal_cruise_throttle > 1.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 1.0f;
|
||||
}
|
||||
|
||||
// Total Controller
|
||||
float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle
|
||||
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
|
||||
+ v_ctl_auto_throttle_of_airspeed_pgain * speed_error
|
||||
+ v_ctl_energy_total_pgain * en_tot_err;
|
||||
|
||||
|
||||
if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f))
|
||||
{
|
||||
// If your energy supply is not sufficient, then neglect the climb requirement
|
||||
en_dis_err = -vdot_err;
|
||||
}
|
||||
|
||||
/* pitch pre-command */
|
||||
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB))
|
||||
{
|
||||
v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt
|
||||
+ v_ctl_energy_diff_igain * en_dis_err * dt;
|
||||
Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
|
||||
}
|
||||
float v_ctl_pitch_of_vz =
|
||||
+ (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
|
||||
- v_ctl_auto_pitch_of_airspeed_pgain * speed_error
|
||||
+ v_ctl_auto_pitch_of_airspeed_dgain * vdot
|
||||
+ v_ctl_energy_diff_pgain * en_dis_err
|
||||
+ v_ctl_auto_throttle_nominal_cruise_pitch;
|
||||
|
||||
nav_pitch = v_ctl_pitch_of_vz;
|
||||
|
||||
ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
|
||||
|
||||
v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
|
||||
}
|
||||
|
||||
|
||||
#ifdef V_CTL_THROTTLE_SLEW_LIMITER
|
||||
#define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER))
|
||||
#endif
|
||||
|
||||
#ifndef V_CTL_THROTTLE_SLEW
|
||||
#define V_CTL_THROTTLE_SLEW 1.
|
||||
#endif
|
||||
|
||||
/** \brief Computes slewed throttle from throttle setpoint
|
||||
called at 20Hz
|
||||
*/
|
||||
void v_ctl_throttle_slew( void ) {
|
||||
pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;
|
||||
BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
|
||||
v_ctl_throttle_slewed += diff_throttle;
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
/*
|
||||
* Copyright (C) 2012 TUDelft
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file firmwares/fixedwing/guidance/energy_ctrl.h
|
||||
* Vertical control using total energy control for fixed wing vehicles.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef FW_V_CTL_ENERGY_H
|
||||
#define FW_V_CTL_ENERGY_H
|
||||
|
||||
#include "firmwares/fixedwing/guidance/guidance_common.h"
|
||||
|
||||
/* outer loop */
|
||||
// extern float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low
|
||||
extern float v_ctl_altitude_setpoint; ///< in meters above MSL
|
||||
extern float v_ctl_altitude_pre_climb; ///< Path Angle
|
||||
extern float v_ctl_altitude_pgain;
|
||||
extern float v_ctl_airspeed_pgain;
|
||||
|
||||
extern float v_ctl_auto_airspeed_setpoint; ///< in meters per second
|
||||
|
||||
extern float v_ctl_max_climb;
|
||||
extern float v_ctl_max_acceleration;
|
||||
|
||||
/* "auto throttle" inner loop parameters */
|
||||
extern float v_ctl_desired_acceleration;
|
||||
|
||||
extern float v_ctl_auto_throttle_nominal_cruise_throttle;
|
||||
extern float v_ctl_auto_throttle_nominal_cruise_pitch;
|
||||
extern float v_ctl_auto_throttle_climb_throttle_increment;
|
||||
extern float v_ctl_auto_throttle_pitch_of_vz_pgain;
|
||||
|
||||
extern float v_ctl_auto_throttle_of_airspeed_pgain;
|
||||
extern float v_ctl_auto_throttle_of_airspeed_igain;
|
||||
extern float v_ctl_auto_pitch_of_airspeed_pgain;
|
||||
extern float v_ctl_auto_pitch_of_airspeed_igain;
|
||||
extern float v_ctl_auto_pitch_of_airspeed_dgain;
|
||||
|
||||
extern float v_ctl_energy_total_pgain;
|
||||
extern float v_ctl_energy_total_igain;
|
||||
|
||||
extern float v_ctl_energy_diff_pgain;
|
||||
extern float v_ctl_energy_diff_igain;
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
// Automatically found airplane characteristics
|
||||
|
||||
extern float ac_char_climb_pitch;
|
||||
extern float ac_char_climb_max;
|
||||
extern float ac_char_descend_pitch;
|
||||
extern float ac_char_descend_max;
|
||||
extern float ac_char_cruise_throttle;
|
||||
extern float ac_char_cruise_pitch;
|
||||
|
||||
#endif /* FW_V_CTL_H */
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file firmwares/fixedwing/guidance/guidance_v.h
|
||||
* Vertical control for fixed wing vehicles.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef FW_V_CTL_COMMON_H
|
||||
#define FW_V_CTL_COMMON_H
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "paparazzi.h"
|
||||
|
||||
/* Vertical mode */
|
||||
#define V_CTL_MODE_MANUAL 0
|
||||
#define V_CTL_MODE_AUTO_THROTTLE 1
|
||||
#define V_CTL_MODE_AUTO_CLIMB 2
|
||||
#define V_CTL_MODE_AUTO_ALT 3
|
||||
#define V_CTL_MODE_NB 4
|
||||
extern uint8_t v_ctl_mode;
|
||||
|
||||
extern float v_ctl_climb_setpoint;
|
||||
extern uint8_t v_ctl_climb_mode;
|
||||
|
||||
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE 0
|
||||
#define V_CTL_CLIMB_MODE_AUTO_PITCH 1
|
||||
|
||||
extern uint8_t v_ctl_auto_throttle_submode;
|
||||
#define V_CTL_AUTO_THROTTLE_STANDARD 0
|
||||
#define V_CTL_AUTO_THROTTLE_AGRESSIVE 1
|
||||
#define V_CTL_AUTO_THROTTLE_BLENDED 2
|
||||
|
||||
// Needed for telemetry
|
||||
extern float v_ctl_auto_throttle_sum_err;
|
||||
|
||||
// Needed for course loop gain
|
||||
extern float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low
|
||||
|
||||
// Old airspeed code wants:
|
||||
extern float v_ctl_auto_airspeed_controlled;
|
||||
extern float v_ctl_auto_groundspeed_setpoint;
|
||||
|
||||
extern float v_ctl_auto_throttle_cruise_throttle;
|
||||
extern pprz_t v_ctl_throttle_setpoint;
|
||||
extern pprz_t v_ctl_throttle_slewed;
|
||||
|
||||
extern void v_ctl_init( void );
|
||||
extern void v_ctl_altitude_loop( void );
|
||||
extern void v_ctl_climb_loop ( void );
|
||||
|
||||
/** Computes throttle_slewed from throttle_setpoint */
|
||||
extern void v_ctl_throttle_slew( void );
|
||||
|
||||
#define guidance_v_SetCruiseThrottle(_v) { \
|
||||
v_ctl_auto_throttle_cruise_throttle = (_v ? _v : v_ctl_auto_throttle_nominal_cruise_throttle); \
|
||||
Bound(v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle, v_ctl_auto_throttle_max_cruise_throttle); \
|
||||
}
|
||||
|
||||
#define guidance_v_SetAutoThrottleIgain(_v) { \
|
||||
v_ctl_auto_throttle_igain = _v; \
|
||||
v_ctl_auto_throttle_sum_err = 0; \
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -235,6 +235,7 @@ void v_ctl_climb_loop ( void ) {
|
||||
v_ctl_climb_auto_throttle_loop();
|
||||
break;
|
||||
#ifdef V_CTL_AUTO_PITCH_PGAIN
|
||||
#pragma message "AUTO PITCH Enabled!"
|
||||
case V_CTL_CLIMB_MODE_AUTO_PITCH:
|
||||
v_ctl_climb_auto_pitch_loop();
|
||||
break;
|
||||
@@ -302,7 +303,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
|
||||
f_throttle = controlled_throttle;
|
||||
v_ctl_auto_throttle_sum_err += err;
|
||||
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
|
||||
nav_pitch += v_ctl_pitch_of_vz;
|
||||
nav_pitch = v_ctl_pitch_of_vz;
|
||||
#if defined AGR_CLIMB
|
||||
break;
|
||||
} /* switch submode */
|
||||
|
||||
@@ -58,7 +58,7 @@
|
||||
#include "firmwares/fixedwing/autopilot.h"
|
||||
#include "estimator.h"
|
||||
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
|
||||
#include "firmwares/fixedwing/guidance/guidance_v.h"
|
||||
#include CTRL_TYPE_H
|
||||
#include "subsystems/nav.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#ifdef TRAFFIC_INFO
|
||||
@@ -474,10 +474,33 @@ void navigation_task( void ) {
|
||||
#endif
|
||||
if (lateral_mode >=LATERAL_MODE_COURSE)
|
||||
h_ctl_course_loop(); /* aka compute nav_desired_roll */
|
||||
if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
|
||||
v_ctl_climb_loop();
|
||||
|
||||
// climb_loop(); //4Hz
|
||||
}
|
||||
energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours)
|
||||
}
|
||||
|
||||
|
||||
#if USE_AHRS
|
||||
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
|
||||
volatile uint8_t new_ins_attitude = 0;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
void attitude_loop( void ) {
|
||||
|
||||
#if USE_INFRARED
|
||||
ahrs_update_infrared();
|
||||
#endif /* USE_INFRARED */
|
||||
|
||||
if (pprz_mode >= PPRZ_MODE_AUTO2)
|
||||
{
|
||||
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE)
|
||||
v_ctl_throttle_setpoint = nav_throttle_setpoint;
|
||||
else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
|
||||
{
|
||||
v_ctl_climb_loop();
|
||||
}
|
||||
|
||||
#if defined V_CTL_THROTTLE_IDLE
|
||||
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ);
|
||||
@@ -495,19 +518,6 @@ void navigation_task( void ) {
|
||||
if (kill_throttle || (!estimator_flight_time && !launch))
|
||||
v_ctl_throttle_setpoint = 0;
|
||||
}
|
||||
energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours)
|
||||
}
|
||||
|
||||
|
||||
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
|
||||
volatile uint8_t new_ins_attitude = 0;
|
||||
#endif
|
||||
|
||||
void attitude_loop( void ) {
|
||||
|
||||
#if USE_INFRARED
|
||||
ahrs_update_infrared();
|
||||
#endif /* USE_INFRARED */
|
||||
|
||||
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
|
||||
v_ctl_throttle_slew();
|
||||
|
||||
@@ -35,7 +35,7 @@
|
||||
#include "estimator.h"
|
||||
#include "subsystems/nav.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/fixedwing/guidance/guidance_v.h"
|
||||
#include CTRL_TYPE_H
|
||||
#include "firmwares/fixedwing/autopilot.h"
|
||||
|
||||
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
#include "estimator.h"
|
||||
#include "subsystems/nav.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/fixedwing/guidance/guidance_v.h"
|
||||
#include CTRL_TYPE_H
|
||||
#include "firmwares/fixedwing/autopilot.h"
|
||||
|
||||
#pragma message "CAUTION! ALL control gains have to be positive now!"
|
||||
|
||||
Reference in New Issue
Block a user