Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[autopilot] support generated autopilot, based on rotorcraft firmware

Browse Source 
By default the original static autopilot is used. A config flag can
enable the use of the generated AP code.
A basic autopilot description is provided (4 modes + failsafe modes).
The server is capable of using the list of generated mode to properly
display mode names.
Tested in NAV and GUIDED mode in sim, and direct attitude control on
real aircraft.
This commit is contained in:
Gautier Hattenberger
2016-12-19 22:21:04 +01:00
parent 5166037055
commit 5613d79dc4
26 changed files with 1731 additions and 700 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/hoops_gen_ap.xml
+287
View File
@@ -0,0 +1,287 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- this is a quadrotor frame in X-configuration equiped with
* Autopilot: ELLE0 1.2 with STM32F4 http://wiki.paparazziuav.org/wiki/ELLE0
* IMU: MPU9250 & MS5611 http://wiki.paparazziuav.org/wiki/ELLE0#IMU
* Actuators: PWM motor controllers http://wiki.paparazziuav.org/wiki/Subsystem/actuators#PWM
* (GPS: Ublox http://wiki.paparazziuav.org/wiki/Subsystem/gps)
* RC: one (two) Spektrum sats http://wiki.paparazziuav.org/wiki/Subsystem/radio_control#Spektrum
-->
<airframe name="Hooper Gen AP">
<autopilot name="rotorcraft_autopilot.xml"/>
<firmware name="rotorcraft">
<configure name="USE_GENERATED_AUTOPILOT" value="TRUE"/>
<configure name="AHRS_ALIGNER_LED" value="2"/>
<define name="IMU_MPU9250_READ_MAG" value="0"/>
<define name="LOW_NOISE_THRESHOLD" value="3500"/>
<define name="LOW_NOISE_TIME" value="10"/>
<target name="ap" board="elle0_1.2">
</target>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim"/>
</target>
<module name="radio_control" type="spektrum">
<define name="RADIO_MODE" value="RADIO_FLAP"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
</module>
<module name="motor_mixing"/>
<module name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
</module>
<module name="telemetry" type="transparent"/>
<module name="imu" type="elle0"/>
<module name="mag" type="hmc58xx">
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
<define name="HMC58XX_CHAN_X" value="1"/>
<define name="HMC58XX_CHAN_Y" value="0"/>
</module>
<module name="gps" type="ublox">
<configure name="GPS_BAUD" value="B115200"/>
</module>
<module name="stabilization" type="int_quat"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins" />
<module name="send_imu_mag_current"/>
<!--module name="geo_mag"/-->
<module name="air_data"/>
<module name="stabilization" type="rate"/>
<!--define name="KILL_ON_GROUND_DETECT" value="TRUE"/-->
<module name="bat_checker">
<define name="BAT_CHECKER_GPIO" value="GPIOA,GPIO0"/>
<define name="USE_PWM5" value="0"/>
</module>
</firmware>
<servos driver="Pwm">
<servo name="FL" no="0" min="1000" neutral="1200" max="2000"/>
<servo name="FR" no="1" min="1000" neutral="1200" max="2000"/>
<servo name="BR" no="2" min="1000" neutral="1200" max="2000"/>
<servo name="BL" no="3" min="1000" neutral="1200" max="2000"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- front left (CW), front right (CCW), back right (CW), back left (CCW) -->
<define name="TYPE" value="QUAD_X"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="FL" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="FR" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BR" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BL" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="-55"/>
<define name="ACCEL_Y_NEUTRAL" value="3"/>
<define name="ACCEL_Z_NEUTRAL" value="-141"/>
<define name="ACCEL_X_SENS" value="2.44645099697" integer="16"/>
<define name="ACCEL_Y_SENS" value="2.44618333167" integer="16"/>
<define name="ACCEL_Z_SENS" value="2.43722535075" integer="16"/>
<!-- MPU MAG -->
<!--define name="MAG_X_NEUTRAL" value="-326"/>
<define name="MAG_Y_NEUTRAL" value="-82"/>
<define name="MAG_Z_NEUTRAL" value="-448"/>
<define name="MAG_X_SENS" value="8.93784052264" integer="16"/>
<define name="MAG_Y_SENS" value="8.76983337164" integer="16"/>
<define name="MAG_Z_SENS" value="8.15871475442" integer="16"/-->
<!-- HMC MAG -->
<define name="MAG_X_SIGN" value="-1"/>
<define name="MAG_Y_SIGN" value="1"/>
<define name="MAG_Z_SIGN" value="1"/>
<define name="MAG_X_NEUTRAL" value="-86"/>
<define name="MAG_Y_NEUTRAL" value="41"/>
<define name="MAG_Z_NEUTRAL" value="-32"/>
<define name="MAG_X_SENS" value="3.73940141026" integer="16"/>
<define name="MAG_Y_SENS" value="3.81423665322" integer="16"/>
<define name="MAG_Z_SENS" value="4.15369264623" integer="16"/>
<!--define name= "MAG_X_CURRENT_COEF" value="0.0350248861409"/>
<define name= "MAG_Y_CURRENT_COEF" value="-0.0118884242797"/>
<define name= "MAG_Z_CURRENT_COEF" value="0.0176235525201"/-->
<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"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module if loaded -->
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="300" unit="deg/s"/>
<define name="SP_MAX_Q" value="300" unit="deg/s"/>
<define name="SP_MAX_R" value="240" unit="deg/s"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<!-- feedback -->
<define name="GAIN_P" value="1000"/>
<define name="GAIN_Q" value="1000"/>
<define name="GAIN_R" value="800"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="200." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="250." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="500"/>
<define name="PHI_DGAIN" value="260"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="500"/>
<define name="THETA_DGAIN" value="260"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.05"/>
<define name="G1_Q" value="0.025"/>
<define name="G1_R" value="0.0022"/>
<define name="G2_R" value="0.20"/>
<define name="FILTER_ROLL_RATE" value="TRUE"/>
<define name="FILTER_PITCH_RATE" value="TRUE"/>
<define name="FILTER_YAW_RATE" value="FALSE"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="170.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="14.3"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</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"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" 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"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="HOOPERFLY/hooperfly_teensyfly_quad" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/> <!-- for compilation -->
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/> <!-- for compilation -->
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
<define name="MILLIAMP_AT_FULL_THROTTLE" value="20000" unit="mA"/>
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" value="mA"/>
<define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/>
</section>
<section name="GCS">
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
<define name="ALT_SHIFT_PLUS" value="1"/>
<define name="ALT_SHIFT_MINUS" value="-1"/>
<define name="AC_ICON" value="quadrotor_x"/>
</section>
</airframe>
conf/autopilot/autopilot.dtd
+46 -5
View File
@@ -1,10 +1,10 @@
<!-- Paparazzi Autopilot DTD -->
<!ELEMENT autopilot (state_machine+)>
<!ELEMENT state_machine (modules*,includes?,control_block*,exceptions?,mode*)>
<!ELEMENT state_machine (modules*,includes?,settings*,control_block*,exceptions?,mode*)>
<!ELEMENT control_block (call*)>
<!ELEMENT exceptions (exception*)>
<!ELEMENT includes (include*)>
<!ELEMENT includes (include*,define*)>
<!ELEMENT mode (select*,control*,exception*)>
<!ELEMENT select EMPTY>
<!ELEMENT control (call|call_block)*>
@@ -16,6 +16,10 @@
<!ELEMENT load (configure|define)*>
<!ELEMENT define EMPTY>
<!ELEMENT configure EMPTY>
<!ELEMENT settings (dl_setting*)>
<!ELEMENT dl_setting (strip_button|key_press)*>
<!ELEMENT strip_button EMPTY>
<!ELEMENT key_press EMPTY>
<!ATTLIST autopilot
name CDATA #IMPLIED>
@@ -24,13 +28,15 @@ name CDATA #IMPLIED>
name CDATA #REQUIRED
freq CDATA #REQUIRED
gcs_mode CDATA #IMPLIED
settings_mode CDATA #IMPLIED>
settings_mode CDATA #IMPLIED
settings_handler CDATA #IMPLIED>
<!ATTLIST control_block
name CDATA #REQUIRED>
<!ATTLIST mode
name CDATA #REQUIRED
shortname CDATA #IMPLIED
start CDATA #IMPLIED
stop CDATA #IMPLIED
gcs_name CDATA #IMPLIED
@@ -52,7 +58,7 @@ cond CDATA #REQUIRED
deroute CDATA #REQUIRED>
<!ATTLIST include
href CDATA #REQUIRED>
name CDATA #REQUIRED>
<!ATTLIST call
fun CDATA #REQUIRED
@@ -69,9 +75,44 @@ target CDATA #IMPLIED>
<!ATTLIST define
name CDATA #REQUIRED
value CDATA #IMPLIED>
value CDATA #IMPLIED
cond CDATA #IMPLIED>
<!ATTLIST configure
name CDATA #REQUIRED
value CDATA #REQUIRED>
<!ATTLIST settings
name CDATA #IMPLIED
>
<!ATTLIST dl_setting
var CDATA #REQUIRED
min CDATA #REQUIRED
max CDATA #REQUIRED
type CDATA #IMPLIED
step CDATA #IMPLIED
widget CDATA #IMPLIED
shortname CDATA #IMPLIED
module CDATA #IMPLIED
handler CDATA #IMPLIED
param CDATA #IMPLIED
unit CDATA #IMPLIED
alt_unit CDATA #IMPLIED
alt_unit_coef CDATA #IMPLIED
values CDATA #IMPLIED
persistent CDATA #IMPLIED
>
<!ATTLIST strip_button
name CDATA #REQUIRED
value CDATA #REQUIRED
icon CDATA #IMPLIED
group CDATA #IMPLIED
>
<!ATTLIST key_press
key CDATA #REQUIRED
value CDATA #REQUIRED
>
conf/autopilot/rotorcraft_autopilot.xml
+85 -39
View File
@@ -2,7 +2,31 @@
<autopilot name="Quadrotor Autopilot (Basic version)">
<state_machine name="main" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<includes>
<include name="generated/airframe.h"/>
<include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/>
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="stabilization/stabilization_attitude.h"/>
<include name="subsystems/radio_control.h"/>
<include name="subsystems/gps.h"/>
<include name="subsystems/actuators.h"/>
<include name="subsystems/actuators/motor_mixing.h"/>
<!--define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT" cond="ifndef MODE_MANUAL"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD" cond="ifndef MODE_AUTO1"/-->
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
<define name="DLModeNav()" value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
</includes>
<settings>
<dl_setting var="kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
</settings>
<control_block name="set_actuators">
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on)"/>
@@ -10,36 +34,40 @@
</control_block>
<control_block name="attitude_loop">
<call fun="SetAttitudeFromRC(rc_values)"/>
<call fun="AddAttitudeFromFMS()" cond="fms.enabled"/>
<call fun="stabilization_attitude_read_rc(autopilot_in_flight, FALSE, FALSE)"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
</control_block>
<control_block name="throttle_direct">
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_z_sp = stateGetPositionNed_i()->z"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t"/>
</control_block>
<control_block name="altitude_loop">
<call fun="SetAltitudeFromFMS()" cond="fms.enabled"/>
<call fun="gv_update_ref_from_z_sp(guidance_v_z_sp)"/>
<call fun="run_hover_loop(autopilot_in_flight)"/>
<call fun="SaturateThrottle(rc_values)"/>
<!--call fun="SaturateThrottle(rc_values)"/-->
</control_block>
<exceptions>
<exception cond="too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>
<mode name="ATTITUDE" start="stabilization_attitude_enter()">
<select cond="$DEFAULT_MODE"/>
<mode name="ATTITUDE_DIRECT" start="stabilization_attitude_enter()" shortname="ATT">
<select cond="RCMode0()"/>
<control freq="512">
<control>
<call_block name="attitude_loop"/>
<call fun="SetThrottleFromRC(rc_values)"/>
<call_block name="throttle_direct"/>
<call_block name="set_actuators"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="VERTICAL" start="stabilization_attitude_enter()|guidance_v_enter()">
<mode name="ATTITUDE_Z_HOLD" start="stabilization_attitude_enter()|guidance_v_z_enter()" shortname="A_ZH">
<select cond="RCMode1()"/>
<control freq="512">
<control>
<call_block name="attitude_loop"/>
<call_block name="altitude_loop"/>
<call_block name="set_actuators"/>
@@ -47,48 +75,66 @@
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="NAV" start="guidance_h_nav_enter()|guidance_v_enter()">
<select cond="RCMode2()" exception="HOME"/>
<select cond="RCMode2() && DLModeNav()"/>
<control freq="32">
<mode name="NAV" start="guidance_h_nav_enter()|stabilization_attitude_enter()|guidance_v_z_enter()">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<control freq="NAV_FREQ">
<call fun="nav_periodic_task()"/>
</control>
<control freq="512">
<call fun="SetCommandFromAP()"/>
<call fun="GuidanceNavHorizontal()"/>
<call fun="GuidanceNavVertical()"/>
<call fun="AddAttitudeFromRC(rc_values)" cond="!RCLost()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call fun="SaturateThrottle(rc_values)" cond="!RCLost()"/>
<control>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight)"/>
<call fun="guidance_v_from_nav(autopilot_in_flight)"/>
<call fun="guidance_h_from_nav(autopilot_in_flight)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="GPSLost()" deroute="FAILSAFE"/>
<exception cond="GpsIsLost() && autopilot_in_flight" deroute="FAILSAFE"/>
</mode>
<mode name="HOME" start="guidance_h_nav_enter()|guidance_v_enter()">
<control freq="32">
<call fun="nav_home()"/>
</control>
<control freq="512">
<call fun="SetCommandFromAP()"/>
<call fun="GuidanceNavHorizontal()"/>
<call fun="GuidanceNavVertical()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<mode name="GUIDED" start="guidance_h.mode=GUIDANCE_H_MODE_GUIDED|guidance_h_hover_enter()|stabilization_attitude_enter()|guidance_v_mode=GUIDANCE_V_MODE_GUIDED|guidance_v_guided_enter()">
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
<!--control freq="NAV_FREQ">
<call fun="nav_periodic_task()"/>
</control-->
<control>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight)"/>
<call fun="guidance_v_guided_run(autopilot_in_flight)"/>
<call fun="guidance_h_guided_run(autopilot_in_flight)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="GPSLost()" deroute="FAILSAFE"/>
<exception cond="GpsIsLost() && autopilot_in_flight" deroute="FAILSAFE"/>
</mode>
<mode name="HOME" start="guidance_h_nav_enter()|stabilization_attitude_enter()|guidance_v_z_enter()">
<control freq="NAV_FREQ">
<call fun="nav_home()"/>
</control>
<control>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight)"/>
<call fun="guidance_v_from_nav(autopilot_in_flight)"/>
<call fun="guidance_h_from_nav(autopilot_in_flight)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
<!-- Safe landing -->
<mode name="FAILSAFE" start="stabilization_attitude_set_failsafe_setpoint()|guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)|guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)">
<control freq="512">
<call fun="SetFailsafeCommand()"/>
<mode name="FAILSAFE" start="stabilization_attitude_set_failsafe_setpoint()|guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)|guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)" shortname="FAIL">
<control>
<call fun="stabilization_attitude_set_failsafe_setpoint()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp)"/>
<call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="run_hover_loop(autopilot_in_flight)"/>
<call fun="actuators_set(autopilot_motors_on)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
</mode>
<!-- Kill throttle mode -->
<mode name="KILL" start="autopilot_in_flight = false | autopilot_in_flight_counter = 0 | autopilot_set_motors_on(false) | stabilization_cmd[COMMAND_THRUST] = 0">
<select cond="$DEFAULT_MODE"/>
<select cond="kill_switch_is_on()"/>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
<exception cond="!GPSLost()" deroute="$LAST_MODE"/>
</mode>
</state_machine>
conf/firmwares/rotorcraft.makefile
+7
View File
@@ -122,7 +122,14 @@ else
ifneq ($(TARGET), fbw)
$(TARGET).srcs += $(SRC_FIRMWARE)/main.c
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot.c
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_utils.c
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_guided.c
ifeq ($(USE_GENERATED_AUTOPILOT), TRUE)
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_generated.c
$(TARGET).CFLAGS += -DUSE_GENERATED_AUTOPILOT=1
else
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_static.c
endif
else
$(TARGET).srcs += $(SRC_FIRMWARE)/main_fbw.c
endif # TARGET == fbw
sw/airborne/firmwares/rotorcraft/autopilot.c
+55 -407
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/rotorcraft/autopilot.h
+23 -71
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
@@ -22,7 +23,8 @@
/**
* @file firmwares/rotorcraft/autopilot.h
*
* Autopilot modes.
* Core autopilot file.
* Using either static or generated autopilot logic.
*
*/
@@ -32,28 +34,19 @@
#include "std.h"
#include "generated/airframe.h"
#include "state.h"
#include "autopilot_guided.h"
#include "firmwares/rotorcraft/autopilot_utils.h"
#define AP_MODE_KILL 0
#define AP_MODE_FAILSAFE 1
#define AP_MODE_HOME 2
#define AP_MODE_RATE_DIRECT 3
#define AP_MODE_ATTITUDE_DIRECT 4
#define AP_MODE_RATE_RC_CLIMB 5
#define AP_MODE_ATTITUDE_RC_CLIMB 6
#define AP_MODE_ATTITUDE_CLIMB 7
#define AP_MODE_RATE_Z_HOLD 8
#define AP_MODE_ATTITUDE_Z_HOLD 9
#define AP_MODE_HOVER_DIRECT 10
#define AP_MODE_HOVER_CLIMB 11
#define AP_MODE_HOVER_Z_HOLD 12
#define AP_MODE_NAV 13
#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
#define AP_MODE_CARE_FREE_DIRECT 15
#define AP_MODE_FORWARD 16
#define AP_MODE_MODULE 17
#define AP_MODE_FLIP 18
#define AP_MODE_GUIDED 19
// include static or generated autopilot
// static version by default
#ifndef USE_GENERATED_AUTOPILOT
#define USE_GENERATED_AUTOPILOT FALSE
#endif
#if USE_GENERATED_AUTOPILOT
#include "firmwares/rotorcraft/autopilot_generated.h"
#else
#include "firmwares/rotorcraft/autopilot_static.h"
#endif
extern uint8_t autopilot_mode;
extern uint8_t autopilot_mode_auto2;
@@ -61,6 +54,7 @@ extern bool autopilot_motors_on;
extern bool autopilot_in_flight;
extern bool kill_throttle;
extern bool autopilot_rc;
extern uint32_t autopilot_in_flight_counter;
extern bool autopilot_power_switch;
@@ -77,24 +71,11 @@ extern bool autopilot_detect_ground_once;
extern uint16_t autopilot_flight_time;
/** Default RC mode.
*/
#ifndef MODE_MANUAL
#define MODE_MANUAL AP_MODE_ATTITUDE_DIRECT
#endif
#ifndef MODE_AUTO1
#define MODE_AUTO1 AP_MODE_HOVER_Z_HOLD
#endif
#ifndef MODE_AUTO2
#define MODE_AUTO2 AP_MODE_NAV
#endif
#define autopilot_KillThrottle(_kill) { \
if (_kill) \
autopilot_set_motors_on(FALSE); \
autopilot_set_motors_on(false); \
else \
autopilot_set_motors_on(TRUE); \
autopilot_set_motors_on(true); \
}
#ifdef POWER_SWITCH_GPIO
@@ -110,39 +91,6 @@ extern uint16_t autopilot_flight_time;
}
#endif
/** Set Rotorcraft commands.
* Limit thrust and/or yaw depending of the in_flight
* and motors_on flag status
*/
#ifdef ROTORCRAFT_IS_HELI
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#else
#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \
if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#else
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#endif
#endif
/** Z-acceleration threshold to detect ground in m/s^2 */
#ifndef THRESHOLD_GROUND_DETECT
#define THRESHOLD_GROUND_DETECT 25.0
@@ -151,7 +99,11 @@ extern uint16_t autopilot_flight_time;
*/
static inline void DetectGroundEvent(void)
{
if (autopilot_mode == AP_MODE_FAILSAFE || autopilot_detect_ground_once) {
if (autopilot_detect_ground_once
#ifdef AP_MODE_FAILSAFE
|| autopilot_mode == AP_MODE_FAILSAFE
#endif
) {
struct NedCoor_f *accel = stateGetAccelNed_f();
if (accel->z < -THRESHOLD_GROUND_DETECT ||
accel->z > THRESHOLD_GROUND_DETECT) {
sw/airborne/firmwares/rotorcraft/autopilot_generated.c
+153
View File
@@ -0,0 +1,153 @@
/*
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/rotorcraft/autopilot_generated.c
*
* Generated autopilot implementation.
*
*/
#define AUTOPILOT_CORE_AP_C
#include "firmwares/rotorcraft/autopilot_generated.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#include "subsystems/settings.h"
#include "subsystems/datalink/telemetry.h"
#include "generated/settings.h"
#if USE_KILL_SWITCH_FOR_MOTOR_ARMING
#include "autopilot_arming_switch.h"
PRINT_CONFIG_MSG("Using kill switch for motor arming")
#elif USE_THROTTLE_FOR_MOTOR_ARMING
#include "autopilot_arming_throttle.h"
PRINT_CONFIG_MSG("Using throttle for motor arming")
#else
#include "autopilot_arming_yaw.h"
PRINT_CONFIG_MSG("Using 2 sec yaw for motor arming")
#endif
/** Set descent speed in failsafe mode */
#ifndef FAILSAFE_DESCENT_SPEED
#define FAILSAFE_DESCENT_SPEED 1.5
PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
#endif
void autopilot_generated_init(void)
{
// call generated init
autopilot_core_ap_init();
// copy generated mode to public mode (set at startup)
autopilot_mode = autopilot_mode_ap;
// init arming
autopilot_arming_init();
}
void autopilot_generated_periodic(void)
{
autopilot_core_ap_periodic_task();
// copy generated mode to public mode (may be changed on internal exceptions)
autopilot_mode = autopilot_mode_ap;
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight) {
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
}
}
/** AP mode setting handler
*
* FIXME generated something else for this?
*/
void autopilot_generated_SetModeHandler(float mode)
{
autopilot_generated_set_mode(mode);
}
void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
{
autopilot_core_ap_set_mode(new_autopilot_mode);
// copy generated mode to public mode
autopilot_mode = autopilot_mode_ap;
}
void autopilot_generated_set_motors_on(bool motors_on)
{
if (ap_ahrs_is_aligned() && motors_on
#ifdef AP_MODE_KILL
&& autopilot_mode != AP_MODE_KILL
#endif
) {
autopilot_motors_on = true;
} else {
autopilot_motors_on = false;
}
autopilot_arming_set(autopilot_motors_on);
}
void autopilot_generated_on_rc_frame(void)
{
// FIXME what to do here ?
/* an arming sequence is used to start/stop motors.
* only allow arming if ahrs is aligned
*/
if (ap_ahrs_is_aligned()) {
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
}
/* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
// if (autopilot_mode != AP_MODE_FAILSAFE && autopilot_mode != AP_MODE_HOME) {
//
// /* if there are some commands that should always be set from RC, do it */
//#ifdef SetAutoCommandsFromRC
// SetAutoCommandsFromRC(commands, radio_control.values);
//#endif
//
// /* if not in NAV_MODE set commands from the rc */
//#ifdef SetCommandsFromRC
// if (autopilot_mode != AP_MODE_NAV) {
// SetCommandsFromRC(commands, radio_control.values);
// }
//#endif
//
// guidance_v_read_rc();
// guidance_h_read_rc(autopilot_in_flight);
// }
}
sw/airborne/firmwares/rotorcraft/autopilot_generated.h
+47
View File
@@ -0,0 +1,47 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/rotorcraft/autopilot_generated.h
*
* Autopilot generated implementation
* Calls the code generated from autopilot XML file
*
*/
#ifndef AUTOPILOT_GENERATED_H
#define AUTOPILOT_GENERATED_H
#include "std.h"
#include "generated/autopilot_core_ap.h"
#include "generated/airframe.h"
#include "state.h"
extern void autopilot_generated_init(void);
extern void autopilot_generated_periodic(void);
extern void autopilot_generated_on_rc_frame(void);
extern void autopilot_generated_set_mode(uint8_t new_autopilot_mode);
extern void autopilot_generated_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
extern void autopilot_generated_set_motors_on(bool motors_on);
#endif /* AUTOPILOT_GENERATED_H */
sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
+27
View File
@@ -52,6 +52,33 @@
(radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
/** RC mode switch position helper
* switch positions threshold are evenly spaced
*
* @param[in] chan RC mode channel number
* @param[in] pos switch position to be tested
* @param[in] max maximum number of position of the switch
* @return true if current position is the same as requested (and RC status is OK)
*/
static inline bool rc_mode_switch(uint8_t chan, uint8_t pos, uint8_t max)
{
if (radio_control.status != RC_OK) return false;
if (pos >= max) return false;
int32_t v = (int32_t)radio_control.values[chan] - MIN_PPRZ;
// round final value
int32_t p = (((((int32_t)max - 1) * 10 * v) / (MAX_PPRZ - MIN_PPRZ)) + 5) / 10;
Bound(p, 0, max - 1); // just in case
return pos == (uint8_t)p;
}
/** Convenience macro for 3way switch
*/
#ifdef RADIO_MODE
#define RCMode0() rc_mode_switch(RADIO_MODE, 0, 3)
#define RCMode1() rc_mode_switch(RADIO_MODE, 1, 3)
#define RCMode2() rc_mode_switch(RADIO_MODE, 2, 3)
#endif
static inline bool rc_attitude_sticks_centered(void)
{
return ROLL_STICK_CENTERED() && PITCH_STICK_CENTERED() && YAW_STICK_CENTERED();
sw/airborne/firmwares/rotorcraft/autopilot_static.c
+405
View File
@@ -0,0 +1,405 @@
/*
* Copyright (C) 2008-2012 The Paparazzi Team
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/rotorcraft/autopilot_static.c
*
* Static autopilot implementation.
*
*/
#include "firmwares/rotorcraft/autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#include "subsystems/electrical.h"
#include "subsystems/settings.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#if USE_STABILIZATION_RATE
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#endif
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "firmwares/rotorcraft/autopilot_guided.h"
#include "generated/settings.h"
#if USE_GPS
#include "subsystems/gps.h"
#else
#if NO_GPS_NEEDED_FOR_NAV
#define GpsIsLost() FALSE
#else
#define GpsIsLost() TRUE
#endif
#endif
#if USE_KILL_SWITCH_FOR_MOTOR_ARMING
#include "autopilot_arming_switch.h"
PRINT_CONFIG_MSG("Using kill switch for motor arming")
#elif USE_THROTTLE_FOR_MOTOR_ARMING
#include "autopilot_arming_throttle.h"
PRINT_CONFIG_MSG("Using throttle for motor arming")
#else
#include "autopilot_arming_yaw.h"
PRINT_CONFIG_MSG("Using 2 sec yaw for motor arming")
#endif
/* Geofence exceptions */
#include "modules/nav/nav_geofence.h"
/** Set descent speed in failsafe mode */
#ifndef FAILSAFE_DESCENT_SPEED
#define FAILSAFE_DESCENT_SPEED 1.5
PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
#endif
/** Mode that is set when the plane is really too far from home */
#ifndef FAILSAFE_MODE_TOO_FAR_FROM_HOME
#define FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE
#endif
#ifndef MODE_STARTUP
#define MODE_STARTUP AP_MODE_KILL
PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
#endif
#ifndef UNLOCKED_HOME_MODE
#if MODE_AUTO1 == AP_MODE_HOME
#define UNLOCKED_HOME_MODE TRUE
PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO1 is AP_MODE_HOME")
#elif MODE_AUTO2 == AP_MODE_HOME
#define UNLOCKED_HOME_MODE TRUE
PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME")
#else
#define UNLOCKED_HOME_MODE FALSE
#endif
#endif
#if MODE_MANUAL == AP_MODE_NAV
#error "MODE_MANUAL mustn't be AP_MODE_NAV"
#endif
void autopilot_static_init(void)
{
/* mode is finally set at end of init if MODE_STARTUP is not KILL */
autopilot_mode = AP_MODE_KILL; // really needed ?
/* set startup mode, propagates through to guidance h/v */
autopilot_static_set_mode(MODE_STARTUP);
/* init arming */
autopilot_arming_init();
}
#define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)
void autopilot_static_periodic(void)
{
RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
if (dist2_to_home > failsafe_mode_dist2) {
autopilot_static_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
} else {
autopilot_static_set_mode(AP_MODE_HOME);
}
}
}
if (autopilot_mode == AP_MODE_HOME) {
RunOnceEvery(NAV_PRESCALER, nav_home());
} else {
// otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
}
/* If in FAILSAFE mode and either already not in_flight anymore
* or just "detected" ground, go to KILL mode.
*/
if (autopilot_mode == AP_MODE_FAILSAFE) {
if (!autopilot_in_flight) {
autopilot_static_set_mode(AP_MODE_KILL);
}
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot_ground_detected) {
autopilot_static_set_mode(AP_MODE_KILL);
}
#endif
}
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight) {
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
}
/* Set fixed "failsafe" commands from airframe file if in KILL mode.
* If in FAILSAFE mode, run normal loops with failsafe attitude and
* downwards velocity setpoints.
*/
if (autopilot_mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
} else {
guidance_v_run(autopilot_in_flight);
guidance_h_run(autopilot_in_flight);
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
/** AP mode setting handler
*
* Checks RC status before calling autopilot_static_set_mode function
*/
void autopilot_static_SetModeHandler(float mode)
{
if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
// safety modes are always accessible via settings
autopilot_static_set_mode(mode);
} else {
if (radio_control.status != RC_OK &&
(mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
// without RC, only nav-like modes are accessible
autopilot_static_set_mode(mode);
}
}
// with RC, other modes can only be changed from the RC
}
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
{
/* force startup mode (default is kill) as long as AHRS is not aligned */
if (!ap_ahrs_is_aligned()) {
new_autopilot_mode = MODE_STARTUP;
}
if (new_autopilot_mode != autopilot_mode) {
/* horizontal mode */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
stabilization_attitude_set_failsafe_setpoint();
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
#endif
case AP_MODE_KILL:
autopilot_in_flight = false;
autopilot_in_flight_counter = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_Z_HOLD:
#if USE_STABILIZATION_RATE
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
#else
return;
#endif
break;
case AP_MODE_ATTITUDE_RC_CLIMB:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
case AP_MODE_FORWARD:
guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
break;
case AP_MODE_CARE_FREE_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
break;
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
break;
case AP_MODE_HOME:
case AP_MODE_NAV:
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
case AP_MODE_MODULE:
#ifdef GUIDANCE_H_MODE_MODULE_SETTING
guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
#endif
break;
case AP_MODE_FLIP:
guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP);
break;
case AP_MODE_GUIDED:
guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED);
break;
default:
break;
}
/* vertical mode */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED);
break;
#endif
case AP_MODE_KILL:
autopilot_set_motors_on(FALSE);
stabilization_cmd[COMMAND_THRUST] = 0;
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_CARE_FREE_DIRECT:
case AP_MODE_FORWARD:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_ATTITUDE_RC_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
break;
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_HOVER_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_HOVER_Z_HOLD:
guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
break;
case AP_MODE_HOME:
case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
case AP_MODE_MODULE:
#ifdef GUIDANCE_V_MODE_MODULE_SETTING
guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
#endif
break;
case AP_MODE_FLIP:
guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP);
break;
case AP_MODE_GUIDED:
guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED);
break;
default:
break;
}
//if switching to rate mode but rate mode is not defined, the function returned
autopilot_mode = new_autopilot_mode;
}
}
void autopilot_static_set_motors_on(bool motors_on)
{
if (autopilot_mode != AP_MODE_KILL && ap_ahrs_is_aligned() && motors_on) {
autopilot_motors_on = true;
} else {
autopilot_motors_on = false;
}
autopilot_arming_set(autopilot_motors_on);
}
void autopilot_static_on_rc_frame(void)
{
if (kill_switch_is_on()) {
autopilot_static_set_mode(AP_MODE_KILL);
} else {
#ifdef RADIO_AUTO_MODE
INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
uint8_t new_autopilot_mode = ap_mode_of_two_switches();
#else
#ifdef RADIO_MODE_2x3
uint8_t new_autopilot_mode = ap_mode_of_3x2way_switch();
#else
uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
#endif
#endif
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
/* always allow to switch to manual */
if (new_autopilot_mode == MODE_MANUAL) {
autopilot_static_set_mode(new_autopilot_mode);
}
/* if in HOME mode, don't allow switching to non-manual modes */
else if ((autopilot_mode != AP_MODE_HOME)
#if UNLOCKED_HOME_MODE
/* Allowed to leave home mode when UNLOCKED_HOME_MODE */
|| !too_far_from_home
#endif
) {
autopilot_static_set_mode(new_autopilot_mode);
}
}
}
/* an arming sequence is used to start/stop motors.
* only allow arming if ahrs is aligned
*/
if (ap_ahrs_is_aligned()) {
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
}
/* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
if (autopilot_mode != AP_MODE_FAILSAFE && autopilot_mode != AP_MODE_HOME) {
/* if there are some commands that should always be set from RC, do it */
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(commands, radio_control.values);
#endif
/* if not in NAV_MODE set commands from the rc */
#ifdef SetCommandsFromRC
if (autopilot_mode != AP_MODE_NAV) {
SetCommandsFromRC(commands, radio_control.values);
}
#endif
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
}
sw/airborne/firmwares/rotorcraft/autopilot_static.h
+77
View File
@@ -0,0 +1,77 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/rotorcraft/autopilot_static.h
*
* Autopilot static implementation
*
*/
#ifndef AUTOPILOT_STATIC_H
#define AUTOPILOT_STATIC_H
/** Static autopilot modes
*/
#define AP_MODE_KILL 0
#define AP_MODE_FAILSAFE 1
#define AP_MODE_HOME 2
#define AP_MODE_RATE_DIRECT 3
#define AP_MODE_ATTITUDE_DIRECT 4
#define AP_MODE_RATE_RC_CLIMB 5
#define AP_MODE_ATTITUDE_RC_CLIMB 6
#define AP_MODE_ATTITUDE_CLIMB 7
#define AP_MODE_RATE_Z_HOLD 8
#define AP_MODE_ATTITUDE_Z_HOLD 9
#define AP_MODE_HOVER_DIRECT 10
#define AP_MODE_HOVER_CLIMB 11
#define AP_MODE_HOVER_Z_HOLD 12
#define AP_MODE_NAV 13
#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
#define AP_MODE_CARE_FREE_DIRECT 15
#define AP_MODE_FORWARD 16
#define AP_MODE_MODULE 17
#define AP_MODE_FLIP 18
#define AP_MODE_GUIDED 19
/** Default RC mode.
*/
#ifndef MODE_MANUAL
#define MODE_MANUAL AP_MODE_ATTITUDE_DIRECT
#endif
#ifndef MODE_AUTO1
#define MODE_AUTO1 AP_MODE_HOVER_Z_HOLD
#endif
#ifndef MODE_AUTO2
#define MODE_AUTO2 AP_MODE_NAV
#endif
/** Specific function for static AP
*/
extern void autopilot_static_init(void);
extern void autopilot_static_periodic(void);
extern void autopilot_static_on_rc_frame(void);
extern void autopilot_static_set_mode(uint8_t new_autopilot_mode);
extern void autopilot_static_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
extern void autopilot_static_set_motors_on(bool motors_on);
#endif /* AUTOPILOT_STATIC_H */
sw/airborne/firmwares/rotorcraft/autopilot_utils.c
+127
View File
@@ -0,0 +1,127 @@
/*
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/rotorcraft/autopilot_utils.c
*
* Utility functions and includes for autopilots
*
*/
#include "firmwares/rotorcraft/autopilot_utils.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "state.h"
#include "subsystems/radio_control.h"
// Utility functions
#ifndef AUTOPILOT_DISABLE_AHRS_KILL
bool ap_ahrs_is_aligned(void)
{
return stateIsAttitudeValid();
}
#else
PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
bool ap_ahrs_is_aligned(void)
{
return true;
}
#endif
#if defined RADIO_MODE_2x3
#define THRESHOLD_1d3_PPRZ (MAX_PPRZ / 3)
#define THRESHOLD_2d3_PPRZ ((MAX_PPRZ / 3) * 2)
/** Get autopilot mode as set by a RADIO_MODE 3-way switch and a 2-way switch, which are mixed together
* The 2 way switch negates the value, the 3 way switch changes in three steps from 0 - MAX_PPRZ.
* E.g. SW_1 has two positions (On/Off), SW_Mode has three positions (M1/M2/M3)
* 1 Mode value
* Off M1 -9500
* Off M2 -4800
* Off M3 -1850
* On M1 2100
* On M2 4900
* On M3 9600
* This function filters out the effect of SW_1, such that a normal 3-way switch comes out.
**/
uint8_t ap_mode_of_3x2way_switch(void)
{
int val = radio_control.values[RADIO_MODE];
if (radio_control.values[RADIO_MODE] < 0) {
val = MAX_PPRZ + val;
}
if (val < THRESHOLD_1d3_PPRZ) {
return MODE_MANUAL;
} else if (val < THRESHOLD_2d3_PPRZ) {
return MODE_AUTO1;
} else {
return autopilot_mode_auto2;
}
}
#else
#define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
#define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
/** get autopilot mode as set by RADIO_MODE 3-way switch */
uint8_t ap_mode_of_3way_switch(void)
{
if (radio_control.values[RADIO_MODE] > THRESHOLD_2_PPRZ) {
return autopilot_mode_auto2;
} else if (radio_control.values[RADIO_MODE] > THRESHOLD_1_PPRZ) {
return MODE_AUTO1;
} else {
return MODE_MANUAL;
}
}
#endif
/**
* Get autopilot mode from two 2way switches.
* RADIO_MODE switch just selectes between MANUAL and AUTO.
* If not MANUAL, the RADIO_AUTO_MODE switch selects between AUTO1 and AUTO2.
*
* This is mainly a cludge for entry level radios with no three-way switch,
* but two available two-way switches which can be used.
*/
#if defined RADIO_AUTO_MODE || defined(__DOXYGEN__)
uint8_t ap_mode_of_two_switches(void)
{
if (radio_control.values[RADIO_MODE] < THRESHOLD_1_PPRZ) {
/* RADIO_MODE in MANUAL position */
return MODE_MANUAL;
} else {
/* RADIO_MODE not in MANUAL position.
* Select AUTO mode bassed on RADIO_AUTO_MODE channel
*/
if (radio_control.values[RADIO_AUTO_MODE] > THRESHOLD_2_PPRZ) {
return autopilot_mode_auto2;
} else {
return MODE_AUTO1;
}
}
}
#endif
sw/airborne/firmwares/rotorcraft/autopilot_utils.h
+84
View File
@@ -0,0 +1,84 @@
/*
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/rotorcraft/autopilot_utils.h
*
* Utility functions and includes for autopilots
*
*/
#ifndef AUTOPILOT_UTILS_H
#define AUTOPILOT_UTILS_H
#include "std.h"
#include "subsystems/commands.h"
/** Set descent speed in failsafe mode */
#ifndef FAILSAFE_DESCENT_SPEED
#define FAILSAFE_DESCENT_SPEED 1.5
#endif
extern bool ap_ahrs_is_aligned(void);
#if defined RADIO_MODE_2x3
extern uint8_t ap_mode_of_3x2way_switch(void);
#else
extern uint8_t ap_mode_of_3way_switch(void);
#endif
#if defined RADIO_AUTO_MODE || defined(__DOXYGEN__)
extern uint8_t ap_mode_of_two_switches(void)
#endif
/** Set Rotorcraft commands.
* Limit thrust and/or yaw depending of the in_flight
* and motors_on flag status
*/
#ifdef ROTORCRAFT_IS_HELI
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#else
#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \
if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#else
#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
}
#endif
#endif
#endif // AUTOPILOT_UTILS_H
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+76 -67
View File
@@ -99,8 +99,6 @@ static void guidance_h_update_reference(void);
#if !GUIDANCE_INDI
static void guidance_h_traj_run(bool in_flight);
#endif
static void guidance_h_hover_enter(void);
static void guidance_h_nav_enter(void);
static inline void transition_run(void);
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight);
@@ -391,72 +389,11 @@ void guidance_h_run(bool in_flight)
/* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
case GUIDANCE_H_MODE_GUIDED:
/* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
if (!in_flight) {
guidance_h_hover_enter();
}
guidance_h_update_reference();
#if GUIDANCE_INDI
guidance_indi_run(in_flight, guidance_h.sp.heading);
#else
/* compute x,y earth commands */
guidance_h_traj_run(in_flight);
/* set final attitude setpoint */
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading);
#endif
stabilization_attitude_run(in_flight);
guidance_h_guided_run(in_flight);
break;
case GUIDANCE_H_MODE_NAV:
if (!in_flight) {
guidance_h_nav_enter();
}
if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
} else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
struct Int32Eulers sp_cmd_i;
sp_cmd_i.phi = nav_roll;
sp_cmd_i.theta = nav_pitch;
sp_cmd_i.psi = nav_heading;
stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
stabilization_attitude_run(in_flight);
#if HYBRID_NAVIGATION
//make sure the heading is right before leaving horizontal_mode attitude
guidance_hybrid_reset_heading(&sp_cmd_i);
#endif
} else {
#if HYBRID_NAVIGATION
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
guidance_hybrid_run();
#else
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
guidance_h_update_reference();
/* set psi command */
guidance_h.sp.heading = nav_heading;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
#if GUIDANCE_INDI
guidance_indi_run(in_flight, guidance_h.sp.heading);
#else
/* compute x,y earth commands */
guidance_h_traj_run(in_flight);
/* set final attitude setpoint */
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
guidance_h.sp.heading);
#endif
#endif
stabilization_attitude_run(in_flight);
}
guidance_h_from_nav(in_flight);
break;
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
@@ -593,7 +530,7 @@ static void guidance_h_traj_run(bool in_flight)
}
#endif
static void guidance_h_hover_enter(void)
void guidance_h_hover_enter(void)
{
/* reset speed setting */
guidance_h.sp.speed.x = 0;
@@ -616,7 +553,7 @@ static void guidance_h_hover_enter(void)
guidance_h.sp.heading = guidance_h.rc_sp.psi;
}
static void guidance_h_nav_enter(void)
void guidance_h_nav_enter(void)
{
ClearBit(guidance_h.sp.mask, 5);
ClearBit(guidance_h.sp.mask, 7);
@@ -629,6 +566,57 @@ static void guidance_h_nav_enter(void)
nav_heading = stateGetNedToBodyEulers_i()->psi;
}
void guidance_h_from_nav(bool in_flight)
{
if (!in_flight) {
guidance_h_nav_enter();
}
if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
} else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
struct Int32Eulers sp_cmd_i;
sp_cmd_i.phi = nav_roll;
sp_cmd_i.theta = nav_pitch;
sp_cmd_i.psi = nav_heading;
stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
stabilization_attitude_run(in_flight);
#if HYBRID_NAVIGATION
//make sure the heading is right before leaving horizontal_mode attitude
guidance_hybrid_reset_heading(&sp_cmd_i);
#endif
} else {
#if HYBRID_NAVIGATION
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
guidance_hybrid_run();
#else
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
guidance_h_update_reference();
/* set psi command */
guidance_h.sp.heading = nav_heading;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
#if GUIDANCE_INDI
guidance_indi_run(in_flight, guidance_h.sp.heading);
#else
/* compute x,y earth commands */
guidance_h_traj_run(in_flight);
/* set final attitude setpoint */
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
guidance_h.sp.heading);
#endif
#endif
stabilization_attitude_run(in_flight);
}
}
static inline void transition_run(void)
{
//Add 0.00625%
@@ -678,6 +666,27 @@ void guidance_h_set_igain(uint32_t igain)
INT_VECT2_ZERO(guidance_h_trim_att_integrator);
}
void guidance_h_guided_run(bool in_flight)
{
/* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
if (!in_flight) {
guidance_h_hover_enter();
}
guidance_h_update_reference();
#if GUIDANCE_INDI
guidance_indi_run(in_flight, guidance_h.sp.heading);
#else
/* compute x,y earth commands */
guidance_h_traj_run(in_flight);
/* set final attitude setpoint */
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading);
#endif
stabilization_attitude_run(in_flight);
}
bool guidance_h_set_guided_pos(float x, float y)
{
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+11
View File
@@ -112,8 +112,19 @@ extern void guidance_h_mode_changed(uint8_t new_mode);
extern void guidance_h_read_rc(bool in_flight);
extern void guidance_h_run(bool in_flight);
extern void guidance_h_hover_enter(void);
extern void guidance_h_nav_enter(void);
/** Set horizontal guidance from NAV and run control loop
*/
extern void guidance_h_from_nav(bool in_flight);
extern void guidance_h_set_igain(uint32_t igain);
/** Run GUIDED mode control
*/
extern void guidance_h_guided_run(bool in_flight);
/** Set horizontal position setpoint in GUIDED mode.
* @param x North position (local NED frame) in meters.
* @param y East position (local NED frame) in meters.
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+90 -78
View File
@@ -138,15 +138,7 @@ int32_t guidance_v_z_sum_err;
int32_t guidance_v_thrust_coeff;
#define GuidanceVSetRef(_pos, _speed, _accel) { \
gv_set_ref(_pos, _speed, _accel); \
guidance_v_z_ref = _pos; \
guidance_v_zd_ref = _speed; \
guidance_v_zdd_ref = _accel; \
}
static int32_t get_vertical_thrust_coeff(void);
static void run_hover_loop(bool in_flight);
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -194,6 +186,8 @@ void guidance_v_init(void)
guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
guidance_v_thrust_coeff = BFP_OF_REAL(1.f, INT32_TRIG_FRAC);
gv_adapt_init();
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
@@ -239,16 +233,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
switch (new_mode) {
case GUIDANCE_V_MODE_GUIDED:
case GUIDANCE_V_MODE_HOVER:
/* disable vertical velocity setpoints */
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
/* set current altitude as setpoint and reset speed setpoint */
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = 0;
/* reset guidance reference */
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
guidance_v_guided_enter();
break;
case GUIDANCE_V_MODE_RC_CLIMB:
@@ -284,10 +269,8 @@ void guidance_v_notify_in_flight(bool in_flight)
}
}
void guidance_v_run(bool in_flight)
void guidance_v_thrust_adapt(bool in_flight)
{
// FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co
guidance_v_thrust_coeff = get_vertical_thrust_coeff();
@@ -298,6 +281,11 @@ void guidance_v_run(bool in_flight)
/* reset estimate while not in_flight */
gv_adapt_init();
}
}
void guidance_v_run(bool in_flight)
{
guidance_v_thrust_adapt(in_flight);
switch (guidance_v_mode) {
@@ -328,34 +316,7 @@ void guidance_v_run(bool in_flight)
case GUIDANCE_V_MODE_HOVER:
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
case GUIDANCE_V_MODE_GUIDED:
switch(guidance_v_guided_mode)
{
case GUIDANCE_V_GUIDED_MODE_ZHOLD:
// Altitude Hold
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
break;
case GUIDANCE_V_GUIDED_MODE_CLIMB:
// Climb
gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
run_hover_loop(in_flight);
break;
case GUIDANCE_V_GUIDED_MODE_THROTTLE:
// Throttle
guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
stabilization_cmd[COMMAND_THRUST] = guidance_v_th_sp;
break;
default:
break;
}
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
guidance_v_guided_run(in_flight);
break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
@@ -365,34 +326,7 @@ void guidance_v_run(bool in_flight)
#endif
case GUIDANCE_V_MODE_NAV: {
if (vertical_mode == VERTICAL_MODE_ALT) {
guidance_v_z_sp = -nav_flight_altitude;
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
} else if (vertical_mode == VERTICAL_MODE_CLIMB) {
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = -nav_climb;
gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
run_hover_loop(in_flight);
} else if (vertical_mode == VERTICAL_MODE_MANUAL) {
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = stateGetSpeedNed_i()->z;
GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
guidance_v_z_sum_err = 0;
guidance_v_delta_t = nav_throttle;
}
#if HYBRID_NAVIGATION
guidance_hybrid_vertical();
#else
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
#endif
guidance_v_from_nav(in_flight);
break;
}
@@ -435,7 +369,7 @@ static int32_t get_vertical_thrust_coeff(void)
#define FF_CMD_FRAC 18
static void run_hover_loop(bool in_flight)
void run_hover_loop(bool in_flight)
{
/* convert our reference to generic representation */
@@ -494,6 +428,84 @@ static void run_hover_loop(bool in_flight)
}
void guidance_v_from_nav(bool in_flight)
{
if (vertical_mode == VERTICAL_MODE_ALT) {
guidance_v_z_sp = -nav_flight_altitude;
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
} else if (vertical_mode == VERTICAL_MODE_CLIMB) {
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = -nav_climb;
gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
run_hover_loop(in_flight);
} else if (vertical_mode == VERTICAL_MODE_MANUAL) {
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = stateGetSpeedNed_i()->z;
GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
guidance_v_z_sum_err = 0;
guidance_v_delta_t = nav_throttle;
}
#if HYBRID_NAVIGATION
guidance_hybrid_vertical();
#else
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
#endif
}
void guidance_v_guided_enter(void)
{
/* disable vertical velocity setpoints */
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
/* set current altitude as setpoint and reset speed setpoint */
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = 0;
/* reset guidance reference */
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
}
void guidance_v_guided_run(bool in_flight)
{
switch(guidance_v_guided_mode)
{
case GUIDANCE_V_GUIDED_MODE_ZHOLD:
// Altitude Hold
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
break;
case GUIDANCE_V_GUIDED_MODE_CLIMB:
// Climb
gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
run_hover_loop(in_flight);
break;
case GUIDANCE_V_GUIDED_MODE_THROTTLE:
// Throttle
guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
stabilization_cmd[COMMAND_THRUST] = guidance_v_th_sp;
break;
default:
break;
}
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
}
bool guidance_v_set_guided_z(float z)
{
if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+41 -2
View File
@@ -78,6 +78,11 @@ extern int32_t guidance_v_z_sum_err; ///< accumulator for I-gain
extern int32_t guidance_v_ff_cmd; ///< feed-forward command
extern int32_t guidance_v_fb_cmd; ///< feed-back command
/** Direct throttle from radio control.
* range 0:#MAX_PPRZ
*/
extern int32_t guidance_v_rc_delta_t;
/** thrust command.
* summation of feed-forward and feed-back commands,
* valid range 0 : #MAX_PPRZ
@@ -94,8 +99,6 @@ extern float guidance_v_nominal_throttle;
*/
extern bool guidance_v_adapt_throttle_enabled;
extern bool guidance_v_guided_vel_enabled;
extern int32_t guidance_v_thrust_coeff;
extern int32_t guidance_v_kp; ///< vertical control P-gain
@@ -105,9 +108,24 @@ extern int32_t guidance_v_ki; ///< vertical control I-gain
extern void guidance_v_init(void);
extern void guidance_v_read_rc(void);
extern void guidance_v_mode_changed(uint8_t new_mode);
extern void guidance_v_thrust_adapt(bool in_flight);
extern void guidance_v_notify_in_flight(bool in_flight);
extern void guidance_v_run(bool in_flight);
extern void run_hover_loop(bool in_flight);
/** Set guidance setpoint from NAV and run hover loop
*/
extern void guidance_v_from_nav(bool in_flight);
/** Enter GUIDED mode control
*/
extern void guidance_v_guided_enter(void);
/** Run GUIDED mode control
*/
extern void guidance_v_guided_run(bool in_flight);
/** Set z setpoint in GUIDED mode.
* @param z Setpoint (down is positive) in meters.
* @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)
@@ -127,4 +145,25 @@ extern bool guidance_v_set_guided_th(float th);
guidance_v_z_sum_err = 0; \
}
#define GuidanceVSetRef(_pos, _speed, _accel) { \
gv_set_ref(_pos, _speed, _accel); \
guidance_v_z_ref = _pos; \
guidance_v_zd_ref = _speed; \
guidance_v_zdd_ref = _accel; \
}
#include "state.h"
static inline void guidance_v_z_enter(void)
{
/* set current altitude as setpoint */
guidance_v_z_sp = stateGetPositionNed_i()->z;
/* reset guidance reference */
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
/* reset speed setting */
guidance_v_zd_sp = 0;
}
#endif /* GUIDANCE_V_H */
sw/airborne/firmwares/rotorcraft/main.c
+3
View File
@@ -331,6 +331,7 @@ STATIC_INLINE void telemetry_periodic(void)
STATIC_INLINE void failsafe_check(void)
{
#if !USE_GENERATED_AUTOPILOT
if (radio_control.status == RC_REALLY_LOST &&
autopilot_mode != AP_MODE_KILL &&
autopilot_mode != AP_MODE_HOME &&
@@ -365,6 +366,8 @@ STATIC_INLINE void failsafe_check(void)
}
#endif
#endif // !USE_GENERATED_AUTOPILOT
autopilot_check_in_flight(autopilot_motors_on);
}
sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
+2
View File
@@ -70,6 +70,7 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct
break;
#endif /* USE_NAVIGATION */
#ifdef AP_MODE_GUIDED
case DL_GUIDED_SETPOINT_NED:
if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { break; }
@@ -79,6 +80,7 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct
DL_GUIDED_SETPOINT_NED_z(buf),
DL_GUIDED_SETPOINT_NED_yaw(buf));
break;
#endif
default:
break;
sw/ground_segment/tmtc/aircraft.ml
+12 -2
View File
@@ -193,7 +193,8 @@ type aircraft = {
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
inflight_calib : inflight_calib
inflight_calib : inflight_calib;
mutable ap_modes : string array option
}
let max_nb_dl_setting_values = 256 (** indexed iwth an uint8 (messages.xml) *)
@@ -225,5 +226,14 @@ let new_aircraft = fun id name fp airframe ->
waypoints = Hashtbl.create 3; survey = None; last_msg_date = 0.; dist_to_wp = 0.;
datalink_status = datalink_status_init (); link_status = Hashtbl.create 1;
time_since_last_survey_msg = 1729.;
inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.}
inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
ap_modes = None
}
let modes_of_aircraft = fun ac ->
match ac.ap_modes, ac.vehicle_type with
| Some m, _ -> m
| None, FixedWing -> Server_globals.fixedwing_ap_modes
| None, Rotorcraft -> Server_globals.rotorcraft_ap_modes
| None, _ -> [| "UKN" |]
sw/ground_segment/tmtc/aircraft.mli
+3 -1
View File
@@ -144,8 +144,10 @@ type aircraft = {
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
inflight_calib : inflight_calib
inflight_calib : inflight_calib;
mutable ap_modes : string array option
}
val new_aircraft : string -> string -> Xml.xml -> Xml.xml -> aircraft
val max_nb_dl_setting_values : int
val modes_of_aircraft : aircraft -> string array
sw/ground_segment/tmtc/fw_server.ml
+2 -2
View File
@@ -246,7 +246,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
if a.fbw.rc_mode = "FAILSAFE" then
a.ap_mode <- 5 (* Override and set FAIL(Safe) Mode *)
else
a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE"
a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE"
| "CAM" ->
a.cam.phi <- (Deg>>Rad) (fvalue "phi");
a.cam.theta <- (Deg>>Rad) (fvalue "theta");
@@ -337,7 +337,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.bat <- fvalue "vsupply" /. 10.;
a.energy <- ivalue "energy" * 100;
a.throttle <- fvalue "throttle";
a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE";
a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE";
a.cur_block <- ivalue "nav_block";
end
| "FORMATION_SLOT_TM" ->
sw/ground_segment/tmtc/rotorcraft_server.ml
+1 -1
View File
@@ -199,7 +199,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
a.fbw.rc_rate <- ivalue "frame_rate";
a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE";
a.ap_mode <- check_index (ivalue "ap_mode") rotorcraft_ap_modes "ROTORCRAFT_AP_MODE";
a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "ROTORCRAFT_AP_MODE";
a.kill_mode <- ivalue "ap_motors_on" == 0;
a.bat <- fvalue "vsupply" /. 10.
| "STATE_FILTER_STATUS" ->
sw/ground_segment/tmtc/server.ml
+6 -7
View File
@@ -52,12 +52,6 @@ let srtm_path = Env.paparazzi_home // "data" // "srtm"
let get_indexed_value = fun t i ->
if i >= 0 then t.(i) else "UNK"
let modes_of_type = fun vt ->
match vt with
FixedWing -> fixedwing_ap_modes
| Rotorcraft -> rotorcraft_ap_modes
| UnknownVehicleType -> [| |]
(** The aircrafts store *)
let aircrafts = Hashtbl.create 3
@@ -422,7 +416,7 @@ let send_aircraft_msg = fun ac ->
"energy", PprzLink.Int a.energy] in
Ground_Pprz.message_send my_id "ENGINE_STATUS" values;
let ap_mode = get_indexed_value (modes_of_type a.vehicle_type) a.ap_mode in
let ap_mode = get_indexed_value (modes_of_aircraft a) a.ap_mode in
let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
let lat_mode = get_indexed_value lat_modes a.lateral_mode in
let horiz_mode = get_indexed_value horiz_modes a.horizontal_mode in
@@ -545,6 +539,11 @@ let new_aircraft = fun get_alive_md5sum real_id ->
ac.svinfo.(i).age <- ac.svinfo.(i).age + 1;
done in
ignore (ac.ap_modes <- try
let (ap_file, _) = Gen_common.get_autopilot_of_airframe airframe_xml in
Some (modes_from_autopilot (ExtXml.parse_file ap_file))
with _ -> None);
ignore (Glib.Timeout.add 1000 (fun _ -> update (); true));
let messages_xml = ExtXml.parse_file (Env.paparazzi_home // root_dir // "var" // "messages.xml") in
sw/ground_segment/tmtc/server_globals.ml
+11
View File
@@ -20,3 +20,14 @@ let if_modes = [|"OFF";"DOWN";"UP"|]
let string_of_values = fun values ->
Compat.bytes_concat " " (List.map (fun (_, v) -> PprzLink.string_of_value v) values)
(** get modes from autopilot xml file *)
let modes_from_autopilot = fun ap_xml ->
let ap = ExtXml.child ap_xml
~select:(fun x -> String.uppercase (ExtXml.attrib_or_default x "gcs_mode" "") = "TRUE")
"state_machine"
in
let modes = List.filter (fun x -> Xml.tag x = "mode") (Xml.children ap) in
let l = List.map (fun x -> try Xml.attrib x "shortname" with _ -> ExtXml.attrib x "name") modes in
Array.of_list l
sw/tools/generators/gen_autopilot.ml
+50 -18
View File
@@ -59,6 +59,10 @@ let get_includes = fun sm ->
try ExtXml.child sm "includes"
with _ -> Xml.Element ("includes", [], [])
let get_settings = fun sm ->
try ExtXml.child sm "settings"
with _ -> Xml.Element ("settings", [], [])
let has_modules = fun sm ->
try
let m = ExtXml.child sm "modules" in
@@ -70,8 +74,22 @@ let get_mode_exceptions = fun mode ->
let print_includes = fun includes out_h ->
List.iter (fun i ->
let name = Xml.attrib i "name" in
lprintf out_h "#include \"%s\"\n" name
match Xml.tag i with
| "include" ->
let name = ExtXml.attrib i "name" in
lprintf out_h "#include \"%s\"\n" name
| "define" ->
let name = ExtXml.attrib i "name"
and value = ExtXml.attrib i "value"
and cond = try Some (Xml.attrib i "cond") with _ -> None in
begin match cond with
| None -> lprintf out_h "#define %s %s\n" name value
| Some c ->
lprintf out_h "#%s\n" c;
lprintf out_h "#define %s %s\n" name value;
lprintf out_h "#endif\n"
end
| _ -> failwith "[gen_autopilot] Unknown includes type"
) (Xml.children includes)
let print_mode_name = fun sm_name name ->
@@ -141,8 +159,8 @@ let print_test_exception = fun modes name out_h ->
let print_exception = fun ex ->
let deroute = Xml.attrib ex "deroute"
and cond = Xml.attrib ex "cond" in
match name with
"$LAST_MODE" -> lprintf out_h "if (%s) { return last_autopilot_mode_%s; }\n" cond name
match deroute with
| "$LAST_MODE" -> lprintf out_h "if (%s) { return last_autopilot_mode_%s; }\n" cond name
| _ -> lprintf out_h "if (%s) { return %s; }\n" cond (print_mode_name name deroute)
in
@@ -338,11 +356,11 @@ let parse_and_gen_modes xml_file ap_name main_freq h_dir sm =
(* Print mode names and variable *)
print_modes modes name_up out_h;
fprintf out_h "\nEXTERN_%s uint8_t autopilot_mode_%s;\n" name_up name;
fprintf out_h "\n#ifdef AUTOPILOT_CORE_%s_C\n" name_up;
fprintf out_h "\n#ifdef AUTOPILOT_CORE_%s_C\n\n" name_up;
(* Print includes and private variables *)
print_includes (get_includes sm) out_h;
if has_modules sm then fprintf out_h "\n#include \"modules.h\"\n";
fprintf out_h "uint8_t private_autopilot_mode_%s;\n" name;
fprintf out_h "\nuint8_t private_autopilot_mode_%s;\n" name;
fprintf out_h "uint8_t last_autopilot_mode_%s;\n\n" name;
(* Print functions *)
print_ap_init modes name out_h;
@@ -402,12 +420,24 @@ let write_settings = fun xml_file out_set ap ->
(current + 1, min, max, values @ n)
end
) (0, None, None, []) modes in
(* check handler *)
let handler = try
let sh = Xml.attrib sm "settings_handler" in
let s = Str.split (Str.regexp "|") sh in
match s with
| [m; h] -> sprintf " module=\"%s\" handler=\"%s\"" m h
| _ -> failwith "invalid handler format"
with _ -> "" in
(* Print if at least one mode has been found *)
match min, max with
begin match min, max with
| Some min_idx, Some max_idx ->
fprintf out_set " <dl_setting min=\"%d\" max=\"%d\" step=\"1\" var=\"autopilot_mode_%s\" shortname=\"%s\" values=\"%s\"/>\n"
min_idx max_idx name name (String.concat "|" values)
fprintf out_set " <dl_setting min=\"%d\" max=\"%d\" step=\"1\" var=\"autopilot_mode_%s\" shortname=\"%s\" values=\"%s\"%s/>\n"
min_idx max_idx name name (String.concat "|" values) handler
| _, _ -> ()
end;
(* check for embedded custom settings *)
let extra_settings = get_settings sm in
List.iter (fun s -> fprintf out_set " %s\n" (Xml.to_string s)) (Xml.children extra_settings)
in
(* Iter and call print function *)
List.iter write_ap_mode sm_filtered;
@@ -422,17 +452,18 @@ let write_settings = fun xml_file out_set ap ->
* Usage: main_freq xml_file_input h_dir_output
*)
let gen_autopilot main_freq xml_file h_dir out_set =
try
let ap_xml = Xml.parse_file xml_file in
let ap_name = ExtXml.attrib_or_default ap_xml "name" "Autopilot" in
let state_machines = get_state_machines ap_xml in
List.iter (parse_and_gen_modes xml_file ap_name main_freq h_dir) state_machines;
write_settings xml_file out_set ap_xml
with
Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1
let ap_xml = try Xml.parse_file xml_file with
| Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1
| Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.prove_error e); exit 1
| Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.check_error e); exit 1
| Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.parse_error e); exit 1
| Xml.File_not_found _ -> (* invalid file, return empty Xml *)
Xml.Element ("autopilot", [], [])
in
let ap_name = ExtXml.attrib_or_default ap_xml "name" "Autopilot" in
let state_machines = get_state_machines ap_xml in
List.iter (parse_and_gen_modes xml_file ap_name main_freq h_dir) state_machines;
write_settings xml_file out_set ap_xml
(* Main call *)
let () =
@@ -448,7 +479,8 @@ let () =
| Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.prove_error e); exit 1
| Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.check_error e); exit 1
| Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.parse_error e); exit 1
| Not_found -> exit 0 (* No autopilot file found *)
| Not_found -> (* No autopilot file found *)
("", None)
in
try
gen_autopilot ap_freq autopilot h_dir out_set;