From ac826cfd91c9b3f5b0bceccaf69b911c46100a17 Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Thu, 15 Feb 2007 10:29:17 +0000 Subject: [PATCH] *** empty log message *** --- conf/airframes/microjet6.xml | 264 ++++++++++++++++++++---------- conf/airframes/twinjet1.xml | 2 + conf/flight_plans/IS.xml | 31 +++- sw/airborne/main_ap.c | 30 ++-- sw/airborne/nav.c | 2 +- sw/airborne/nav.h | 3 - sw/airborne/pprz_transport.h | 2 +- sw/ground_segment/cockpit/live.ml | 4 +- sw/tools/gen_flight_plan.ml | 2 +- 9 files changed, 223 insertions(+), 117 deletions(-) diff --git a/conf/airframes/microjet6.xml b/conf/airframes/microjet6.xml index 4bcf1588df..7cf7d6cf4f 100755 --- a/conf/airframes/microjet6.xml +++ b/conf/airframes/microjet6.xml @@ -1,27 +1,32 @@ - + + + - - - + + + + + +
- - + +
@@ -30,11 +35,12 @@ +
- - + +
@@ -43,13 +49,14 @@ - - -
+ + + +
- + @@ -60,104 +67,152 @@ + + + + + + + + + + +
- -
- - - - + +
+ + + - +
- +
+ + + +
- - - + + + + + + + + + + +
-
- - - - - - - - +
+ + + + + + + + + + + + + + + + + + + + + + + + + +
- -
- - - - - - - - + +
+ + + + + + + + + + + + + + + + + + + + + +
- +
- - +
-
- - - -
- -
- - - - - - +
+ + + + + +
-
+ +
- - - + + +
- - - - - + + + + + +
+ +
+ +
- +
+ +
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 - - diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml index 1ab5cc8606..01cc79eec6 100644 --- a/conf/airframes/twinjet1.xml +++ b/conf/airframes/twinjet1.xml @@ -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 + diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml index 2b575e4170..7c10ddf120 100644 --- a/conf/flight_plans/IS.xml +++ b/conf/flight_plans/IS.xml @@ -10,6 +10,10 @@ + + + + @@ -52,18 +56,18 @@ - + - + - + @@ -134,7 +138,7 @@ - + @@ -153,6 +157,25 @@ + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index 5048f1019a..12398d857d 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -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 */ diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index 5ab3bab19c..77e9c3876c 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -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) */ diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index 00961cbc22..5d12eb5adf 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -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; diff --git a/sw/airborne/pprz_transport.h b/sw/airborne/pprz_transport.h index feafcab38e..36db7f1e28 100644 --- a/sw/airborne/pprz_transport.h +++ b/sw/airborne/pprz_transport.h @@ -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]); \ diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 4086ab9401..90d97c6e3a 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -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; diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 8e8ca4a87d..87647562ca 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -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