diff --git a/conf/airframes/microjet7.xml b/conf/airframes/microjet7.xml
index 42e90db39a..2f7039e51f 100644
--- a/conf/airframes/microjet7.xml
+++ b/conf/airframes/microjet7.xml
@@ -106,6 +106,7 @@
+
diff --git a/conf/airframes/slayer1.xml b/conf/airframes/slayer1.xml
index b88c5daaac..ae2ea44557 100644
--- a/conf/airframes/slayer1.xml
+++ b/conf/airframes/slayer1.xml
@@ -54,7 +54,7 @@
-
+
@@ -64,14 +64,14 @@
-
+
-
+
@@ -79,7 +79,7 @@
-
+
@@ -101,6 +101,9 @@
+
+
+
diff --git a/conf/airframes/slayer2.xml b/conf/airframes/slayer2.xml
index abf257d57f..e30b06d1cf 100644
--- a/conf/airframes/slayer2.xml
+++ b/conf/airframes/slayer2.xml
@@ -51,7 +51,7 @@
-
+
@@ -59,7 +59,7 @@
-
+
@@ -68,7 +68,7 @@
-
+
@@ -76,7 +76,7 @@
-
+
@@ -96,6 +96,9 @@
+
+
+
-
+
diff --git a/conf/airframes/slayer3.xml b/conf/airframes/slayer3.xml
index c4b9b3e0c5..8ce89517ef 100755
--- a/conf/airframes/slayer3.xml
+++ b/conf/airframes/slayer3.xml
@@ -99,6 +99,9 @@
+
+
+
diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd
index c74832773b..854db26dde 100644
--- a/conf/flight_plans/flight_plan.dtd
+++ b/conf/flight_plans/flight_plan.dtd
@@ -12,14 +12,14 @@
-
+
-
-
+
+
@@ -27,6 +27,7 @@
+
@@ -145,6 +146,11 @@ pitch CDATA #IMPLIED
throttle CDATA #IMPLIED
until CDATA #IMPLIED>
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c
index 63036155c2..ae018b156d 100644
--- a/sw/airborne/infrared.c
+++ b/sw/airborne/infrared.c
@@ -46,10 +46,27 @@ float ir_roll_neutral;
float ir_pitch_neutral;
bool_t ir_360;
-float ir_estimated_phi_pi_4;
+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_MINUS_PI_4
+#define IR_ESTIMATED_PHI_MINUS_PI_4 IR_ESTIMATED_PHI_PI_4
+#endif
+
+#ifndef IR_ESTIMATED_THETA_PI_4
+#define IR_ESTIMATED_THETA_PI_4 IR_ESTIMATED_PHI_PI_4
+#endif
+
+#ifndef IR_ESTIMATED_THETA_MINUS_PI_4
+#define IR_ESTIMATED_THETA_MINUS_PI_4 IR_ESTIMATED_THETA_PI_4
+#endif
+
#endif
@@ -133,6 +150,9 @@ void ir_init(void) {
ir_360 = IR_360;
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;
+ ir_estimated_theta_minus_pi_4 = IR_ESTIMATED_THETA_MINUS_PI_4;
ir_contrast = IR_DEFAULT_CONTRAST;
ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / IR_DEFAULT_CONTRAST;
@@ -249,17 +269,27 @@ void estimator_update_ir_estim( void ) {
}
-static inline float correct_angle(float m_angle, float est_pi_4) {
- if (fabs(m_angle) < est_pi_4)
+/* Correction of the infrared estimation roll angle: returns pi/4 for
+ est_pi_4 */
+static inline float correct_angle(float m_angle, float est_pi_4, float est_minus_pi_4) {
+ if (m_angle >= 0 && m_angle < est_pi_4)
+ /* 0 .. est_pi_4 */
return (m_angle * M_PI_4 / est_pi_4);
else if (m_angle > M_PI - est_pi_4)
+ /* pi-est_pi_4 .. pi */
return (m_angle - (M_PI - est_pi_4))* M_PI_4 / est_pi_4 + 3*M_PI_4;
else if (m_angle >= est_pi_4)
+ /* est_pi_4 .. pi-est_pi_4 */
return (m_angle - est_pi_4) * M_PI_4 / (M_PI_2 - est_pi_4) + M_PI_4;
- else if (m_angle < - (M_PI - est_pi_4))
- return (m_angle + M_PI)* M_PI_4 / est_pi_4 + -M_PI;
+ else if (m_angle <= 0 && m_angle > -est_minus_pi_4)
+ /* -est_pi_4 .. 0 */
+ return (m_angle * M_PI_4 / est_minus_pi_4);
+ else if (m_angle < - (M_PI - est_minus_pi_4))
+ /* -pi .. -(pi-est_pi_4) */
+ return (m_angle + M_PI)* M_PI_4 / est_minus_pi_4 + -M_PI;
else
- return (m_angle - (-M_PI+est_pi_4)) * M_PI_4 / (M_PI_2 - est_pi_4) - 3 * M_PI_4;
+ /* -(pi-est_pi_4) .. -est_pi_4 */
+ return (m_angle - (-M_PI+est_minus_pi_4)) * M_PI_4 / (M_PI_2 - est_minus_pi_4) - 3 * M_PI_4;
}
@@ -306,10 +336,10 @@ void estimator_update_state_infrared( void ) {
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);
+ 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_phi_pi_4);
+ 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)
diff --git a/sw/airborne/infrared.h b/sw/airborne/infrared.h
index 306c2f63a7..fdaabdaa24 100644
--- a/sw/airborne/infrared.h
+++ b/sw/airborne/infrared.h
@@ -49,7 +49,9 @@ extern float ir_rad_of_ir;
extern int16_t ir_contrast;
extern float z_contrast_mode;
extern bool_t ir_360;
-extern float ir_estimated_phi_pi_4;
+extern float ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4;
+extern float ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4;
+
#if defined IR_CORRECTION_LEFT && defined IR_CORRECTION_RIGHT
extern float ir_correction_left;
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index e6fa4cbc6d..c404c0af86 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -116,10 +116,16 @@ uint8_t light_mode = 0;
/** \brief Update paparazzi mode
*/
static inline uint8_t pprz_mode_update( void ) {
- /** We remain in home mode until explicit reset from the RC */
if ((pprz_mode != PPRZ_MODE_HOME &&
pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
- || CheckEvent(rc_event_1)) {
+ ||
+#ifdef UNLOCKED_HOME_MODE
+ TRUE
+#else
+ /** We remain in home mode until explicit reset from the RC */
+ CheckEvent(rc_event_1)
+#endif
+ ) {
return ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE], fbw_state->status));
} else
return FALSE;
@@ -224,7 +230,7 @@ inline void telecommand_task( void ) {
copy_from_to_fbw();
uint8_t really_lost = bit_is_set(fbw_state->status, RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
- if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch && (/* really_lost || */ too_far_from_home)) {
+ if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch && (really_lost || too_far_from_home)) {
pprz_mode = PPRZ_MODE_HOME;
mode_changed = TRUE;
}
@@ -473,11 +479,6 @@ void periodic_task_ap( void ) {
ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
-#ifdef COMMAND_HATCH_CMD
- extern pprz_t hatch_cmd;
- ap_state->commands[COMMAND_HATCH_CMD] = hatch_cmd;
-#endif
-
#if defined MCU_SPI_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index 017c847c75..83c509f3c4 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -371,22 +371,28 @@ bool_t moved_waypoints[NB_WAYPOINT+1];
* uav has not gone past waypoint.
* Return true if it is the case.
*/
-static bool_t approaching(uint8_t wp, float approaching_time) {
+static bool_t approaching_xy(float x, float y, float approaching_time) {
/** distance to waypoint in x */
- float pw_x = waypoints[wp].x - estimator_x;
+ float pw_x = x - estimator_x;
/** distance to waypoint in y */
- float pw_y = waypoints[wp].y - estimator_y;
+ float pw_y = y - estimator_y;
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
float min_dist = approaching_time * estimator_hspeed_mod;
if (dist2_to_wp < min_dist*min_dist)
return TRUE;
- float scal_prod = (waypoints[wp].x - last_x) * pw_x + (waypoints[wp].y - last_y) * pw_y;
+ float scal_prod = (x - last_x) * pw_x + (y - last_y) * pw_y;
return (scal_prod < 0.);
}
+static bool_t approaching(uint8_t wp, float approaching_time) {
+ return approaching_xy(waypoints[wp].x, waypoints[wp].y, approaching_time);
+}
+
+
+
/** static inline void fly_to_xy(float x, float y)
* \brief Computes \a desired_x, \a desired_y and \a desired_course.
*/
@@ -422,7 +428,6 @@ static void route_to_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y
float carrot = CARROT * estimator_hspeed_mod;
alpha += Max(carrot / leg, 0.);
- alpha = Min(1., alpha);
in_segment = TRUE;
segment_x_1 = last_wp_x;
segment_y_1 = last_wp_y;
@@ -531,3 +536,75 @@ void nav_without_gps(void) {
nav_desired_gaz = TRIM_UPPRZ((CRUISE_THROTTLE)*MAX_PPRZ);
#endif
}
+
+
+enum eight_status { EW, CW, WE, CE };
+
+static enum eight_status eight_status;
+void nav_eight_init( void ) {
+ eight_status = CE;
+}
+
+void nav_eight(uint8_t center, uint8_t TA1, float radius) {
+ float alt = waypoints[center].a;
+ waypoints[TA1].a = alt;
+
+ float center_TA1_x = waypoints[TA1].x - waypoints[center].x;
+ float center_TA1_y = waypoints[TA1].y - waypoints[center].y;
+
+ struct point TA2 = { waypoints[TA1].x - 2*center_TA1_x,
+ waypoints[TA1].y - 2*center_TA1_y,
+ alt };
+
+ float d = sqrt(center_TA1_x*center_TA1_x+center_TA1_y*center_TA1_y);
+ float u_x = center_TA1_x / d;
+ float u_y = center_TA1_y / d;
+
+ struct point TA1N = { waypoints[TA1].x + radius * -u_y,
+ waypoints[TA1].y + radius * u_x,
+ alt };
+ struct point TA1S = { waypoints[TA1].x - radius * -u_y,
+ waypoints[TA1].y - radius * u_x,
+ alt };
+
+ struct point TA2N = { TA2.x + radius * -u_y,
+ TA2.y + radius * u_x,
+ alt };
+ struct point TA2S = { TA2.x - radius * -u_y,
+ TA2.y - radius * u_x,
+ alt };
+
+ float qdr_out = M_PI - atan2(u_y, u_x);
+
+ switch (eight_status) {
+ case CE :
+ Circle(TA1, radius);
+ if (Qdr(DegOfRad(qdr_out)-10)) {
+ eight_status = EW;
+ InitStage();
+ }
+ return;
+ case EW:
+ route_to_xy(TA1S.x, TA1S.y, TA2N.x, TA2N.y);
+ if (approaching_xy(TA2N.x, TA2N.y,CARROT)) {
+ eight_status = CW;
+ InitStage();
+ }
+ return;
+ case CW :
+ CircleXY(TA2.x, TA2.y, -radius);
+ if (Qdr(DegOfRad(qdr_out)+10)) {
+ eight_status = WE;
+ InitStage();
+ }
+ return;
+ case WE:
+ route_to_xy(TA2S.x, TA2S.y, TA1N.x, TA1N.y);
+ if (approaching_xy(TA1N.x, TA1N.y,CARROT)) {
+ eight_status = CE;
+ InitStage();
+ }
+ return;
+
+ }
+}
diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h
index b1abfa3a6d..0c840ec14f 100644
--- a/sw/airborne/nav.h
+++ b/sw/airborne/nav.h
@@ -94,6 +94,10 @@ extern uint8_t horizontal_mode;
} \
}
+extern void nav_eight_init( void );
+extern void nav_eight(uint8_t, uint8_t, float);
+#define Eight(a, b, c) nav_eight((a), (b), (c))
+
extern float ground_alt;
extern float survey_west, survey_east, survey_north, survey_south;
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 284628099d..a287a0aafa 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -75,6 +75,7 @@ type aircraft = {
mutable in_kill_mode : bool;
mutable speed : float;
mutable alt : float;
+ mutable target_alt : float;
mutable flight_time : int;
mutable wind_speed : float;
mutable wind_dir : float (* Rad *)
@@ -91,7 +92,7 @@ let log =
let last = ref M.empty in
fun ?(say = false) (a:Pages.alert) ac_id s ->
if not (M.mem ac_id !last) || M.find ac_id !last <> s then begin
- last := M.add ac_id s !last;
+ last := M.add ac_id s (M.remove ac_id !last);
if say then Speech.say s;
a#add s
end
@@ -307,7 +308,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
let select_this_tab =
let n = acs_notebook#page_num ac_frame#coerce in
fun () -> acs_notebook#goto_page n in
- let strip = Strip.add config color select_this_tab center_ac commit_moves (mark geomap ac_id track !Plugin.frame) in
+ let strip = Strip.add config color select_this_tab center_ac (mark geomap ac_id track !Plugin.frame) in
(** Build the XML flight plan, connect then "jump_to_block" *)
@@ -340,7 +341,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
let label = ExtXml.attrib b "strip_button"
and id = ExtXml.int_attrib b "no" in
let b = GButton.button ~label () in
- Strip.add_widget strip b#coerce;
+ strip#add_widget b#coerce;
ignore (b#connect#clicked (fun _ -> jump_to_block ac_id id))
with
_ -> ())
@@ -380,18 +381,19 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
prerr_endline (Printexc.to_string exc);
Xml.Element("empty", [], [])
in
+ let dl_setting_callback = fun idx value ->
+ let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx;"value", Pprz.Float value] in
+ Ground_Pprz.message_send "dl" "DL_SETTING" vs in
let dl_settings_page =
try
let xml_settings = Xml.children (ExtXml.child settings_xml "dl_settings") in
- let callback = fun idx value ->
- let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx;"value", Pprz.Float value] in
- Ground_Pprz.message_send "dl" "DL_SETTING" vs in
- let settings_tab = new Pages.settings ~visible xml_settings callback strip in
+ let settings_tab = new Pages.settings ~visible xml_settings dl_setting_callback strip in
+
let tab_label = (GMisc.label ~text:"Settings" ())#coerce in
ac_notebook#append_page ~tab_label settings_tab#widget;
Some settings_tab
with exc ->
- prerr_endline (Printexc.to_string exc);
+ log alert ac_id (Printexc.to_string exc);
None in
let rc_settings_page =
@@ -415,7 +417,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
dl_settings_page = dl_settings_page;
rc_settings_page = rc_settings_page;
strip = strip; first_pos = true;
- last_block_name = ""; alt = 0.;
+ last_block_name = ""; alt = 0.; target_alt = 0.;
in_kill_mode = false; speed = 0.;
wind_dir = 42.;
wind_speed = 0.; } in
@@ -443,7 +445,24 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
true
in
- ignore (Glib.Timeout.add 10000 send_wind)
+ ignore (Glib.Timeout.add 10000 send_wind);
+
+ (** Connect the shift altitude buttons *)
+ begin
+ match dl_settings_page with
+ Some settings_tab ->
+ let flight_altitude_id, _flight_altitude_label =
+ try
+ settings_tab#assoc "flight_altitude"
+ with Not_found ->
+ failwith "flight_altitude not setable" in
+ strip#connect_shift_alt
+ (fun x ->
+ dl_setting_callback flight_altitude_id (ac.target_alt +. x));
+ | None -> ()
+ end
+
+
@@ -488,8 +507,8 @@ let get_wind_msg = fun (geomap:G.widget) _sender vs ->
let get_fbw_msg = fun _sender vs ->
let ac = get_ac vs in
let status = Pprz.string_assoc "rc_status" vs in
- Strip.set_label ac.strip "RC" status;
- Strip.set_color ac.strip "RC"
+ ac.strip#set_label "RC" status;
+ ac.strip#set_color "RC"
(match status with
"LOST" | "REALLY_LOST" -> warning_color
| _ -> ok_color)
@@ -498,9 +517,9 @@ let get_fbw_msg = fun _sender vs ->
let get_engine_status_msg = fun _sender vs ->
let ac = get_ac vs in
- Strip.set_label ac.strip "throttle"
+ ac.strip#set_label "throttle"
(string_of_float (Pprz.float_assoc "throttle" vs));
- Strip.set_bat ac.strip (Pprz.float_assoc "bat" vs)
+ ac.strip#set_bat (Pprz.float_assoc "bat" vs)
let get_if_calib_msg = fun _sender vs ->
let ac = get_ac vs in
@@ -589,14 +608,14 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->
then sprintf "- %.1f" (abs_float value)
else sprintf "%.1f" value
in
- Strip.set_label ac.strip lbl_name s
+ ac.strip#set_label lbl_name s
in
set_label "alt" alt;
set_label "speed" speed;
set_label "climb" climb;
let agl = (a "agl") in
ac.alt <- alt;
- Strip.set_agl ac.strip agl;
+ ac.strip#set_agl agl;
if (ac.flight_time > 10 && agl < 20.) then
log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name "Ground Proximity Warning")
@@ -612,25 +631,25 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->
and cur_stage = Pprz.int_assoc "cur_stage" vs in
highlight_fp ac cur_block cur_stage;
let set_label = fun l f ->
- Strip.set_label ac.strip l (sprintf "%.1f" (Pprz.float_assoc f vs)) in
+ ac.strip#set_label l (sprintf "%.1f" (Pprz.float_assoc f vs)) in
set_label "->" "target_alt";
set_label "/" "target_climb";
let target_alt = Pprz.float_assoc "target_alt" vs in
- Strip.set_label ac.strip "diff_target_alt" (sprintf "%+.0f" (ac.alt -. target_alt));
-
+ ac.strip#set_label "diff_target_alt" (sprintf "%+.0f" (ac.alt -. target_alt));
+ ac.target_alt <- target_alt;
let b = List.assoc cur_block ac.blocks in
if b <> ac.last_block_name then begin
log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name b);
ac.last_block_name <- b;
let b = String.sub b 0 (min 10 (String.length b)) in
- Strip.set_label ac.strip "block_name" b
+ ac.strip#set_label "block_name" b
end;
let block_time = Int32.to_int (Pprz.int32_assoc "block_time" vs)
and stage_time = Int32.to_int (Pprz.int32_assoc "stage_time" vs) in
let bt = sprintf "%02d:%02d" (block_time / 60) (block_time mod 60) in
- Strip.set_label ac.strip "block_time" bt;
+ ac.strip#set_label "block_time" bt;
let st = sprintf "%02d:%02d" (stage_time / 60) (block_time mod 60) in
- Strip.set_label ac.strip "stage_time" st
+ ac.strip#set_label "stage_time" st
in
safe_bind "NAV_STATUS" get_ns;
@@ -686,15 +705,15 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->
if ap_mode <> ac.last_ap_mode then begin
log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name ap_mode);
ac.last_ap_mode <- ap_mode;
- Strip.set_label ac.strip "AP" (Pprz.string_assoc "ap_mode" vs);
- Strip.set_color ac.strip "AP" (if ap_mode="HOME" then alert_color else ok_color);
+ 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);
end;
let gps_mode = Pprz.string_assoc "gps_mode" vs in
- Strip.set_label ac.strip "GPS" gps_mode;
- Strip.set_color ac.strip "GPS" (if gps_mode<>"3D" then alert_color else ok_color);
+ ac.strip#set_label "GPS" gps_mode;
+ ac.strip#set_color "GPS" (if gps_mode<>"3D" then alert_color else ok_color);
let ft =
sprintf "%02d:%02d:%02d" (flight_time / 3600) ((flight_time / 60) mod 60) (flight_time mod 60) in
- Strip.set_label ac.strip "flight_time" ft;
+ ac.strip#set_label "flight_time" ft;
let kill_mode = Pprz.string_assoc "kill_mode" vs in
if not ac.in_kill_mode then
if kill_mode <> "OFF" then begin
@@ -775,8 +794,8 @@ let message_request = Ground_Pprz.message_req
let get_ts = fun _sender vs ->
let ac = get_ac vs in
let t = Pprz.float_assoc "time_since_last_bat_msg" vs in
- Strip.set_label ac.strip "telemetry_status" (if t > 2. then sprintf "%.1f" t else " ");
- Strip.set_color ac.strip "telemetry_status" (if t > 5. then alert_color else ok_color)
+ ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.1f" t else " ");
+ ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else ok_color)
let listen_telemetry_status = fun () ->
diff --git a/sw/ground_segment/cockpit/pages.ml b/sw/ground_segment/cockpit/pages.ml
index 6bc2b14516..5d08601f27 100644
--- a/sw/ground_segment/cockpit/pages.ml
+++ b/sw/ground_segment/cockpit/pages.ml
@@ -202,14 +202,15 @@ class misc ~packing (widget: GBin.frame) =
(*****************************************************************************)
(* Dataling settings paged *)
(*****************************************************************************)
-let one_setting = fun i do_change packing s (tooltips:GData.tooltips) strip ->
+let one_setting = fun i do_change packing s (tooltips:GData.tooltips) (strip:Strip.t) ->
let f = fun a -> float_of_string (ExtXml.attrib s a) in
let lower = f "min"
and upper = f "max"
and step_incr = f "step" in
let hbox = GPack.hbox ~packing () in
- let text = try ExtXml.attrib s "shortname" with _ -> ExtXml.attrib s "var" in
+ let varname = ExtXml.attrib s "var" in
+ let text = try ExtXml.attrib s "shortname" with _ -> varname in
let _l = GMisc.label ~width:100 ~text ~packing:hbox#pack () in
let _v = GMisc.label ~width:50 ~text:"N/A" ~packing:hbox#pack () in
@@ -261,10 +262,10 @@ let one_setting = fun i do_change packing s (tooltips:GData.tooltips) strip ->
let label = ExtXml.attrib x "name"
and sp_value = ExtXml.float_attrib x "value" in
let b = GButton.button ~label () in
- Strip.add_widget strip b#coerce;
+ strip#add_widget b#coerce;
ignore (b#connect#clicked (fun _ -> do_change i sp_value)))
(Xml.children s);
- _v
+ (i, varname, _v)
let rec build_settings = fun do_change i flat_list xml_settings packing tooltips strip ->
@@ -303,17 +304,21 @@ class settings = fun ?(visible = fun _ -> true) xml_settings do_change strip ->
let vbox = GPack.vbox ~packing:sw#add_with_viewport () in
let tooltips = GData.tooltips () in
let i = ref 0 and l = ref [] in
- let current_values =
+ let ordered_list =
build_settings do_change i l xml_settings vbox#add tooltips strip;
- Array.of_list (List.rev !l) in
+ List.rev !l in
+ let variables = Array.of_list ordered_list in
+ let assocs = List.map (fun (i,var,_v) -> (var, (i, _v))) ordered_list in
object (self)
method widget = sw#coerce
method length = !i
method set = fun i v ->
if visible self#widget then
let s = string_of_float v in
- if current_values.(i)#text <> s then
- current_values.(i)#set_text s
+ let (_, _, current_value) = variables.(i) in
+ if current_value#text <> s then
+ current_value#set_text s
+ method assoc var = List.assoc var assocs
end
diff --git a/sw/ground_segment/cockpit/pages.mli b/sw/ground_segment/cockpit/pages.mli
index c824e1baa3..19a4952508 100644
--- a/sw/ground_segment/cockpit/pages.mli
+++ b/sw/ground_segment/cockpit/pages.mli
@@ -28,6 +28,7 @@ class settings : ?visible:(GObj.widget -> bool) -> Xml.xml list -> (int -> float
object
method length : int
method set : int -> float -> unit
+ method assoc : string -> int * GMisc.label
method widget : GObj.widget
end
diff --git a/sw/ground_segment/cockpit/strip.ml b/sw/ground_segment/cockpit/strip.ml
index 817d23eef2..d798ead6c4 100644
--- a/sw/ground_segment/cockpit/strip.ml
+++ b/sw/ground_segment/cockpit/strip.ml
@@ -24,6 +24,14 @@
*
*)
+type t =
+ < add_widget : GObj.widget -> unit;
+ connect_shift_alt : (float -> unit) -> unit;
+ set_agl : ?color:string -> float -> unit;
+ set_bat : ?color:string -> float -> unit;
+ set_color : string -> string -> unit;
+ set_label : string -> string -> unit >
+
let bat_max = 12.5
let bat_min = 9.
let agl_max = 150.
@@ -33,12 +41,58 @@ let scrolled = GBin.scrolled_window ~hpolicy: `AUTOMATIC ~vpolicy: `AUTOMATIC ()
let table = GPack.table ~rows: 1 ~columns: 1 ~row_spacings: 5 ~packing: (scrolled#add_with_viewport) ()
-type t = {
- agl: GMisc.drawing_area;
- gauge: GMisc.drawing_area;
- labels: (string * (GBin.event_box * GMisc.label)) list;
- buttons_box : GPack.box
- }
+
+
+(** set a label *)
+let set_label labels name value =
+ try
+ let _eb, l = List.assoc (name^"_value") labels in
+ if l#text <> value then
+ l#set_label value
+ with
+ Not_found ->
+ Printf.fprintf stderr "Strip.set_label: '%s' unknown\n%!" name
+
+(** set a color *)
+let set_color labels name color =
+ let eb, _l = List.assoc (name^"_value") labels in
+ eb#coerce#misc#modify_bg [`NORMAL, `NAME color]
+
+
+let set_gauge = fun ?(color="green") gauge v_min v_max value string ->
+ let {Gtk.width=width; height=height} = gauge#misc#allocation in
+ let dr = GDraw.pixmap ~width ~height ~window:gauge () in
+ dr#set_foreground (`NAME "orange");
+ dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
+
+ let f = (value -. v_min) /. (v_max -. v_min) in
+ let f = max 0. (min 1. f) in
+ let h = truncate (float height *. f) in
+ dr#set_foreground (`NAME color);
+ dr#rectangle ~x:0 ~y:(height-h) ~width ~height:h ~filled:true ();
+
+ let context = gauge#misc#create_pango_context in
+ let layout = context#create_layout in
+ Pango.Layout.set_text layout string;
+ let (w,h) = Pango.Layout.get_pixel_size layout in
+ dr#put_layout ~x:((width-w)/2) ~y:((height-h)/2) ~fore:`BLACK layout;
+
+ (new GDraw.drawable gauge#misc#window)#put_pixmap ~x:0 ~y:0 dr#pixmap
+
+
+
+(** set the battery *)
+let set_bat ?color gauge value =
+ set_gauge ?color gauge bat_min bat_max value (string_of_float value)
+
+
+(** set the AGL *)
+let set_agl ?color agl value =
+ set_gauge ?color agl 0. agl_max value (Printf.sprintf "%3.0f" value)
+
+
+let add_widget = fun buttons_box widget ->
+ buttons_box#add widget
let labels_name = [|
[| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "/" |]; [| "GPS"; "speed"; "throttle" |]
@@ -53,8 +107,12 @@ let rows = 1 + Array.length labels_name + 1
let columns = 1 + 2 * Array.length labels_name.(0) + 1
+
+
+
+
(** add a strip to the panel *)
-let add config color select center_ac commit_moves mark =
+let add config color select center_ac mark =
let widget = table in
(* number of the strip *)
let strip_number = gen_int () in
@@ -107,13 +165,13 @@ let add config color select center_ac commit_moves mark =
gauge#misc#realize ();
(* AGL gauge *)
- let agl_box = GBin.event_box ~packing:(strip#attach ~top:1 ~bottom:(rows-1) ~left:(columns-1)) () in
+ 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
agl#misc#realize ();
tooltips#set_tip agl_box#coerce ~text:"AGL (m)";
(* Diff to target altitude *)
- let dta_box = GBin.event_box ~packing:(strip#attach ~top:(rows-1) ~left:(columns-1)) () in
+ let dta_box = GBin.event_box ~packing:(strip#attach ~top:3 ~left:(columns-1)) () in
let diff_target_alt = GMisc.label ~text: "+0" ~packing:dta_box#add () in
add_label "diff_target_alt_value" (plane_color, diff_target_alt);
tooltips#set_tip dta_box#coerce ~text:"Height to target (m)";
@@ -141,63 +199,26 @@ let add config color select center_ac commit_moves mark =
let top = rows - 1 in
let b = GButton.button ~label:"Center A/C" ~packing:(strip#attach ~top ~left:1 ~right:3) () in
ignore(b#connect#clicked ~callback:center_ac);
- let b = GButton.button ~label:"Mark" ~packing:(strip#attach ~top ~left:5 ~right:7) () in
+ let b = GButton.button ~label:"Mark" ~packing:(strip#attach ~top ~left:3 ~right:5) () in
+ ignore (b#connect#clicked ~callback:mark);
+
+ let minus5 = GButton.button ~label:"-5m" ~packing:(strip#attach ~top ~left:5) ()
+ and plus5 = GButton.button ~label:"+5m" ~packing:(strip#attach ~top ~left:6) ()
+ and plus30 = GButton.button ~label:"+30m" ~packing:(strip#attach ~top ~left:7) () in
ignore (b#connect#clicked ~callback:mark);
(* User buttons *)
let hbox = GPack.hbox ~packing:framevb#add () in
- { agl=agl; gauge=gauge ; labels= !strip_labels; buttons_box = hbox}
-
-
-(** set a label *)
-let set_label strip name value =
- try
- let _eb, l = List.assoc (name^"_value") strip.labels in
- if l#text <> value then
- l#set_label value
- with
- Not_found ->
- Printf.fprintf stderr "Strip.set_label: '%s' unknown\n%!" name
-
-(** set a color *)
-let set_color strip name color =
- let eb, _l = List.assoc (name^"_value") strip.labels in
- eb#coerce#misc#modify_bg [`NORMAL, `NAME color]
-
-
-let set_gauge = fun ?(color="green") gauge v_min v_max value string ->
- let {Gtk.width=width; height=height} = gauge#misc#allocation in
- let dr = GDraw.pixmap ~width ~height ~window:gauge () in
- dr#set_foreground (`NAME "orange");
- dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
-
- let f = (value -. v_min) /. (v_max -. v_min) in
- let f = max 0. (min 1. f) in
- let h = truncate (float height *. f) in
- dr#set_foreground (`NAME color);
- dr#rectangle ~x:0 ~y:(height-h) ~width ~height:h ~filled:true ();
-
- let context = gauge#misc#create_pango_context in
- let layout = context#create_layout in
- Pango.Layout.set_text layout string;
- let (w,h) = Pango.Layout.get_pixel_size layout in
- dr#put_layout ~x:((width-w)/2) ~y:((height-h)/2) ~fore:`BLACK layout;
-
- (new GDraw.drawable gauge#misc#window)#put_pixmap ~x:0 ~y:0 dr#pixmap
-
-
-
-(** set the battery *)
-let set_bat ?color strip value =
- set_gauge ?color strip.gauge bat_min bat_max value (string_of_float value)
-
-
-(** set the AGL *)
-let set_agl ?color strip value =
- set_gauge ?color strip.agl 0. agl_max value (Printf.sprintf "%3.0f" value)
-
-
-let add_widget = fun strip widget ->
- strip.buttons_box#add widget
+ object
+ method set_agl ?color value = set_agl ?color agl value
+ method set_bat ?color value = set_bat ?color gauge value
+ method set_label name value = set_label !strip_labels name value
+ method set_color name value = set_color !strip_labels name value
+ method add_widget w = add_widget hbox w
+ method connect_shift_alt callback =
+ ignore (plus5#connect#clicked (fun () -> callback 5.));
+ ignore (plus30#connect#clicked (fun () -> callback 30.));
+ ignore (minus5#connect#clicked (fun () -> callback (-5.)))
+ end
diff --git a/sw/ground_segment/cockpit/strip.mli b/sw/ground_segment/cockpit/strip.mli
index f5ace9dccb..eebfc7a546 100644
--- a/sw/ground_segment/cockpit/strip.mli
+++ b/sw/ground_segment/cockpit/strip.mli
@@ -1,11 +1,19 @@
-type t
+type t =
+ < add_widget : GObj.widget -> unit;
+ connect_shift_alt : (float -> unit) -> unit;
+ set_agl : ?color:string -> float -> unit;
+ set_bat : ?color:string -> float -> unit;
+ set_color : string -> string -> unit;
+ set_label : string -> string -> unit >
+
+
+val scrolled : GBin.scrolled_window
+
val add :
Pprz.values ->
string ->
- (unit -> 'a) -> (unit -> unit) -> (unit -> unit) -> (unit -> unit) -> t
-val set_label : t -> string -> string -> unit
-val set_color : t -> string -> string -> unit
-val set_bat : ?color:string -> t -> float -> unit
-val set_agl : ?color:string -> t -> float -> unit
-val scrolled : GBin.scrolled_window
-val add_widget : t -> GObj.widget -> unit
+ (unit -> 'a) -> (unit -> unit) -> (unit -> unit) ->
+ t
+
+
+
diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index d25be8582f..84369c7377 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/sw/ground_segment/tmtc/aircraft.ml
@@ -105,6 +105,7 @@ type aircraft = {
mutable gaz_mode : int;
mutable lateral_mode : int;
mutable horizontal_mode : int;
+ mutable periodic_callbacks : Glib.Timeout.id list;
cam : ac_cam;
mutable gps_mode : int;
inflight_calib : inflight_calib;
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index a249a2e0fd..c77a036266 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/sw/ground_segment/tmtc/aircraft.mli
@@ -92,6 +92,7 @@ type aircraft = {
mutable gaz_mode : int;
mutable lateral_mode : int;
mutable horizontal_mode : int;
+ mutable periodic_callbacks : Glib.Timeout.id list;
cam : ac_cam;
mutable gps_mode : int;
inflight_calib : inflight_calib;
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index e82771b2ab..f73f05462d 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -575,7 +575,7 @@ let new_aircraft = fun id ->
gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0;
throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 42.; amp = 0.; energy = 0; ap_mode= -1; agl = 0.;
gaz_mode= -1; lateral_mode= -1;
- gps_mode =0;
+ gps_mode =0; periodic_callbacks = [];
desired_altitude = 0.;
desired_climb = 0.;
pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
@@ -607,7 +607,11 @@ let wind_clear = fun _sender vs ->
Wind.clear (Pprz.string_assoc "ac_id" vs)
let periodic = fun period cb ->
- ignore (Glib.Timeout.add period (fun () -> cb (); true))
+ Glib.Timeout.add period (fun () -> cb (); true)
+
+
+let register_periodic = fun ac x ->
+ ac.periodic_callbacks <- x :: ac.periodic_callbacks
(** add the periodic airprox check for the aircraft (name) on all aircraft *)
(** already known *)
@@ -635,15 +639,15 @@ let periodic_airprox_check = fun name ->
List.iter
(fun ac ->
- periodic aircraft_alerts_period (fun () -> check_airprox ac)
+ register_periodic thisac (periodic aircraft_alerts_period (fun () -> check_airprox ac))
) list_ac
let register_aircraft = fun name a ->
Hashtbl.add aircrafts name a;
- periodic aircraft_msg_period (fun () -> send_aircraft_msg name);
- periodic aircraft_alerts_period (fun () -> check_alerts a);
+ register_periodic a (periodic aircraft_msg_period (fun () -> send_aircraft_msg name));
+ register_periodic a (periodic aircraft_alerts_period (fun () -> check_alerts a));
periodic_airprox_check name;
- periodic wind_msg_period (fun () -> send_wind a);
+ register_periodic a (periodic wind_msg_period (fun () -> send_wind a));
Wind.new_ac name 1000;
ignore(Ground_Pprz.message_bind "WIND_CLEAR" wind_clear)
diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index f34bf918a7..26626a695d 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -185,7 +185,7 @@ class waypoint = fun (wpts_group:group) (name :string) ?(alt=0.) wgs84 ->
(* Update AGL on pos or alt change *)
let callback = fun _ ->
try
- let wgs84 = LL.of_string (sprintf "WGS84 %s" e_pos#text) in
+ let wgs84 = wgs84_of_string !selected_georef e_pos#text in
let agl = float_of_string ea#text -. float (try Srtm.of_wgs84 wgs84 with _ -> 0) in
agl_lab#set_text (sprintf " AGL: %4.0fm" agl)
with _ -> ()
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index e10622c42d..0363679e8a 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -244,7 +244,7 @@ let rec index_stage = fun x ->
| "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" ->
incr stage;
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
- | "survey" | "survey_rectangle" ->
+ | "survey" | "survey_rectangle" | "eight" ->
incr stage; incr stage;
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
| "exception" ->
@@ -398,6 +398,19 @@ let rec print_stage = fun index_of_waypoints sectors x ->
end;
output_cam_mode x index_of_waypoints;
lprintf "return;\n"
+ | "eight" ->
+ stage ();
+ lprintf "nav_eight_init();\n";
+ lprintf "NextStage();\n";
+ left ();
+ stage ();
+ let center = get_index_waypoint (ExtXml.attrib x "center") index_of_waypoints
+ and turn_about = get_index_waypoint (ExtXml.attrib x "turn_around") index_of_waypoints in
+ let r = parsed_attrib x "radius" in
+ let _vmode = output_vmode x center "" in
+ lprintf "Eight(%s, %s, %s);\n" center turn_about r;
+ output_cam_mode x index_of_waypoints;
+ lprintf "return;\n"
| "set" ->
stage ();
let var = ExtXml.attrib x "var"