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

*** empty log message ***

Browse Source
This commit is contained in:
Pascal Brisset
2007-02-15 10:29:17 +00:00
parent 69089a7f5c
commit ac826cfd91
9 changed files with 223 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/microjet6.xml
+174 -90
View File
@@ -1,27 +1,32 @@
<airframe name="Microjet Tiny 1.1">
<!DOCTYPE airframe SYSTEM "airframe.dtd">
<!-- rc 41.050 -->
<airframe name="Microjet6 Tiny 1.1">
<!-- commands section -->
<servos>
<servo name="GAZ" no="2" min="1250" neutral="1250" max="1850"/>
<servo name="AILEVON_LEFT" no="1" min="2000" neutral="1600" max="1000"/>
<servo name="AILEVON_RIGHT" no="0" min="1000" neutral="1400" max="2000"/>
<servo name="GAZ" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILEVON_LEFT" no="6" min="1075" neutral="1480" max="2025"/>
<servo name="AILEVON_RIGHT" no="3" min="1975" neutral="1515" max="975"/>
<servo name="HATCH" no="2" max="2000" neutral="1000" min="1000"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="HATCH" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="HATCH" value="@GAIN1"/>
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.8"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.8"/>
<define name="AILEVON_AILERON_RATE" value="0.3"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.7"/>
</section>
<command_laws>
@@ -30,11 +35,12 @@
<set servo="GAZ" value="@THROTTLE"/>
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
<set servo="HATCH" value="@HATCH"/>
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.96"/>
<define name="MAX_PITCH" value="0.8"/>
<define name="MAX_ROLL" value="0.6"/>
<define name="MAX_PITCH" value="0.6"/>
</section>
<section name="adc" prefix="ADC_CHANNEL_">
@@ -43,13 +49,14 @@
<define name="IR_TOP" value="ADC_6"/>
<define name="IR_NB_SAMPLES" value="16"/>
<define name="GYRO_ROLL" value="ADC_4"/>
<define name="GYRO_PITCH" value="ADC_5"/>
<define name="GYRO_NB_SAMPLES" value="16"/>
</section>
<define name="GYRO_TEMP" value="ADC_5"/>
<define name="GYRO_NB_SAMPLES" value="16"/>
</section>
<section name="INFRARED" prefix="IR_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="10" unit="deg"/>
<define name="DEFAULT_CONTRAST" value="200"/>
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
<linear name="RollOfIrs" arity="2" coeff1="-0.7" coeff2="0.7"/>
@@ -60,104 +67,152 @@
<define name="ADC_ROLL_NEUTRAL" value="0"/>
<define name="ADC_PITCH_NEUTRAL" value="-716"/>
<define name="ADC_TOP_NEUTRAL" value="512"/>
<define name="CORRECTION_UP" value="1."/>
<define name="CORRECTION_DOWN" value="1."/>
<define name="CORRECTION_LEFT" value="1."/>
<define name="CORRECTION_RIGHT" value="1."/>
<define name="ESTIMATED_PHI_PI_4" value="0.9"/>
<define name="360_LATERAL_CORRECTION" value="1."/>
<define name="360_LONGITUDINAL_CORRECTION" value="1."/>
<define name="360_VERTICAL_CORRECTION" value="1."/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_COEF" value="1"/>
<define name="ADC_ROLL_NEUTRAL" value="503"/>
<define name="ADC_PITCH_NEUTRAL" value="469"/>
<define name="ADC_TEMP_SLOPE" value="0"/>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="500"/>
<define name="ADC_TEMP_NEUTRAL" value="476"/>
<define name="ADC_TEMP_SLOPE" value="0"/>
<define name="ROLL_SCALE" value="0.44"/>
<define name="ROLL_DIRECTION" value="1"/>
<define name="ROLL_DIRECTION" value="1."/>
</section>
<section name="BAT">
<define name="MILLIAMP_PER_PERCENT" value="0.86"/>
<define name="ADC_CHANNEL_VSUPPLY" value="AdcBank1(6)"/>
<define name="VoltageOfAdc(adc)" value="(0.01787109375*adc)"/>
<define name="LOW_BATTERY" value="9.3" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="20." unit="m/s"/>
<define name="CARROT" value="3." unit="s"/>
<define name="CONTROL_RATE" value="60"/>
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="TRIGGER_DELAY" value="1."/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
<define name="MIN_CIRCLE_RADIUS" value="50."/>
</section>
<section name="PID">
<define name="ROLL_PGAIN" value="10000."/>
<define name="PITCH_OF_ROLL" value="0.0"/>
<define name="PITCH_PGAIN" value="6000."/>
<define name="MAX_ROLL" value="0.73"/>
<define name="MAX_PITCH" value="0.7"/>
<define name="MIN_PITCH" value="-0.7"/>
<define name="AILERON_OF_GAZ" value="0.0"/>
<define name="CRUISE_THROTTLE" value="0.50"/>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
<!-- outer loop proportional gain -->
<define name="ALTITUDE_PGAIN" value="-0.03"/>
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.35"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.30"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.80"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-1000"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
<!-- auto pitch inner loop -->
<define name="AUTO_PITCH_PGAIN" value="-0.05"/>
<define name="AUTO_PITCH_IGAIN" value="0.075"/>
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
<define name="THROTTLE_SLEW" value="0.05"/>
</section>
<section name="ALT" prefix="CLIMB_">
<define name="PITCH_PGAIN" value="-0.1"/>
<define name="PITCH_IGAIN" value="0.025"/>
<define name="PGAIN" value="-0.025"/>
<define name="IGAIN" value="0.25"/>
<define name="MAX" value="3."/>
<define name="PITCH_OF_VZ_PGAIN" value="0.5"/>
<define name="GAZ_OF_CLIMB" value="0.15" unit="%/(m/s)"/>
<define name="MAX_DIFF_GAZ" value="0.05" unit="%"/>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-0.8"/>
<define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="ROLL_PGAIN" value="5000."/>
<define name="AILERON_OF_THROTTLE" value="0.0"/>
<define name="PITCH_PGAIN" value="-9000."/>
<define name="PITCH_DGAIN" value="1.5"/>
<define name="ELEVATOR_OF_ROLL" value="1250"/>
<!-- roll rate loop -->
<define name="ROLL_RATE_MODE_DEFAULT" value="1"/>
<define name="ROLL_RATE_SETPOINT_PGAIN" value="-5." unit="rad/s/rad"/>
<define name="ROLL_RATE_MAX_SETPOINT" value="10"/>
<define name="LO_THROTTLE_ROLL_RATE_PGAIN" value="1000."/>
<define name="HI_THROTTLE_ROLL_RATE_PGAIN" value="1000."/>
<define name="ROLL_RATE_IGAIN" value="0."/>
<define name="ROLL_RATE_DGAIN" value="0."/>
<define name="ROLL_RATE_SUM_NB_SAMPLES" value="64"/>
</section>
<section name="NAV">
<define name="COURSE_PGAIN" value="-1.3"/>
<define name="ALTITUDE_PGAIN" value="-0.065"/>
<define name="NAV_PITCH" value="0."/>
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
</section>
<section name="SIMU">
<define name="ROLL_RESPONSE_FACTOR" value="2."/>
<define name="YAW_RESPONSE_FACTOR" value="1.35"/>
<define name="WEIGHT" value="1.3"/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="CLIMB_GAZ" value="1.0"/><!-- Gaz for Aggressive Climb -->
<define name="DESCENT_GAZ" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="CLIMB_PITCH" value="0.52"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_PITCH" value="-0.7"/><!-- Pitch for Aggressive Decent -->
<define name="BLEND_START" value="30"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="15"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<section name="AGGRESSIVE" prefix="AGR_">
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="0.8"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
<section name="GYRO">
<section name="GYRO_GAINS">
<define name="GYRO_MAX_RATE" value="200."/>
<define name="ROLLRATESUM_NB_SAMPLES" value="60"/>
<define name="ALT_ROLL_PGAIN" value="120.0"/>
<define name="ROLL_RATE_PGAIN" value="85.0"/>
<define name="ROLLRATESUM_NB_SAMPLES" value="64"/>
<define name="ALT_ROLL__PGAIN" value="1.0"/>
<define name="ROLL_RATE_PGAIN" value="1000.0"/>
<define name="ROLL_RATE_IGAIN" value="0.0"/>
<define name="ROLL_RATE_DGAIN" value="0.0"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
<define name="DEFAULT_GAZ" value="0.0" unit="%"/>
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
<define name="HOME_RADIUS" value="100" unit="m"/>
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
<define name="HOME_RADIUS" value="100" unit="m"/>
</section>
<section name="DATALINK" prefix="DATALINK_">
<define name="DEVICE_TYPE" value="XBEE"/>
<define name="DEVICE_ADDRESS" value="...."/>
</section>
<!--
<section name="DATALINK">
<define name="DATALINK_DEVICE_TYPE" value="XBEE"/>
<define name="DATALINK_DEVICE_ADDRESS" value="...."/>
<define name="XBEE_INIT" value="\"ATPL2\""/>
</section>
-->
<section name="SIMU">
<define name="YAW_RESPONSE_FACTOR" value="0.5"/>
</section>
<makefile>
include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
FLASH_MODE=IAP
ap.CFLAGS += -DFBW -DAP -DCONFIG=\"tiny_1_1.h\" -DLED -DTIME_LED=1
ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
@@ -165,16 +220,16 @@ ap.srcs += commands.c
ap.CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
ap.srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
#ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017
#ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
#ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c
ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
#TRANSPARENT ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
#TRANSPARENT ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c
ap.CFLAGS += -DINTER_MCU
ap.srcs += inter_mcu.c
@@ -186,25 +241,54 @@ ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B19200
ap.srcs += gps_ubx.c gps.c
ap.CFLAGS += -DINFRARED
ap.CFLAGS += -DINFRARED -DALT_KALMAN -DIR_360
ap.srcs += infrared.c estimator.c
ap.srcs += nav.c pid.c
ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
ap.CFLAGS += -DGYRO -DIDG300 -DPID_RATE_LOOP
ap.srcs += gyro.c
ap.CFLAGS += -DGYRO -DADXRS150
ap.srcs += gyro.c nav_line.c chemotaxis.c anemotaxis.c discsurvey.c
ap.srcs += traffic_info.c
ap.srcs += bomb.c
# Hack to use the same tuning file than slayer1
ap.CFLAGS += -DUSE_GPIO
ap.srcs += $(SRC_ARCH)/gpio.c
# Harware In The Loop
#ap.CFLAGS += -DHITL
test.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4015_hw.c main.c
#tunel.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
# Config for SITL simulation
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN -DIR_360
sim.srcs += traffic_info.c
sim.srcs += bomb.c nav_line.c chemotaxis.c anemotaxis.c discsurvey.c
# a test program to setup actuators
setup_actuators.ARCHDIR = $(ARCHI)
setup_actuators.ARCH = arm7tdmi
setup_actuators.TARGET = setup_actuators
setup_actuators.TARGETDIR = setup_actuators
setup_actuators.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4015_hw.c main.c
# a test program to tunnel between both uart
tunnel.ARCHDIR = $(ARCHI)
tunnel.ARCH = arm7tdmi
tunnel.TARGET = tunnel
tunnel.TARGETDIR = tunnel
tunnel.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
</makefile>
</airframe>
conf/airframes/twinjet1.xml
+2
View File
@@ -303,6 +303,8 @@ test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_tran
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
sim.CFLAGS += -DCONFIG=\"classix.h\" -DAGR_CLIMB -DLOITER_TRIM
sim.srcs += traffic_info.c
sim.srcs += nav_line.c chemotaxis.c anemotaxis.c discsurvey.c bomb.c
</makefile>
</airframe>
conf/flight_plans/IS.xml
+27 -4
View File
@@ -10,6 +10,10 @@
</header>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="AF" x="-200" y="100" alt="215"/>
<waypoint name="TOD" x="-150" y="0"/>
<waypoint name="OVERRUN" x="50" y="0"/>
<waypoint name="TD" x="100" y="0" alt="185"/>
<waypoint name="1" x="-75" y="75"/>
<waypoint name="2" x="75" y="75"/>
<waypoint name="MOB" x="200" y="200"/>
@@ -52,18 +56,18 @@
</block>
<block name="oval" strip_button="Oval">
<block name="oval">
<oval p1="1" p2="2" radius="80"/>
</block>
<block name="MOB" strip_button="MOB">
<block name="MOB">
<call fun="NavSetWaypointHere(WP_MOB)"/>
<circle wp="MOB" radius="100"/>
</block>
<block name="Anemotaxis" strip_button="Anem">
<call fun="nav_anemotaxis_downwind(WP_C, 400)"/>
<go wp="C" hmode="route" approaching_time="0"/>
<go from="HOME" wp="C" hmode="route" approaching_time="0"/>
<call fun="nav_anemotaxis_init(WP_C)"/>
<call fun="nav_anemotaxis(WP_C, WP_C1, WP_C2, WP_PLUME)"/>
</block>
@@ -134,7 +138,7 @@
<follow ac_id="5" distance="25" height="10"/>
</block>
<block name="bomb" strip_button="Bomb">
<block name="bomb">
<set value="BombComputeApproach()" var="unit"/>
<circle radius="BOMB_RADIUS" until="NavQdrCloseTo(DegOfRad(bomb_start_qdr)-10)" wp="BASELEG"/>
</block>
@@ -153,6 +157,25 @@
<deroute block="wait"/>
</block>
<block name="land">
<set value="compute_baseleg()" var="unit"/>
<circle radius="BOMB_RADIUS" until="NavCircleCount() > 0.5" wp="BASELEG"/>
<set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
<circle radius="BOMB_RADIUS" until="NavQdrCloseTo(DegOfRad(bomb_start_qdr)-10) && baseleg_alt_tolerance > fabs(estimator_z - baseleg_alt)" wp="BASELEG"/>
</block>
<block name="final">
<exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
<go alt="baseleg_alt" approaching_time="16" from="AF" hmode="route" wp="TD"/>
<go approaching_time="2" from="AF" hmode="route" throttle="0.1" vmode="throttle" wp="TD"/>
</block>
<block name="flare">
<go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
sw/airborne/main_ap.c
+14 -16
View File
@@ -298,25 +298,23 @@ static void navigation_task( void ) {
#if defined FAILSAFE_DELAY_WITHOUT_GPS
/** This section is used for the failsafe of GPS */
static uint8_t last_pprz_mode;
/** Test if we lost the GPS */
if (!GpsFixValid() ||
(cpu_time_sec - last_gps_msg_t > FAILSAFE_DELAY_WITHOUT_GPS)) {
/** If aircraft is launch and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe). */
if (launch && (pprz_mode == PPRZ_MODE_AUTO2 ||
pprz_mode == PPRZ_MODE_HOME)) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
/** If aircraft is launched and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
if (launch) {
if (cpu_time_sec - last_gps_msg_t > FAILSAFE_DELAY_WITHOUT_GPS) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
PERIODIC_SEND_PPRZ_MODE();
gps_lost = TRUE;
}
} else /* GPS is ok */ if (gps_lost) {
/** If aircraft was in failsafe mode, come back in previous mode */
pprz_mode = last_pprz_mode;
gps_lost = FALSE;
PERIODIC_SEND_PPRZ_MODE();
gps_lost = TRUE;
}
}
/** If aircraft was in failsafe mode, come back in previous mode */
else if (gps_lost) {
pprz_mode = last_pprz_mode;
gps_lost = FALSE;
PERIODIC_SEND_PPRZ_MODE();
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
/** Default to keep compatibility with previous behaviour */
sw/airborne/nav.c
+1 -1
View File
@@ -399,7 +399,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
float leg_y = wp_y - last_wp_y;
float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
nav_leg_progress = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2;
nav_leg_progress = Max(nav_leg_progress, 0.);
// nav_leg_progress = Max(nav_leg_progress, 0.);
nav_leg_length = sqrt(leg2);
/** distance of carrot (in meter) */
sw/airborne/nav.h
-3
View File
@@ -75,9 +75,6 @@ extern uint16_t stage_time, block_time;
/** in second */
extern float stage_time_ds;
/** One full circle == 1. */
extern float circle_count;
extern float carrot_x, carrot_y;
extern bool_t nav_in_circle;
sw/airborne/pprz_transport.h
+1 -1
View File
@@ -91,7 +91,7 @@ extern uint8_t ck_a, ck_b;
#define PprzTransportPutFloatByAddr(_x) PprzTransportPut4ByteByAddr((const uint8_t*)_x)
#define PprzTransportPutArray(_put, _n, _x) { \
uint8_t i; \
uint8_t _i; \
PprzTransportPutUint8(_n); \
for(_i = 0; _i < _n; _i++) { \
_put(&_x[_i]); \
sw/ground_segment/cockpit/live.ml
+3 -1
View File
@@ -36,6 +36,8 @@ module Alert_Pprz = Pprz.Messages(struct let name = "alert" end)
let approaching_alert_time = 3.
let track_size = ref 500
let ok_modes = ["MANUAL"; "AUTO1"; "AUTO2"]
let rotate = fun a (x, y) ->
let cosa = cos a and sina = sin a in
(cosa *.x +. sina *.y, -. sina*.x +. cosa *. y)
@@ -750,7 +752,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->
log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name ap_mode);
ac.last_ap_mode <- ap_mode;
ac.strip#set_label "AP" (Pprz.string_assoc "ap_mode" vs);
ac.strip#set_color "AP" (if ap_mode="HOME" then alert_color else ok_color);
ac.strip#set_color "AP" (if List.mem ap_mode ok_modes then ok_color else alert_color);
end;
let gps_mode = Pprz.string_assoc "gps_mode" vs in
ac.strip#set_label "GPS" gps_mode;
sw/tools/gen_flight_plan.ml
+1 -1
View File
@@ -347,7 +347,7 @@ let rec print_stage = fun index_of_waypoints x ->
let last_wp =
try
get_index_waypoint (ExtXml.attrib x "from") index_of_waypoints
with _ -> "last_wp" in
with ExtXml.Error _ -> "last_wp" in
let hmode = output_hmode x wp last_wp in
let vmode = output_vmode x wp last_wp in
if vmode = "glide" && hmode <> "route" then