mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 00:53:41 +08:00
*** empty log message ***
This commit is contained in:
Pascal Brisset
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* 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 cam.c
|
||||
* \brief Pan/Tilt camera library
|
||||
*
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include "cam.h"
|
||||
#include "nav.h"
|
||||
#include "autopilot.h"
|
||||
#include "flight_plan.h"
|
||||
#include "estimator.h"
|
||||
#include "inter_mcu.h"
|
||||
#include "nav.h"
|
||||
|
||||
#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
|
||||
#define DELTA_ALPHA 0.2
|
||||
|
||||
#define MAX_CAM_ROLL M_PI/2
|
||||
|
||||
float cam_roll_phi; /* radian */
|
||||
float phi_c; /* radian */
|
||||
float theta_c; /* have to be defined for telemetry message */
|
||||
|
||||
float target_x, target_y, target_alt;
|
||||
|
||||
#ifdef MOBILE_CAM
|
||||
|
||||
#define MODE_MANUAL 0
|
||||
#define MODE_STABILIZED 1
|
||||
|
||||
uint8_t cam_roll_mode;
|
||||
bool_t cam_roll_switch;
|
||||
|
||||
void cam_init( void ) {
|
||||
cam_roll_switch = 0;
|
||||
#ifdef VIDEO_SWITCH_PIN
|
||||
IO0DIR |= _BV(VIDEO_SWITCH_PIN);
|
||||
IO0CLR = _BV(VIDEO_SWITCH_PIN);
|
||||
#endif
|
||||
}
|
||||
|
||||
void cam_periodic( void ) {
|
||||
switch (cam_roll_mode) {
|
||||
case MODE_STABILIZED:
|
||||
phi_c = cam_roll_phi + estimator_phi;
|
||||
break;
|
||||
case MODE_MANUAL:
|
||||
phi_c = cam_roll_phi;
|
||||
break;
|
||||
default:
|
||||
phi_c = 0;
|
||||
}
|
||||
ap_state->commands[COMMAND_CAM_ROLL] = TRIM_PPRZ(phi_c * MAX_PPRZ / RadOfDeg(CAM_PHI_MAX_DEG));
|
||||
}
|
||||
|
||||
#endif // MOBILE_CAM
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef CAM_ROLL_H
|
||||
#define CAM_ROLL_H
|
||||
|
||||
extern uint8_t cam_roll_mode;
|
||||
extern float cam_roll_phi;
|
||||
extern bool_t cam_roll_switch;
|
||||
|
||||
#define cam_roll_SetPhiDeg(_deg) { cam_roll_phi = RadOfDeg(_deg); }
|
||||
|
||||
#define cam_roll_Switch(_x) { cam_roll_switch = _x; if (_x) IO0SET = _BV(VIDEO_SWITCH_PIN); else IO0CLR = _BV(VIDEO_SWITCH_PIN); }
|
||||
|
||||
|
||||
#endif /* CAM_ROLL_H */
|
||||
@@ -32,6 +32,9 @@ play : play.ml
|
||||
plotter : plotter.ml
|
||||
$(OCAMLC) $(INCLUDES) -custom -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma -I +lablgtk2 -I ../lib/ocaml lablgtk.cma lib-pprz.cma gtkInit.cmo $^
|
||||
|
||||
plot : plot.ml
|
||||
$(OCAMLC) $(INCLUDES) -custom -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma -I +lablgtk2 -I ../lib/ocaml lablgtk.cma lib-pprz.cma gtkInit.cmo $^
|
||||
|
||||
|
||||
|
||||
pt : ahrsview imuview ahrs2fg
|
||||
@@ -105,4 +108,4 @@ ahrs2fg: ahrs2fg.c network.c flight_gear.c utils.c
|
||||
|
||||
|
||||
ivy_example: ivy_example.c
|
||||
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
|
||||
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
|
||||
|
||||
@@ -0,0 +1,278 @@
|
||||
(*
|
||||
* $Id$
|
||||
*
|
||||
* Real time plotter
|
||||
*
|
||||
* Copyright (C) 2007- ENAC, Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*)
|
||||
|
||||
open Printf
|
||||
|
||||
|
||||
module P = Pprz.Messages (struct let name = "telemetry" end)
|
||||
|
||||
|
||||
let double__ =
|
||||
let underscore = Str.regexp "_" in
|
||||
fun s -> Str.global_replace underscore "__" s
|
||||
|
||||
|
||||
let (//) = Filename.concat
|
||||
|
||||
let colors = [|"red"; "blue"; "green"; "orange"; "purple"; "magenta"|]
|
||||
|
||||
let labelled_entry = fun ?width_chars text value (h:GPack.box) ->
|
||||
let label = GMisc.label ~text ~packing:h#pack () in
|
||||
label, GEdit.entry ?width_chars ~text:value ~packing:h#pack ()
|
||||
|
||||
class plot = fun ~width ~height ~packing () ->
|
||||
let da = GMisc.drawing_area ~width ~height ~show:true ~packing () in
|
||||
let curves = Hashtbl.create 3 in
|
||||
object (self)
|
||||
val mutable min_x = max_float
|
||||
val mutable max_x = min_float
|
||||
val mutable min = max_float
|
||||
val mutable max = min_float
|
||||
val mutable color_index = 0
|
||||
val mutable csts = ([] : float list)
|
||||
val mutable auto_scale = true
|
||||
|
||||
method auto_scale = auto_scale
|
||||
method set_auto_scale = fun x -> auto_scale <- x
|
||||
method min = min
|
||||
method set_min = fun x -> min <- x
|
||||
method max = max
|
||||
method set_max = fun x -> max <- x
|
||||
|
||||
method destroy = fun () -> ()
|
||||
|
||||
method drawing_area = da
|
||||
|
||||
method add_cst = fun v ->
|
||||
csts <- v :: csts
|
||||
|
||||
method delete_cst = fun v ->
|
||||
csts <- List.filter (fun x -> x <> v) csts
|
||||
|
||||
method add_curve = fun (name:string) (values:(float*float) array) ->
|
||||
Hashtbl.add curves name values
|
||||
|
||||
method delete_curve = fun (name:string) ->
|
||||
Hashtbl.remove curves name
|
||||
|
||||
method update_curves = fun () ->
|
||||
let {Gtk.width=width; height=height} = da#misc#allocation in
|
||||
let dr = GDraw.pixmap ~width ~height ~window:da () in
|
||||
dr#set_foreground (`NAME "white");
|
||||
dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
|
||||
|
||||
(* Time Graduations *)
|
||||
let context = da#misc#create_pango_context in
|
||||
context#set_font_by_name "sans 10 ";
|
||||
let layout = context#create_layout in
|
||||
|
||||
let f = fun x y s ->
|
||||
Pango.Layout.set_text layout s;
|
||||
let (w, h) = Pango.Layout.get_pixel_size layout in
|
||||
dr#put_layout ~x ~y:(y-h) ~fore:`BLACK layout in
|
||||
()
|
||||
end
|
||||
|
||||
|
||||
(* Indexed by log *)
|
||||
let logs = Hashtbl.create 3
|
||||
|
||||
let pprz_float = function
|
||||
Pprz.Int i -> float i
|
||||
| Pprz.Float f -> f
|
||||
| Pprz.Int32 i -> Int32.to_float i
|
||||
| Pprz.String s -> float_of_string s
|
||||
| Pprz.Array _ -> failwith "pprz_float"
|
||||
|
||||
|
||||
let load_log = fun (plot:plot) (menubar:GMenu.menu_shell GMenu.factory) xml_file ->
|
||||
let xml = Xml.parse_file xml_file in
|
||||
let data_file = ExtXml.attrib xml "data_file" in
|
||||
|
||||
let f = Ocaml_tools.find_file [Filename.dirname xml_file] data_file in
|
||||
let f = Ocaml_tools.open_compress f in
|
||||
let acs = Hashtbl.create 3 in (* indexed by A/C *)
|
||||
Hashtbl.add logs data_file acs;
|
||||
try
|
||||
while true do
|
||||
let l = input_line f in
|
||||
try
|
||||
Scanf.sscanf l "%f %s %[^\n]"
|
||||
(fun t ac m ->
|
||||
if not (Hashtbl.mem acs ac) then
|
||||
Hashtbl.add acs ac (Hashtbl.create 97);
|
||||
let msgs = Hashtbl.find acs ac in
|
||||
|
||||
let msg_id, vs = P.values_of_string m in
|
||||
if not (Hashtbl.mem msgs msg_id) then
|
||||
Hashtbl.add msgs msg_id (Hashtbl.create 97);
|
||||
let fields = Hashtbl.find msgs msg_id in
|
||||
|
||||
List.iter (fun (f, v) -> Hashtbl.add fields f (t, v)) vs
|
||||
)
|
||||
with
|
||||
exc -> prerr_endline (Printexc.to_string exc)
|
||||
done
|
||||
with
|
||||
End_of_file ->
|
||||
close_in f;
|
||||
(* Build the A/C menus *)
|
||||
Hashtbl.iter
|
||||
(fun ac msgs ->
|
||||
let menu_name = sprintf "%s:%s" (String.sub data_file 0 18) ac in
|
||||
let menu = menubar#add_submenu menu_name in
|
||||
let menu_fact = new GMenu.factory menu in
|
||||
let l = ref [] in
|
||||
Hashtbl.iter (fun msg fields -> l := (P.message_of_id msg, fields):: !l) msgs;
|
||||
let l = List.sort (fun (a,_) (b,_) -> compare a b) !l in
|
||||
|
||||
(* Build the msg menus *)
|
||||
List.iter
|
||||
(fun (msg, fields) ->
|
||||
let menu = menu_fact#add_submenu (double__ msg.Pprz.name) in
|
||||
let menu_fact = new GMenu.factory menu in
|
||||
let l = ref [] in
|
||||
Hashtbl.iter
|
||||
(fun f v -> if not (List.mem f !l) then l := f :: !l)
|
||||
fields;
|
||||
let l = List.sort compare !l in
|
||||
(* Build the field menus *)
|
||||
List.iter
|
||||
(fun f ->
|
||||
let values = Hashtbl.find_all fields f in
|
||||
let values = List.map (fun (t, v) -> (t, pprz_float v)) values in
|
||||
let values = Array.of_list values in
|
||||
Array.sort compare values;
|
||||
let name = sprintf "%s:%s:%s:%s" menu_name ac msg.Pprz.name f in
|
||||
let callback = fun _ -> plot#add_curve name values; plot#update_curves () in
|
||||
ignore (menu_fact#add_item ~callback (double__ f)))
|
||||
l
|
||||
)
|
||||
l
|
||||
)
|
||||
acs
|
||||
|
||||
|
||||
|
||||
let file_dialog ~title ~callback () =
|
||||
let sel = GWindow.file_selection ~title ~filename:"var/logs/*.log" ~modal:true () in
|
||||
ignore (sel#cancel_button#connect#clicked ~callback:sel#destroy);
|
||||
ignore
|
||||
(sel#ok_button#connect#clicked
|
||||
~callback:(fun () ->
|
||||
let name = sel#filename in
|
||||
sel#destroy ();
|
||||
callback name));
|
||||
sel#show ()
|
||||
|
||||
|
||||
let open_log = fun plot menubar () ->
|
||||
ignore (file_dialog ~title:"Open Log" ~callback:(fun name -> load_log plot menubar name) ())
|
||||
|
||||
|
||||
let rec plot_window = fun init ->
|
||||
let plotter = GWindow.window ~allow_shrink:true ~title:"Log Plotter" () in
|
||||
let vbox = GPack.vbox ~packing:plotter#add () in
|
||||
let quit = fun () -> GMain.Main.quit (); exit 0 in
|
||||
|
||||
let tooltips = GData.tooltips () in
|
||||
|
||||
let menubar = GMenu.menu_bar ~packing:vbox#pack () in
|
||||
let factory = new GMenu.factory menubar in
|
||||
let accel_group = factory#accel_group in
|
||||
let file_menu = factory#add_submenu "File" in
|
||||
let file_menu_fact = new GMenu.factory file_menu ~accel_group in
|
||||
|
||||
let width = 900 and height = 200 in
|
||||
let plot = new plot ~width ~height ~packing:(vbox#pack ~expand:true) () in
|
||||
|
||||
ignore (file_menu_fact#add_item "Open Log" ~key:GdkKeysyms._O ~callback:(open_log plot factory));
|
||||
|
||||
ignore (file_menu_fact#add_item "New" ~key:GdkKeysyms._N ~callback:(fun () -> plot_window []));
|
||||
(*
|
||||
let close = fun () -> plotter#destroy () in
|
||||
ignore (file_menu_fact#add_item "Close" ~key:GdkKeysyms._W ~callback:close); *)
|
||||
|
||||
ignore (file_menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit);
|
||||
let curves_menu = factory#add_submenu "Curves" in
|
||||
let curves_menu_fact = new GMenu.factory curves_menu in
|
||||
|
||||
let h = GPack.hbox ~packing:vbox#pack () in
|
||||
|
||||
tooltips#set_tip plot#drawing_area#coerce ~text:"Drop a messages field here to draw it";
|
||||
ignore (plotter#connect#destroy ~callback:(fun () -> plot#destroy ()));
|
||||
|
||||
(* Auto Scale *)
|
||||
let auto_scale = GButton.check_button ~label:"Auto Scale" ~active:true ~packing:h#pack () in
|
||||
ignore (auto_scale#connect#toggled (fun () -> plot#set_auto_scale auto_scale#active));
|
||||
let _, min_entry= labelled_entry ~width_chars:5 "Min" "" h in
|
||||
let _, max_entry= labelled_entry ~width_chars:5 "Max" "" h in
|
||||
ignore (GMain.Timeout.add 1000 (fun () -> if plot#auto_scale then begin min_entry#set_text (string_of_float plot#min); max_entry#set_text (string_of_float plot#max) end; true));
|
||||
ignore (min_entry#connect#activate ~callback:(fun () -> if not plot#auto_scale then plot#set_min (float_of_string min_entry#text)));
|
||||
ignore (max_entry#connect#activate ~callback:(fun () -> if not plot#auto_scale then plot#set_max (float_of_string max_entry#text)));
|
||||
|
||||
(* X range *)
|
||||
let _, _x_min_entry= labelled_entry ~width_chars:5 "Min" "" h in
|
||||
let _, _x_max_entry= labelled_entry ~width_chars:5 "Max" "" h in
|
||||
|
||||
(* Constants *)
|
||||
let _, cst = labelled_entry ~width_chars:5 "Constant:" "" h in
|
||||
let add_cst = fun s ->
|
||||
let v = float_of_string s in
|
||||
plot#add_cst v;
|
||||
let eb = GBin.event_box ~width:10 ~height:10 () in
|
||||
eb#coerce#misc#modify_bg [`NORMAL, `NAME "black"];
|
||||
let item = curves_menu_fact#add_image_item ~image:eb#coerce ~label:s () in
|
||||
|
||||
let delete = fun () ->
|
||||
plot#delete_cst v;
|
||||
curves_menu#remove (item :> GMenu.menu_item) in
|
||||
ignore (item#connect#activate ~callback:delete);
|
||||
in
|
||||
ignore (cst#connect#activate ~callback:(fun () ->add_cst cst#text));
|
||||
tooltips#set_tip cst#coerce ~text:"Enter value for a constant curve";
|
||||
|
||||
List.iter (fun f -> load_log plot factory f) init;
|
||||
|
||||
plotter#add_accel_group accel_group;
|
||||
plotter#show ()
|
||||
|
||||
|
||||
|
||||
|
||||
let _ =
|
||||
let logs = ref [] in
|
||||
Arg.parse
|
||||
[]
|
||||
(fun x -> logs := x :: !logs)
|
||||
"Usage: ";
|
||||
|
||||
plot_window !logs;
|
||||
|
||||
let loop = Glib.Main.create true in
|
||||
while Glib.Main.is_running loop do
|
||||
ignore (Glib.Main.iteration true)
|
||||
done
|
||||
Reference in New Issue
Block a user