diff --git a/conf/airframes/black_one.xml b/conf/airframes/black_one.xml
index 819f023b33..45871d7c85 100644
--- a/conf/airframes/black_one.xml
+++ b/conf/airframes/black_one.xml
@@ -111,6 +111,8 @@
+
+
@@ -143,6 +145,9 @@
+
+
+
@@ -154,7 +159,8 @@
-
+
+
@@ -244,6 +250,12 @@ ap.srcs += bomb.c
#Cap.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM
#Cap.srcs += traffic_info.c point.c cam.c
+
+# Video switch
+ap.CFLAGS += -DUSE_GPIO
+ap.srcs += $(SRC_ARCH)/gpio.c
+
+
# Hardware In The Loop
#ap.CFLAGS += -DHITL
@@ -251,7 +263,7 @@ ap.srcs += bomb.c
# Config for SITL simulation
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM
+sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DIR_360
sim.srcs += bomb.c
#Csim.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM
diff --git a/conf/airframes/microjet5.xml b/conf/airframes/microjet5.xml
index 0f0623ef2e..7875d1b399 100644
--- a/conf/airframes/microjet5.xml
+++ b/conf/airframes/microjet5.xml
@@ -112,10 +112,64 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/microjet7.xml b/conf/airframes/microjet7.xml
index 6a90615211..931707331f 100644
--- a/conf/airframes/microjet7.xml
+++ b/conf/airframes/microjet7.xml
@@ -5,8 +5,8 @@
-
-
+
+
diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml
index b0f5119fef..84585084b0 100644
--- a/conf/airframes/twinjet1.xml
+++ b/conf/airframes/twinjet1.xml
@@ -103,10 +103,64 @@
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -138,8 +192,8 @@
-
-
+
+
@@ -205,7 +259,7 @@ ap.srcs += infrared.c estimator.c
ap.CFLAGS += -DGYRO -DADXRS150 -DPID_RATE_LOOP
ap.srcs += gyro.c
-ap.CFLAGS += -DNAV
+ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
ap.srcs += nav.c pid.c
test.CFLAGS += -DFBW -DCONFIG=\"classix.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING -DUSE_UART0 -DDATALINK -DPPRZ_INPUT -DPPRZ_UART=Uart0 -DUART0_BAUD=B9600
@@ -213,7 +267,7 @@ test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_tran
# Config for SITL simulation
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DCONFIG=\"classix.h\"
+sim.CFLAGS += -DCONFIG=\"classix.h\" -DAGR_CLIMB -DLOITER_TRIM
diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml
index f9320153a4..694e9a17de 100644
--- a/conf/airframes/twinstar1.xml
+++ b/conf/airframes/twinstar1.xml
@@ -79,11 +79,70 @@
-
+
+
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/mav06.xml b/conf/flight_plans/mav06.xml
index da1e18a7af..40ad09ffc4 100644
--- a/conf/flight_plans/mav06.xml
+++ b/conf/flight_plans/mav06.xml
@@ -6,10 +6,6 @@
-
-
-
-
@@ -29,7 +25,7 @@
-
+
@@ -45,6 +41,11 @@
+
+
+
+
+
@@ -59,14 +60,10 @@
+
-
-
-
-
-
-
+
diff --git a/conf/flight_plans/mav06_S2.xml b/conf/flight_plans/mav06_S2.xml
index ecbe0a4087..4b12f03299 100644
--- a/conf/flight_plans/mav06_S2.xml
+++ b/conf/flight_plans/mav06_S2.xml
@@ -4,8 +4,6 @@
-
-
@@ -29,7 +27,7 @@
-
+
@@ -45,6 +43,17 @@
+
+
+
+
+
+
+
+
+
+
+
@@ -58,13 +67,12 @@
-
-
-
+
+
diff --git a/conf/gcs/horizontal.xml b/conf/gcs/horizontal.xml
index 67866c9256..5e619645ee 100644
--- a/conf/gcs/horizontal.xml
+++ b/conf/gcs/horizontal.xml
@@ -4,8 +4,8 @@
-
-
+
+
diff --git a/conf/gcs/left_col.xml b/conf/gcs/left_col.xml
index 54c63dc340..a60183442a 100644
--- a/conf/gcs/left_col.xml
+++ b/conf/gcs/left_col.xml
@@ -3,7 +3,7 @@
-
+
diff --git a/conf/settings/basic.xml b/conf/settings/basic.xml
index 55121259c0..4a00be85a4 100644
--- a/conf/settings/basic.xml
+++ b/conf/settings/basic.xml
@@ -5,7 +5,7 @@
-
+
diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml
index 9029bfa5dc..decedd4b33 100644
--- a/conf/settings/tuning.xml
+++ b/conf/settings/tuning.xml
@@ -5,7 +5,7 @@
-
+
@@ -48,15 +48,6 @@
-
-
-
-
-
-
-
-
-
diff --git a/sw/airborne/bomb.c b/sw/airborne/bomb.c
index caeb7e40b4..fbf4ef4281 100644
--- a/sw/airborne/bomb.c
+++ b/sw/airborne/bomb.c
@@ -6,6 +6,7 @@
#include "inter_mcu.h"
+#if defined WP_RELEASE
/** Speed limit of the paint ball */
#ifndef TRIGGER_DELAY
#define TRIGGER_DELAY 1.
@@ -17,23 +18,6 @@
#define DT 0.1
#define MAX_STEPS 100
-/*
-Twinjet, z=30, x0=8, w =-4
-DT=0.01 : 2.82 12.37 0.00 -0.13
-DT=0.05 : 2.80 12.18 0.00 -0.26
-DT=0.1 : 2.80 11.96 0.00 -0.88
-DT=0.2 : 2.80 11.52 0.00 -2.13
-
-Slayer, z=30, x0=15, w=-5
-DT=0.01 : 2.89 21.35 0.00 -0.09
-DT=0.1 : 2.90 20.45 0.00 -1.41
-
-Slayer, z=50, x0=10, w=-10
-DT=0.001 : 3.96 2.43 0.00 -0.00
-DT=0.1 : 3.90 1.46 0.00 -0.40
-*/
-
-
float bomb_trigger_delay = TRIGGER_DELAY;
float airspeed = 14.;
float bomb_start_qdr;
@@ -133,10 +117,6 @@ unit_t bomb_compute_approach( void ) {
integrate();
- /***
-printf("x0=%.1f y0=%.1f z=%.1f x=%.1f y=%1.f vx=%.1f vy=%.1f\n", x0, y_0, z, x, y, vx, vy);
-***/
-
waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + CLIMB_TIME * vx0;
waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + CLIMB_TIME * vy0;
waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
@@ -147,6 +127,16 @@ printf("x0=%.1f y0=%.1f z=%.1f x=%.1f y=%1.f vx=%.1f vy=%.1f\n", x0, y_0, z, x,
+unit_t bomb_shoot( void ) {
+ ap_state->commands[COMMAND_HATCH] = MAX_PPRZ;
+ return 0;
+}
+
+#endif /* WP_RELEASE */
+
+
+#if defined WP_TD
+
float baseleg_alt, downwind_altitude;
const float baseleg_alt_tolerance = 15.;
@@ -204,9 +194,4 @@ bool_t compute_tod( void ) {
return (distance_to_TD_2 < d1*d1);
}
-
-
-unit_t bomb_shoot( void ) {
- ap_state->commands[COMMAND_HATCH] = MAX_PPRZ;
- return 0;
-}
+#endif /* WP_TD */
diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c
index ae018b156d..0af3e6495d 100644
--- a/sw/airborne/infrared.c
+++ b/sw/airborne/infrared.c
@@ -49,11 +49,9 @@ bool_t ir_360;
float ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4;
float ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4;
-#ifndef IR_360
-
-#define IR_360 FALSE
-
-#else
+#ifndef IR_ESTIMATED_PHI_PI_4
+#define IR_ESTIMATED_PHI_PI_4 M_PI_4
+#endif
#ifndef IR_ESTIMATED_PHI_MINUS_PI_4
#define IR_ESTIMATED_PHI_MINUS_PI_4 IR_ESTIMATED_PHI_PI_4
@@ -67,8 +65,6 @@ float ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4;
#define IR_ESTIMATED_THETA_MINUS_PI_4 IR_ESTIMATED_THETA_PI_4
#endif
-#endif
-
#if defined IR_CORRECTION_LEFT && defined IR_CORRECTION_RIGHT
float ir_correction_left;
@@ -148,7 +144,7 @@ void ir_init(void) {
ir_correction_down = IR_CORRECTION_DOWN;
#endif
- ir_360 = IR_360;
+ ir_360 = TRUE;
ir_estimated_phi_pi_4 = IR_ESTIMATED_PHI_PI_4;
ir_estimated_phi_minus_pi_4 = IR_ESTIMATED_PHI_MINUS_PI_4;
ir_estimated_theta_pi_4 = IR_ESTIMATED_THETA_PI_4;
@@ -301,14 +297,31 @@ void estimator_update_state_infrared( void ) {
estimator_rad_of_ir :
ir_rad_of_ir);
- if (!ir_360) {
+#ifdef IR_360
+ if (ir_360) { /* 360° estimation */
+ /* 250 us for the whole block */
+ float tmp_ir_roll = ir_roll * IR_360_LATERAL_CORRECTION;
+ float tmp_ir_pitch = ir_pitch * IR_360_LONGITUDINAL_CORRECTION;
+ float tmp_ir_top = ir_top * IR_360_VERTICAL_CORRECTION;
+ estimator_phi = atan2(tmp_ir_roll, tmp_ir_top) - ir_roll_neutral;
+ estimator_phi = correct_angle(estimator_phi, ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4);
+
+ estimator_theta = atan2(tmp_ir_pitch, tmp_ir_top) - ir_pitch_neutral;
+ estimator_theta = correct_angle(estimator_theta, ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4);
+ if (estimator_theta < -M_PI_2)
+ estimator_theta += M_PI;
+ else if (estimator_theta > M_PI_2)
+ estimator_theta -= M_PI;
+ } else
+#endif /* IR_360 */
+ {
ir_top = Max(ir_top, 1);
float c = rad_of_ir*(1-z_contrast_mode)+z_contrast_mode*((float)IR_RAD_OF_IR_CONTRAST/fabs(ir_top));
estimator_phi = c * ir_roll - ir_roll_neutral;
estimator_theta = c * ir_pitch - ir_pitch_neutral;
-
+
/* infrared compensation */
#if defined IR_CORRECTION_LEFT && defined IR_CORRECTION_RIGHT
@@ -325,24 +338,9 @@ void estimator_update_state_infrared( void ) {
else
estimator_theta *= ir_correction_down;
#endif
-
+
Bound(estimator_phi, -M_PI_2, M_PI_2);
Bound(estimator_theta, -M_PI_2, M_PI_2);
-
- } else { /* 360° estimation */
- /* 250 us for the whole block */
- float tmp_ir_roll = ir_roll * IR_360_LATERAL_CORRECTION;
- float tmp_ir_pitch = ir_pitch * IR_360_LONGITUDINAL_CORRECTION;
- float tmp_ir_top = ir_top * IR_360_VERTICAL_CORRECTION;
-
- estimator_phi = atan2(tmp_ir_roll, tmp_ir_top) - ir_roll_neutral;
- estimator_phi = correct_angle(estimator_phi, ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4);
-
- estimator_theta = atan2(tmp_ir_pitch, tmp_ir_top) - ir_pitch_neutral;
- estimator_theta = correct_angle(estimator_theta, ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4);
- if (estimator_theta < -M_PI_2)
- estimator_theta += M_PI;
- else if (estimator_theta > M_PI_2)
- estimator_theta -= M_PI;
- }
+
+ }
}
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 1a8eb81163..ec832323b9 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -420,7 +420,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
strip = strip; first_pos = true;
last_block_name = ""; alt = 0.; target_alt = 0.;
in_kill_mode = false; speed = 0.;
- wind_dir = 42.; ground_prox = false;
+ wind_dir = 42.; ground_prox = true;
wind_speed = 0.; } in
Hashtbl.add live_aircrafts ac_id ac;
diff --git a/sw/ground_segment/cockpit/strip.ml b/sw/ground_segment/cockpit/strip.ml
index d798ead6c4..1c535782d0 100644
--- a/sw/ground_segment/cockpit/strip.ml
+++ b/sw/ground_segment/cockpit/strip.ml
@@ -99,7 +99,7 @@ let labels_name = [|
|]
let labels_print = [|
- [| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "->" |]; [| "GPS"; "speed"; "throttle" |]
+ [| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "->" |]; [| "GPS"; "speed"; "throtl" |]
|]
let gen_int = let i = ref (-1) in fun () -> incr i; !i
@@ -166,7 +166,7 @@ let add config color select center_ac mark =
(* AGL gauge *)
let agl_box = GBin.event_box ~packing:(strip#attach ~top:1 ~bottom:3 ~left:(columns-1)) () in
- let agl = GMisc.drawing_area ~width:30 ~height:60 ~show:true ~packing:agl_box#add () in
+ let agl = GMisc.drawing_area ~width:40 ~height:60 ~show:true ~packing:agl_box#add () in
agl#misc#realize ();
tooltips#set_tip agl_box#coerce ~text:"AGL (m)";
diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index 26626a695d..916d7ccbd3 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -26,6 +26,7 @@
module LL = Latlong
open Printf
+open LL
let s = 6.
let losange = [|s;0.; 0.;s; -.s;0.; 0.;-.s|]
@@ -144,6 +145,7 @@ class waypoint = fun (wpts_group:group) (name :string) ?(alt=0.) wgs84 ->
alt <- float_of_string ea#text;
label#set [`TEXT name];
let wgs84 = wgs84_of_string !selected_georef e_pos#text in
+
self#set wgs84;
updated ();
if wpts_group#show_moved then
diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml
index ee4b8bb28a..50f5264a12 100644
--- a/sw/simulator/flightModel.ml
+++ b/sw/simulator/flightModel.ml
@@ -163,6 +163,7 @@ module Make(A:Data.MISSION) = struct
let phi_dot_dot = roll_response_factor *. state.delta_a -. state.phi_dot in
state.phi_dot <- state.phi_dot +. phi_dot_dot *. dt;
state.phi <- norm_angle (state.phi +. state.phi_dot *. dt);
+ state.phi <- bound state.phi (-.max_phi) max_phi;
let psi_dot = -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi) in
state.psi <- norm_angle (state.psi +. psi_dot *. dt);
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index 0363679e8a..e7aba81614 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -308,7 +308,7 @@ let rec print_stage = fun index_of_waypoints sectors x ->
let until = parsed_attrib x "until" in
lprintf "if (%s) NextStage() else {\n" until;
right ();
- lprintf "desired_course = RadOfDeg(%s);\n" (parsed_attrib x "course");
+ lprintf "h_ctl_course_setpoint = RadOfDeg(%s);\n" (parsed_attrib x "course");
ignore (output_vmode x "" "");
output_cam_mode x index_of_waypoints;
left (); lprintf "}\n";