diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 0000000000..88464f7ed6 --- /dev/null +++ b/AUTHORS @@ -0,0 +1,2 @@ +Pascal Brisset +Antoine Drouin diff --git a/BUGS b/BUGS new file mode 100644 index 0000000000..beaeeef805 --- /dev/null +++ b/BUGS @@ -0,0 +1,15 @@ +paparazzi.pl +CpGui et CpSeesionManager n'ont pas les meme variables !!! + + + + +receive +ne cree pas son repertoire de log et meurt + + + + + +visu3D +ne trouve pas son fichier d'aide. a mettre dans conf ?? diff --git a/COPYING b/COPYING new file mode 100644 index 0000000000..916d1f0f28 --- /dev/null +++ b/COPYING @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc. + 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Library General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + Appendix: How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) 19yy + + This program 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 of the License, or + (at your option) any later version. + + This program 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 this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) 19yy name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Library General +Public License instead of this License. diff --git a/Makefile b/Makefile new file mode 100644 index 0000000000..cc01450549 --- /dev/null +++ b/Makefile @@ -0,0 +1,119 @@ +# Paparazzi main $Id$ +# Copyright (C) 2004 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. + +include conf/Makefile.local + +LIB=sw/lib +AIRBORNE=sw/airborne +CONFIGURATOR=sw/configurator +FBW=$(AIRBORNE)/fly_by_wire +AP=$(AIRBORNE)/autopilot +COCKPIT=sw/ground_segment/cockpit +TMTC=sw/ground_segment/tmtc +WIND=sw/ground_segment/wind +VISU3D=sw/ground_segment/visu3d +LOGALIZER=sw/logalizer +SIMULATOR=sw/simulator +MAKE=make + +static : lib tools configurator cockpit tmtc visu3d logalizer sim_static wind + +configure : configurator + PAPARAZZI_DIR=`pwd` $(CONFIGURATOR)/configurator + +lib: + cd $(LIB)/ocaml; $(MAKE) + cd $(LIB)/perl; $(MAKE) + +tools: + cd $(TOOLS); make + +logalizer: lib + cd $(LOGALIZER); $(MAKE) + +configurator: lib + cd $(CONFIGURATOR); $(MAKE) + +sim_static : + cd $(SIMULATOR); $(MAKE) + +sim_sitl : + cd $(SIMULATOR); $(MAKE) sim_sitl + +fbw fly_by_wire: + cd $(FBW); $(MAKE) all + +ap autopilot: + cd $(AP); $(MAKE) all + +upload_fbw: fbw + cd $(FBW); $(MAKE) upload + +upload_ap: ap + cd $(AP); $(MAKE) upload + +erase_fbw: + cd $(FBW); $(MAKE) erase + +erase_ap: + cd $(AP); $(MAKE) erase + +airborne: fbw ap + +cockpit: lib + cd $(COCKPIT); $(MAKE) all + +tmtc: lib + cd $(TMTC); $(MAKE) all + +visu3d: lib + cd $(VISU3D); $(MAKE) +wind: + cd $(WIND); $(MAKE) + +receive: tmtc + $(TMTC)/receive + +static_h : + make -f Makefile.gen + +ac_h : + $(TOOLS)/gen_aircraft.out $(AIRCRAFT) + +ac: static_h ac_h ap fbw sim_sitl + +clean_ac : + rm -fr $(PAPARAZZI_HOME)/var/$(AIRCRAFT) + +run_sitl : + $(PAPARAZZI_HOME)/var/$(AIRCRAFT)/sim/simsitl.out + +t1: ac + +install : static t1 + ./Makefile.pl -install -destdir $(DESTDIR) + +uninstall : + ./Makefile.pl -uninstall -destdir $(DESTDIR) + +clean: + find . -name Makefile -mindepth 2 -exec sh -c '$(MAKE) -C `dirname {}` $@' \; + find . -name '*~' -exec rm -f {} \; + diff --git a/Makefile.ac b/Makefile.ac new file mode 100644 index 0000000000..f81cb227d3 --- /dev/null +++ b/Makefile.ac @@ -0,0 +1,53 @@ +# Paparazzi main $Id$ +# Copyright (C) 2004 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. + +# Preprocessing of XML configuration files + +include conf/Makefile.local + +CONF=$(PAPARAZZI_HOME)/conf +CONF_XML=$(CONF)/conf.xml +ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) +AIRFRAME_H=$(ACINCLUDE)/airframe.h +RADIO_H=$(ACINCLUDE)/radio.h +FLIGHT_PLAN_H=$(ACINCLUDE)/flight_plan.h +INFLIGHT_CALIB_H=$(ACINCLUDE)/inflight_calib.h + +all: $(AIRFRAME_H) $(RADIO_H) $(FLIGHT_PLAN_H) $(INFLIGHT_CALIB_H) + echo $(AIRFRAME_H) $(CONF)/$(AIRFRAME) + +$(AIRFRAME_H) : $(CONF)/$(AIRFRAME) $(CONF_XML) + $(TOOLS)/gen_airframe.out $(AIRCRAFT) $< > /tmp/airframe.h + mv /tmp/airframe.h $@ + +$(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML) + $(TOOLS)/gen_radio.out $< > /tmp/radio.h + mv /tmp/radio.h $@ + +$(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) + $(TOOLS)/gen_flight_plan.out $< > /tmp/fp.h + mv /tmp/fp.h $@ + +$(INFLIGHT_CALIB_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) + $(TOOLS)/gen_calib.out $< > /tmp/c.h + mv /tmp/c.h $@ + +clean : + rm -f $(ACINCLUDE)/*.h diff --git a/Makefile.gen b/Makefile.gen new file mode 100644 index 0000000000..a1842aa770 --- /dev/null +++ b/Makefile.gen @@ -0,0 +1,47 @@ +# Paparazzi main $Id$ +# Copyright (C) 2004 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. + +# Preprocessing of XML configuration files + +include conf/Makefile.local + +CONF=conf +XML_GET=sw/lib/ocaml/xml_get.out + + +STATICINCLUDE =$(PAPARAZZI_HOME)/var/include +MESSAGES_H=$(STATICINCLUDE)/messages.h +UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h +MESSAGES_XML = $(CONF)/messages.xml +UBX_XML = $(CONF)/ubx.xml + + +static: $(MESSAGES_H) $(UBX_PROTOCOL_H) + +$(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML) + $(TOOLS)/gen_messages.out $< telemetry_ap > /tmp/messages.h + mv /tmp/messages.h $@ + +$(UBX_PROTOCOL_H) : $(UBX_XML) $(CONF_XML) + $(TOOLS)/gen_ubx.out $< > /tmp/ubx.h + mv /tmp/ubx.h $@ + +clean : + rm -f $(H_OF_XML) diff --git a/Makefile.pl b/Makefile.pl new file mode 100755 index 0000000000..c9a1bdea36 --- /dev/null +++ b/Makefile.pl @@ -0,0 +1,72 @@ +#!/usr/bin/perl -w + +use strict; +use File::Basename; +use Getopt::Long; +use Data::Dumper; +use XML::DOM; + +my $destdir="/usr"; +my $install = undef; +my $uninstall = undef; +my @sections; + +GetOptions("install" => \$install, + "uninstall" => \$uninstall, + "destdir=s" => \$destdir); + +read_xml("./conf/install.xml"); + +foreach my $section (@sections) { + my ($inst_dir, $files) = @{$section}; + do_install($inst_dir, $files) if ($install); + do_uninstall($inst_dir, $files) if ($uninstall); +} + +sub do_install { + my ($dest_dir, $files) = @_; + `install -d $dest_dir`;# or warn "creation of directory $dest_dir failed"; + foreach my $file (@{$files}) { + my ($path, $new_name) = @{$file}; + print "installing file $path in $dest_dir ".($new_name?"as $new_name":"")."\n"; + my $cmd = "install $path $dest_dir".($new_name?"/$new_name":""); + `$cmd`;# or warn "intall of $path failed"; + } +} + +sub do_uninstall { + my ($dest_dir, $files) = @_; + foreach my $file (@{$files}) { + my ($path, $new_name) = @{$file}; + my $to_be_removed = $dest_dir."/".($new_name?"$new_name":basename($path)); + print "removing $to_be_removed\n"; + `rm -f $to_be_removed`; + } +} + +sub read_xml { + my ($filename) = @_; + my $parser = XML::DOM::Parser->new(); + my $doc = $parser->parsefile($filename); + my $cp = $doc->getElementsByTagName("install")->[0]; + my $sections = $cp->getElementsByTagName("section"); + foreach my $section (@{$sections}) { + my $section_name = $section->getAttribute('name'); + my $dest_loc = $destdir."/".$section->getAttribute('dest'); + my $files = $section->getElementsByTagName("file"); + my $file_a = []; + foreach my $file (@{$files}) { + push @{$file_a}, [$file->getAttribute('name'), $file->getAttribute('new_name')]; + } + my $dirs = $section->getElementsByTagName("directory"); + foreach my $dir (@{$dirs}) { + my $dirname=$dir->getAttribute('name'); + opendir(DIR,$dirname); + my @dir_files = grep { -f "$dirname/$_" } readdir(DIR); + map { s#^(.*)#$dirname/$1# } @dir_files; + closedir(DIR); + push @{$file_a}, @dir_files; + } + push @sections, [$dest_loc, $file_a]; + } +} diff --git a/README b/README new file mode 100644 index 0000000000..8da310bac0 --- /dev/null +++ b/README @@ -0,0 +1,208 @@ +# Paparazzi $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. +# + +Intro +----- + +Paparazzi is an attempt to develop a cheap fixed wing UAV (Unmanned Air +Vehicle). As of today we have successfully flown autonomously several small +electro powered fixed wing aircraft: the Twinstar and the Microjet of +Multiplex. + +Up to date informations are available from the website + + www.nongnu.org/paparazzi + +and from the mailing list. + +Directories description: +----------------------- + +conf: the configuration directory (airframe, radio, ... descriptions). YOU HAVE +TO EDIT THERE the Makefile.local file + +data: where to put read-only data (e.g. maps and terrain files) + +doc: the paparazzi documentation. + +hw: hardware definitions (electronic schemas, PCBs, ...) + +sw: software (onboard, ground station, simulation, ...) + + +Required Software +----------------- + - AVR micro-controller development environnment (avr-gcc, uisp, avr-libc) + - OCaml (ocaml.org), xml-light library (http://tech.motion-twin.com/xmllight) + - gcc, GTK2, Glib2, libgnomecanvas, libxml2 + - ... + +For Debian users: Required packages are available at + http://www.rechercher.enac.fr/paparazzi/debian +Installation of the meta-package "paparazzi" will install everything +needed (if something is missing, please ask) + +Compilation +----------- + + 0) Configuration. Default PAPARAZZI_HOME is $(HOME)/PAPARAZZI. You +can change it by setting an environment variable + + 1) "make" in the top directory + + 2) Set the PAPARAZZI_SRC environment variable to the top directory +(default is /usr/share/paparazzi) + + 2) "make init" creates a directory $PAPARAZZI_HOME for your own files. +Configure there the conf/conf.xml + + 3) "make ac AIRCRAFT=" compiles everything for the specified +aircraft (default is twinstar2 for which conf files are provided) and +set the files in $PAPARAZZI_HOME/var/ + + 5) "make clean_ac AIRCRAFT=" cleans files for the specified aircraft. + + 6) $PAPARAZZI_HOME/var//sim/simsitl.out runs the soft simulator. + +Uploading of the embedded software +---------------------------------- + + 1) Power the flight controller board. Plug the pc-link to the board + and to the host parallel port. + + 2) Upload with + + make upload_fbw # Fly by wire + make upload_ap # Autopilot + + Important notes: + - The pclink must be switched accordingly with the target + - The "fly by wire" controller cannot be uploaded when the + "autopilot" controller is running. They cannot independently be + modified; then an upload of the fly by wire usually requires + + make erase_ap + make upload_fbw + make upload_ap + + +Running the ground segment monitoring +------------------------------------- + 1) The transmitter must be plugged to the flight controller and both must + be powered. + + 2) The ground modem must be powered and plugged to the antenna and + to the host (trough a serial port) + + 3) Launch the supervision + + sw/supervision/paparazzi.pl + + 4) Launch "receive", "cockpit", "map", ... + + +Log replay +---------- + 1) Run the supervision + + sw/supervision/paparazzi.pl + + 2) Launch "play", "cockpit", "map", ... + + +Software in the loop simulator +------------------------------ +This simulator allows to run the stabilization and navigation controllers +and play with the ground control station. + +0) Use the conf.xml and ground_segment.xml examples for the configuration. +Recompile everything to be sure you run what you want ("make clean; make" in +the top directory). The mission takes place in Braunschweig, Germany +(flight competion of EMAV'04). + +1) Run the "control panel" (sw/supervision/paparazzi.pl) + This window helps to launch the different components. + +2) Launch the "cockpit" to display flight parameters + +3) Launch "sim" (aircraft simulator) + You get two windows standing for + - The aircraft + - The radio-controller (RC, displayed as one slider for each channel, +even if some of them are buttons on the real RC) + +4) "Boot" the aircraft (button in the aircraft window) + The cockpit now displays some parameters. You can check + - The autopilot mode: "auto1" (stabilized manual mode) + - The altitude (on the right of the horizon) + - The speed: null (on the left of the horizon) + +5) "Launch" the aircraft (button in the aircraft window) + The speed is now 10m/s. + The altitude is going down: push the THROTTLE to go up ! + Ok, you were probably too slow: the aircraft went too far from HOME +and the autopilot mode is now "home" (cockpit window): it is going +back home automatically and you do not control anything with the RC. + +6) Launch "map" (from the control panel) + In this window, you can zoom with mouse wheel and pan with the middle +button. + The aicraft is going around the "HOME" waypoint. + Now, reset the autopilot mode with GAIN1 slider (push full left for +one second, put it back around 0 when "auto1" is displayed in the +cockpit window). The aircraft is going away: turn right or left with +the "ROLL" slider which directly controls the "roll" angle (set it to 0 +to go straight) + Look at the altitude. Control its variation with the "THROTTLE". + +7) Launch "mission" (from the control panel) + This window displays the flight plan the aircraft will follow in +autonomous mode. If your current altitude is realistic, the second +block should be active (if not, go up with more THROTTLE) + +8) Switch to autonomous mode "auto2" with the "MODE" slider (push right) + + The aircraft successively goes to waypoints 1 and 3 while trying to +stay at a constant altitude of 200m. The trajectory is better if you +active the automatic calibration of the attitude with the "LLS" slider +(with a large positive value, you get "ON" on the Cockpit window) + +9) Activate the next block of the flight plan with "GAIN1" slider (full left) + The active block is now the "height". On the "map" window (type +CRTL-C to clear the track), you can observe a red point (the "carrot") +which moves in front of the aircraft: it is the guide of the aircraft +(that you probably should consider as a donkey in this case), always +5 second before the aircraft on the desired track. + +10) Add some west wind (with the slider on the aircraft window) + 5m/s is an acceptable value for this approximative flight model. The +aircraft no longer can follow the "height" trajectory. + +11) Activate the next block of the mission ("GAIN1" slider, full left) + In this "xyz" mode, you can control the carrot position with the +"YAW" (west-east) and "PITCH" (south-north) sliders: the slider value is +the speed of the carrot. + +12) Activate the next block + In this block, the aircraft follows a circle around the "HOME" +waypoint at a fixed distance. + +13) Close the control panel to quit diff --git a/TODO b/TODO new file mode 100644 index 0000000000..4a5bfe1082 --- /dev/null +++ b/TODO @@ -0,0 +1,96 @@ +dans flybywire +chop servo ne depends pas du servo + + +pour les missions +possibilité de "transformer" (rotation Z, translation XYZ) une mission + +possibilitite de faire la meme chose pour une partie des waypoints (on en a une partie pour les evolutions et une partie pour le circuit d'atterissage. On deplace ceux du circuit d'atterissage pour qu'il colle a la piste. et on deplace ceux des evolution pour etre en face du jury :) + + +On stocke la/les transformations et on peut avoir des missions communes muret/ricou + +des declarations de points locales aux blocs et des transformations par blocs + + +faire medit avec visu3d + +proposer d'ajouter un waypoint en relatif par rapport a un autre - et en coordonnees polaires (dist, QDM) + + + + + +l'interface du captureur de video - c'est aussi visu3d . Il a une liste de textures (photos) et on peut les transformer. La description est sauvées dans un fichier xml et peut etre rechargée. les photos vont dans var/photos + + +dans visu3d, il faudrait pouvoir pivoter en Z sur la position courante (en tenant compte du zoom ) + + +###################### + +logger les simus comme les vols - y penser en refaisant receive - code commun + +Pour les simus a plusieurs avions, il doit y avoir partage d'un certain nombre d'informations entre les differentes instances des simu (par exemple le vent) + +Dans un circle, il faut afficher le QDR - calcul au sol?? + +Pour les missions, il faudrait pouvoir dire . faire un cercle pendant 180° ou faire un cercle pendant n secondes. Il faudrait donc disposer du temps depuis le block et du de l'angle parcouru depuis le debut du cercle. +C'est pour faire un palier en haut de la monté. Pour laisser le terme accumulateur se recaler avant la descente. + +########################################### + +Sujet : procedure automatique d'interuption de vol pour microdrone + +-identifier des scenarios: + +cause de l'interuption : autonomie, meteo, defaillance systeme + +-modeliser la zone d'evolution et les autres contraintres (systemes defaillants, meteo) + +(dans un meeting, on veut a tout prix eviter le public, les routes etc...) + +- initialisation +- iteratif ? + +####################################### + +integrer les gazs pour estimer l'autonomie restante + + +########################################### + + +Sujet Drone Thales + +Les eleves (1A ou 2A) construisent un avion et apprennent a le faire voler d'ici juillet. + +commande PCB +commande composants radiospare/melexys/coronis/ublox +labo pour assembler (labo micro onde ?) + +commande garat (avion, moteur, servos, batteries, radiocommande etc....) +achat de petit outillage (dremel, fer a souder etc...) +assemblage au labo drone + +cours de pilotages sur le twinstar + +On leur donne les petits projets pendant l'année sur Paparazzi. + + + +############################################ + +mettre les projets enac sur la page web enac + +############################################ + + +Pourquoi on ne laisse pas message.xml modifiable, pour les taux de telemesure par exemple. + +on split les Makefiles + + +########### + +manque gerbmerge dans les dependance??? ha non ca n'existe pas... a packager ! diff --git a/conf/Makefile.avr b/conf/Makefile.avr new file mode 100644 index 0000000000..91d39d7b32 --- /dev/null +++ b/conf/Makefile.avr @@ -0,0 +1,140 @@ +# +# $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. +# + + +# +# This is the common Makefile for the avr-target. +# Edit the configuration part to suit your local install +# + +OBJDIR = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)/$(TARGET) + +CC = $(ATMELBIN)/avr-gcc -mmcu=$(ARCH) +LD = $(CC) $(ATMEL_LIBPATH) +SIZE = $(ATMELBIN)/avr-size +OBJCOPY = $(ATMELBIN)/avr-objcopy + + +SERIAL_FLAGS = \ + -dprog=avr910 \ + -dpart=auto \ + -dserial=/dev/ttyS0 \ + -dspeed=38400 \ + +ISP_FLAGS = \ + -dlpt=$(PROG_PORT) -dprog=stk200 -v=3 \ + +UISP = uisp +UISP_FLAGS = $(ISP_FLAGS) +#UISP_FLAGS = $(SERIAL_FLAGS) + + +# +# End of configuration part. +# + + +CFLAGS = \ + -W -Wall \ + $(ATMEL_INCLUDES) \ + $(INCLUDES) \ + -Wall \ + -Wstrict-prototypes \ + $(LOCAL_CFLAGS) \ + -O2 \ + +LDFLAGS = -lm \ + +# +# General rules +# + +all compile: $(OBJDIR)/$(TARGET).elf + +load upload: \ + $(TARGET).install + +$(TARGET).objs = \ + $($(TARGET).srcs:%.c=$(OBJDIR)/%.o) \ + +# +# Fuses +# + +rd_fuses: check_arch + $(UISP) $(ISP_FLAGS) --rd_fuses + +wr_fuses : check_arch + $(UISP) $(ISP_FLAGS) --wr_fuse_h=$(HIGH_FUSE) + $(UISP) $(ISP_FLAGS) --wr_fuse_l=$(LOW_FUSE) + $(UISP) $(ISP_FLAGS) --wr_fuse_e=$(EXT_FUSE) + $(UISP) $(ISP_FLAGS) --wr_lock=$(LOCK_FUSE) + + +$(OBJDIR)/%.elf: $($(TARGET).objs) + $(LD) \ + $(LOCAL_LDFLAGS) \ + $^ \ + -o $@ \ + $(LDFLAGS) + $(SIZE) $@ + + +$(OBJDIR)/%.s: %.c + $(CC) $(CFLAGS) -S -o $@ $< + +$(OBJDIR)/%.o: %.c + $(CC) $(CFLAGS) -c -o $@ $< + +$(OBJDIR)/%.hex: $(OBJDIR)/%.elf + $(OBJCOPY) -O ihex -R .eeprom $< $@ + + +%.install: $(OBJDIR)/%.hex check_arch + # stk200 needs to be erased first + $(UISP) $(UISP_FLAGS) --erase + $(UISP) $(UISP_FLAGS) --upload if=$< + +erase: check_arch + $(UISP) $(ISP_FLAGS) --erase + +check_arch : + if ($(UISP) $(UISP_FLAGS) 2>&1 | tr '[:upper:]' '[:lower:]' | grep $(ARCH)); then : ; else echo "Wrong architecture (mcu0 vs mcu1 ?)"; exit 1; fi + +avr_clean: + cd $(OBJDIR); rm -f *.hex *.elf *.out core *.o *.a *~ *.s *.cm* .depend + + +# +# Dependencies +# + +$(OBJDIR)/.depend: + $(CC) -M $(CFLAGS) $($(TARGET).srcs) > $@ + +ifneq ($(MAKECMDGOALS),clean) +ifneq ($(MAKECMDGOALS),erase) +-include $(OBJDIR)/.depend +endif +endif + + diff --git a/conf/Makefile.local b/conf/Makefile.local new file mode 100644 index 0000000000..8914f769d7 --- /dev/null +++ b/conf/Makefile.local @@ -0,0 +1,19 @@ +DESTDIR=/usr + +ifeq ($(PAPARAZZI_HOME),) +PAPARAZZI_HOME=$(HOME)/paparazzi +endif + +ifeq ($(PAPARAZZI_SRC),) +TOOLS=$(DESTDIR)/share/paparazzi/bin +else +TOOLS=$(PAPARAZZI_SRC)/sw/tools +endif + +AIRCRAFT=twinstar1 + +ATMELBIN = /usr/bin +ATMEL_INCLUDES = -I /usr/avr/include +ATMEL_LIBPATH = -B /usr/avr/lib/avr4 -B /usr/avr/lib/avr5 +PROG_PORT = /dev/parport0 + diff --git a/conf/airframes/microjet1.xml b/conf/airframes/microjet1.xml new file mode 100644 index 0000000000..5a3378d816 --- /dev/null +++ b/conf/airframes/microjet1.xml @@ -0,0 +1,64 @@ + +
+ +
+
+ + + + + +
+ + + + + + + + + + + + +
+ + +
+
+ + + + + + +
+
+ + + + +
+
+ + + + + +
+
+ + + +
+
+ + + + +
+
+ + +
+ diff --git a/conf/airframes/microjet2.xml b/conf/airframes/microjet2.xml new file mode 100644 index 0000000000..044f6f6fab --- /dev/null +++ b/conf/airframes/microjet2.xml @@ -0,0 +1,63 @@ + +
+ + +
+ + + + + + + + + + + + +
+ + +
+
+ + + + + + +
+
+ + + + +
+
+ + + + + +
+
+ + + +
+
+ + + + +
+
+ + + +
+
+ + +
+ diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml new file mode 100644 index 0000000000..ce87717acf --- /dev/null +++ b/conf/airframes/twinstar1.xml @@ -0,0 +1,61 @@ + +
+ + +
+ + + + + + + + + + + + + + + +
+ + +
+
+ + + + + + +
+
+ + + + +
+
+ + + + + +
+
+ + + +
+
+ + + + +
+
+ + +
+ diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml new file mode 100644 index 0000000000..4c27817376 --- /dev/null +++ b/conf/airframes/twinstar2.xml @@ -0,0 +1,61 @@ + +
+ + +
+ + + + + + + + + + + + + + + +
+ + +
+
+ + + + + + +
+
+ + + + +
+
+ + + + + +
+
+ + + +
+
+ + + + +
+
+ + +
+ diff --git a/conf/conf.xml b/conf/conf.xml new file mode 100644 index 0000000000..2f89ca9835 --- /dev/null +++ b/conf/conf.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/conf/control_panel.xml b/conf/control_panel.xml new file mode 100644 index 0000000000..ac78d3f94e --- /dev/null +++ b/conf/control_panel.xml @@ -0,0 +1,105 @@ + + + + +
+ + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + diff --git a/conf/control_panel.xml.sys b/conf/control_panel.xml.sys new file mode 100644 index 0000000000..990ff0818a --- /dev/null +++ b/conf/control_panel.xml.sys @@ -0,0 +1,84 @@ + + + + +
+ +
+ +
+ + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ + diff --git a/conf/flight_plans/circles.xml b/conf/flight_plans/circles.xml new file mode 100644 index 0000000000..ccfde0e8b1 --- /dev/null +++ b/conf/flight_plans/circles.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd new file mode 100644 index 0000000000..562d26e1d6 --- /dev/null +++ b/conf/flight_plans/flight_plan.dtd @@ -0,0 +1,116 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/hippo.xml b/conf/flight_plans/hippo.xml new file mode 100644 index 0000000000..1c578029e9 --- /dev/null +++ b/conf/flight_plans/hippo.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/huit.xml b/conf/flight_plans/huit.xml new file mode 100644 index 0000000000..dc93a0eb1a --- /dev/null +++ b/conf/flight_plans/huit.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/muret1.xml b/conf/flight_plans/muret1.xml new file mode 100644 index 0000000000..ff095d7e8a --- /dev/null +++ b/conf/flight_plans/muret1.xml @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/muret2.xml b/conf/flight_plans/muret2.xml new file mode 100644 index 0000000000..07f9ec994b --- /dev/null +++ b/conf/flight_plans/muret2.xml @@ -0,0 +1,92 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/muret3.xml b/conf/flight_plans/muret3.xml new file mode 100644 index 0000000000..07f9ec994b --- /dev/null +++ b/conf/flight_plans/muret3.xml @@ -0,0 +1,92 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/procedure.dtd b/conf/flight_plans/procedure.dtd new file mode 100644 index 0000000000..6a21c87d21 --- /dev/null +++ b/conf/flight_plans/procedure.dtd @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/hosts_wavecard.xml b/conf/hosts_wavecard.xml new file mode 100644 index 0000000000..c1bfc79bbc --- /dev/null +++ b/conf/hosts_wavecard.xml @@ -0,0 +1,9 @@ + + +
+ + + + + +
diff --git a/conf/install.xml b/conf/install.xml new file mode 100644 index 0000000000..630a53866c --- /dev/null +++ b/conf/install.xml @@ -0,0 +1,226 @@ + + +
+ +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + + + + + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ +
+ +
+ +
+ + diff --git a/conf/messages.xml b/conf/messages.xml new file mode 100644 index 0000000000..897d239767 --- /dev/null +++ b/conf/messages.xml @@ -0,0 +1,314 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/radios/cockpitMM.xml b/conf/radios/cockpitMM.xml new file mode 100644 index 0000000000..e6d87992c3 --- /dev/null +++ b/conf/radios/cockpitMM.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + diff --git a/conf/radios/fc28.xml b/conf/radios/fc28.xml new file mode 100644 index 0000000000..17a80d4d70 --- /dev/null +++ b/conf/radios/fc28.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/radios/mc3030.xml b/conf/radios/mc3030.xml new file mode 100644 index 0000000000..e884040408 --- /dev/null +++ b/conf/radios/mc3030.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/conf/ubx.dtd b/conf/ubx.dtd new file mode 100644 index 0000000000..3585d22e8d --- /dev/null +++ b/conf/ubx.dtd @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + diff --git a/conf/ubx.xml b/conf/ubx.xml new file mode 100644 index 0000000000..1ac484b8bb --- /dev/null +++ b/conf/ubx.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/wavecard.xml b/conf/wavecard.xml new file mode 100644 index 0000000000..431efd5638 --- /dev/null +++ b/conf/wavecard.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + 0x40, "REQ_WRITE_RADIO_PARAM"; + 0x41, "RES_WRITE_RADIO_PARAM"; + 0x50, "REQ_READ_RADIO_PARAM"; + 0x51, "RES_READ_RADIO_PARAM"; + 0x60, "REQ_SELECT_CHANNEL"; + 0x61, "RES_SELECT_CHANNEL"; + 0x62, "REQ_READ_CHANNEL"; + 0x63, "RES_READ_CHANNEL"; + 0x64, "REQ_SELECT_PHYCONFIG"; + 0x65, "RES_SELECT_PHYCONFIG"; + 0x66, "REQ_READ_PHYCONFIG"; + 0x67, "RES_READ_PHYCONFIG"; + 0x68, "REQ_READ_REMOTE_RSSI"; + 0x69, "RES_READ_REMOTE_RSSI"; + 0x6A, "REQ_READ_LOCAL_RSSI"; + 0x6B, "RES_READ_LOCAL_RSSI"; + 0xA0, "REQ_FIRMWARE_VERSION"; + 0xA1, "RES_ FIRMWARE_VERSION"; + + + + + + + + + + + + + + + + + diff --git a/data/maps/muret_UTM.gif b/data/maps/muret_UTM.gif new file mode 100644 index 0000000000..ccec3aa29c Binary files /dev/null and b/data/maps/muret_UTM.gif differ diff --git a/data/maps/muret_UTM.xml b/data/maps/muret_UTM.xml new file mode 100644 index 0000000000..939e195353 --- /dev/null +++ b/data/maps/muret_UTM.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/data/pictures/cockpitMM.gif b/data/pictures/cockpitMM.gif new file mode 100644 index 0000000000..d478f10d33 Binary files /dev/null and b/data/pictures/cockpitMM.gif differ diff --git a/data/pictures/fc28.gif b/data/pictures/fc28.gif new file mode 100644 index 0000000000..0f262d3f7c Binary files /dev/null and b/data/pictures/fc28.gif differ diff --git a/data/pictures/penguin_logo.gif b/data/pictures/penguin_logo.gif new file mode 100644 index 0000000000..2c655c1cb3 Binary files /dev/null and b/data/pictures/penguin_logo.gif differ diff --git a/data/pictures/t7cap.jpg b/data/pictures/t7cap.jpg new file mode 100644 index 0000000000..5dc3f25438 Binary files /dev/null and b/data/pictures/t7cap.jpg differ diff --git a/doc/checklist/checklist.tex b/doc/checklist/checklist.tex new file mode 100644 index 0000000000..4dfb974049 --- /dev/null +++ b/doc/checklist/checklist.tex @@ -0,0 +1,102 @@ +\documentclass{article} + +\usepackage{myfrench} +\usepackage{a4wide} + +\title{Check-List Paparazzi} +\author{} +\date{\today} +\twocolumn + +\begin{document} + +\maketitle + + + +\section{Vecteur} + +\begin{enumerate} + \item Cellule + \item Batteries propulsion, émetteur + \item Verrière + \item Scotch + \item Radio +\end{enumerate} + +\section{Segment sol} + +\begin{enumerate} + \item Portable + \item Batterie + \item Récepteur + \item Moniteur +\end{enumerate} + +\section{Prévol} +\begin{enumerate} + \item Sol +\begin{enumerate} + \item Brancher récepteur + \item Récepteur ON + \item Console: \verb"rm log_test" + \item Console: \verb"rm ./new_display" +\end{enumerate} + + \item Cellule +\begin{enumerate} + \item Brancher émetteur Paparazzi + \item Radio-commande ON + \item Radio-commande, tout OFF, mode AUTO1 + \item Brancher batterie propulsion + \item Radio-commande, mode MANUAL + \item Scotcher Verrière + \item Chronomètre ON +\end{enumerate} + + \item Calibration +\begin{enumerate} + \item Avion sur le nez + \item Radio-commande: commande aileron +\end{enumerate} + + \item Contrôle cellule +\begin{enumerate} + \item Armer variateur + \item Mode MANUEL: gouvernes actives et dans le bon sens + \item Mode AUTO1: gouvernes actives et dans le bon sens +\end{enumerate} +\end{enumerate} + +\section{Décollage} +\begin{enumerate} + \item Console: GPS 4D OK + \item Radio-commande: tout OFF + \item Briefing avant décollage + \item Plein gaz +\end{enumerate} + +\section{Procédure} +\begin{enumerate} + \item Montée en MANUEL + \item Dès attitude de sécurité, AUTO1 + \item LLS ON + \item AUTO2 ON + \item Poser la radio +\end{enumerate} + +\section{Monitoring} +\begin{enumerate} + \item Altitude + \item Vitesse + \item Modes + \item GPS + \item Batterie + \item Temps de vol +\end{enumerate} + +\section{Atterissage} +\begin{enumerate} + \item Radio-commande: tout OFF +\end{enumerate} +\end{document} diff --git a/doc/user_manual/Makefile b/doc/user_manual/Makefile new file mode 100644 index 0000000000..8d7766992f --- /dev/null +++ b/doc/user_manual/Makefile @@ -0,0 +1,47 @@ + +# +# $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. +# + + + + +all: fly_by_wire.png fly_by_wire.eps overall.png overall.eps + latex paparazzi + makeindex paparazzi + latex paparazzi + latex paparazzi + pdflatex paparazzi + + +paparazzi.dvi: paparazzi.tex + latex $< + +paparazzi.pdf: paparazzi.tex + pdflatex $< + +%.png:%.dia + dia -e $@ -t png $< +%.eps:%.dia + dia -e $@ -t png $< + +clean: + rm -rf *~ paparazzi.dvi paparazzi.pdf fly_by_wire.png *.log *.aux *.info *.eps *.idx *.ilg *.ind *.out *.texi *.hind diff --git a/doc/user_manual/fly_by_wire.dia b/doc/user_manual/fly_by_wire.dia new file mode 100644 index 0000000000..e94ff56808 Binary files /dev/null and b/doc/user_manual/fly_by_wire.dia differ diff --git a/doc/user_manual/overall.dia b/doc/user_manual/overall.dia new file mode 100644 index 0000000000..d6b78aba88 Binary files /dev/null and b/doc/user_manual/overall.dia differ diff --git a/doc/user_manual/overall.png b/doc/user_manual/overall.png new file mode 100644 index 0000000000..29852c8bc8 Binary files /dev/null and b/doc/user_manual/overall.png differ diff --git a/doc/user_manual/paparazzi.tex b/doc/user_manual/paparazzi.tex new file mode 100644 index 0000000000..8c85546e94 --- /dev/null +++ b/doc/user_manual/paparazzi.tex @@ -0,0 +1,472 @@ +% +% +% +% $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. +% + +% +% +% This may become the paparazzi user manual. +% +% + + +\documentclass{article} + +\usepackage{a4wide} +\usepackage{graphicx} +\usepackage{makeidx} +\usepackage[pagebackref=true,hyperindex=true]{hyperref} + +\title{Paparazzi User's Manual} +\author{Pascal Brisset and Antoine Drouin} +\date{\today} + +\makeindex + +\begin{document} + +\maketitle + + + +\begin{abstract} +The system described in this document is an autopilot for model aircrafts. +It consist of custom airborne hardware, a laptop as ground station and a +retail radio control transmitter for uplink (manual/assisted control, reconfiguration, etc..). +The sensor used are a GPS receiver and infrared thermopiles (melexys mlx90247) +for horizon sensing. This system is able to fly autonomously a small electro +powered aircraft. It transmits live video and telemetry data. +The ground station permits decoding, logging, replay and analysis of these data. +It also permits airborne code configuration, generation, simulation and flashing +on target MCU. + +\end{abstract} + +\section{Description} + +\subsection{Architecture : two 8 bits MCUs} + + goals: + - maximum availability (modes degrades, manual control) + - ease of development + + logical tasks of increasing conplexity and decreasing importance in separate devices. + + \subsubsection{fly by wire} + (avr mega8) responsible for radio control decoding, mixings, servos (c.f. figure ref{fbw}). + + short code, well tested, features similar to a programmable radio control transmitter + + allows manual control and programmed failsafe (radio link loss). + + + health monitoring (battery voltage, drawn current etc...) + +\ + \subsubsection{autopilot} + (avr mega128) measures + control loops + telemetry + navigation + + The model can be safely flown in manual mode with only the fly by wire MCU. + + We want to keep and improve this simple system for cheap/small aircrafts. + + We also want to expend it with a third 32 bit processor (like arm or xscale) + to get network communications and processing power (FMS like). + +\subsection{Technology : cheap and widely available parts} + 4800bps FSK Telemetry signal. Can be fed in most transmitter. + We use the audio channel of a 50 mW 2.4GHz video transmitter. + The video Channel is used for real time video. We reach 600m in line of sight. + The rx antenna is a small patch mounted on top of a helm (cheap self pointing antenna). + FIXME: ADD PIC + + Modified retail RC receiver : solder a wire after the HF section. + + controller board, sensor, GPS : custom PCBs, all(most) parts smd. + home pcbs and home soldering for protos + + Free software for tool (GNU/Linux, gcc, gtk, ocaml....) + No Windows port known but should be faisible. + +\section{Ground station} + \subsection{hardware} + Gnu/linux laptop + + 2.4GHz video receiver + CMX469 modem board or rtty + ==FIXME== add pic + camcorder + + \subsection{obtaining and installing} + + The source code is available from the Project page ( http://savannah.nongnu.org/projects/paparazzi/ ) + + Use the anonymous CVS server to get the up to date source code and documentation: + +{\em export CVS\_RSH="ssh"} +{\em cvs -z3 -d:ext:anoncvs@savannah.nongnu.org:/cvsroot/paparazzi co paparazzi2} + + You can also download a tarball from this website ( http://www.recherche.enac.fr/paparazzi/paparazzi.tar.gz ). + Debian sarge users can get the required extra packages from there ( http://www.recherche.enac.fr/paparazzi ). + + *********************************************************************** + Set the PAPARAZZI\_HOME environment variable to the top directory + of the distribution (this variable is used by some of the components). + *********************************************************************** + {\em export PAPARAZZI\_HOME=/some/dir/paparazzi2 } +Default configurations files (in conf/ directory) should allow to +compile both embedded and ground software: +\begin{itemize} + \item 1) HAVE A LOOK at conf/Makefile.local + + \item 2) Create conf/conf.xml and the related files for your convenience +("make configure" runs a graphics interface which may help; however +this gui is in a very early alpha stage). Some examples are provided. + + \item 3) "make" in top directory should compile everything + +\end{itemize} + +\subsection{The ivy software bus} + + + +\subsection{configuration interface} + This windowed program allows to graphicaly edit the configuration of a Paparazzi. + It also can also be used to program your controller board with various test and calibration + programs. + type {\em{make configure}} in the source top directory. + + \subsection{telemetry interface : recording and display} + \index{telemetry} + receive : retrieve telemetry data, store them on disk and broadcast them over a network + gui : display telemetry data. Can be feed live by the receive programm or by the replay programm + + map calibration : + uses 3 points. trivial projection but sufficient for short range. + \subsection{replay interface} + + \subsection{hitl simulator} + Airborn programs runs on their target MCUs. + Their inputs and outputs (GPS, infrared and servos) are bypassed to the laptop. + A dumb flight model allows debugging and non regression testing. Also useful for + tunnig navigation + +\section{Airborne software} + \subsection{Fly by wire} + + + + only supports PPM - subject to jamming - filtering - would be better with a PCM encoding + +\begin{figure} +\includegraphics[width=15cm]{fly_by_wire} +\caption{\label{fbw}Fly by wire data processing} +\end{figure} + + + \subsection{Autopilot} + + \subsubsection{Low level control loop} + 20Hz . P controller for pitch and roll + PI for throttle + + mostly unfiltered attitude data from infrared sensor + GPS climb rate for throttle + + + \subsubsection{Navigtion loop} + 1Hz P controllers on heading to waypoint and altitude + GPS data + + + \subsubsection{Infrared calibration} + contrast + LLS + + + \subsubsection{FMS} + modes (auto1 auto2) + waypoints circling + waypoint crossing + mode home + automatic take off + +\section{Assembling boards} +PCBS : homebuild eurocircuits + +see part list + +solder one(a group of) component at a time. test with voltmeter or scope +use provided programms. + +mcu fuses - used to define type of clock - factory supplied with 1MHz internal oscillator - +must switch to {\em{ceramic resonator}} for ``fly by wire''. He will be generating clock for autopilot. +``autopilot'' will have to be programmed to ``external clok''. If you mess up, you can make a zombie +out of your MCU unless you can provide the awaited signal or crystal. You can read current fuses configuration by typing {\em{make read\_fuses}} in an avr source directory. The correct values are contained +in the Makefile and can be programmed by typing {\em{make wr\_fuses}}. +The graphical configurator also allows these operations. + + +mcu flashing - The method (and corresponding wiring) we use for flashing and fuses programming is +called serial (SPI) programming. It is possible to programm a resident bootloader who will take care +of following programmations using serial RS232 + +\subsection{power supply} + +\subsection{pc link} + This board is a level converter. It converts between the TTL 5V of the controller board and respectively, the + 3.3V of the parallel port and the 10V of the rs232 port. + The parallel port is used for SPI programming of MCUs. The rs232 port are used fo serial + communications with MCUs, for example during simulations. + This board is meant to stay on ground. + + +\subsection{controller board} + solder the fly by wire MCU (mega8), its crystal and the programmation socket + Connect to a current limited power supply and check current. + + build a wire harness to the pc link board + plug the pc link in your parallel port + try connecting to MCU in serial programming mode (SPI) (button on gui) + FIXME: if it fails + programm the fuses of the MCU (describe the crystal connected to the mcu) (button on gui) + check crystal oscillating with scope if available. + + try programming the uart test + plug a straight serial cable in the serial1 connector of the pc link and the other end in one of your computer rs232 port. If your computer doesn't have any, use a usb to rsr232 converter. + you should see a message comming from your board telling you the link is ok. Check the other direction, writing to the board. + + solder servo driver and connector (maybe later...) + run the test programm (servo calibration) + + find ppm signal and supply in receiver. solder wire (computer cdrom wire) + solder the other end to the controller board. + run the test programm (radio calibration) + + + solder the autopilot MCU + try uart 1 + + try spi (write a test with SPI and UART) + + solder the modem + try modem (in line input - with rtty ?? ) + + + +\subsection{ground modem} + same story with mega8 and crystal + check connection, write fuse, program serial test + solder modem + connect to airborne modem + watch telemetry + +\subsection{infrared sensor} + solder amp, resistor and capa and thermopiles. + connect to autopilot MCU ADCs + watch telemetrie values; + +\section{Fitting system in the airframe} + +All the processing available on programmable radio transmitters (travel adj, mixing etc..) are here +done by the fly by wire MCU. This is cool because you don't need to change your transmitter programm +when you change aircraft, but it also enables the autopilot to use these features. + +\subsection{radio control transmitter calibration} +tab in gui. +use the default programm of your rc transmitter with travels set to 100\% and trim centered. +programm the controller board (actually fly by wire MCU) with the test programm (button on gui). +if everything goes well you will see values of the channels in the signal send by your transmitter. +record min max neutral for each channel and setup control +give it a name +generate a configuration file. + + +\subsection{servos travel and mixer setup} + +Mount the board in the airframe and connect servos. programm the board with the servo setting programm (tab in gui) +for each servo, define name of the control, travel neutral and direction. +Try to use maximal travel and long control arms. + +\subsection{infrared sensor} +describe the way to mout it on the airframe and the needed configuration. + + +\section{Simulation} +Don't attempt to fly your aircraft until you've succesfully simulated with your configuration and learned how the system works. There are two type of simulation : + +\subsection{``Software in loop'' simulation} +needs no hardware but the laptop. +It is great to learn while you are building the hardware. +Thanx to our magnificient C compiler, we are able to compile the same code for the AVR mcu and for the i386 laptop. + +\subsection{``Hardware in the loop'' simulation} + + + +\section{Test Flight} + +\subsection{checklist} + +This checklist applies to our twinstar. + +Switch ground station on - connect modem - launch receive and gui - check modem messages + check ground batterie + +Switch rc transmitter on - check programm - all switches pushed - mode auto1 - throttle low + +Switch airplane on - check model name and rc transmitter name + +check "waiting calibration" on ground station - switch to mode manual + +Put airplane on nose - push roll stick - check contrast on ground station + +switch briefly to full throttle to trigger speed controllers + +Check command direction and travel + +Switch to auto1. Check corrections direction (if you put the plane nose down, the elevator should raise, if you bank to the right, the left aileron should raise). + +Check GPS status on ground station. + +Flight briefing - check mission on map + +check autopilot mode + +take off. For automatic take off (auto 2), full throttle will signal take off and trigger full throttle. + + +\subsection{adjusting trim} + + +This first flight is flown in manual mode. It is used to trim the airframe and get an estimation of infrared neutrals. +It is very important that you trim your model perfectly. Choose a day without wind or turbulence. Fly long +straight lines trying not to touch your sticks. + +Watch your batterie voltage on the laptop (ask someone or use a vocal synthetiser). + +After the flight + offset servos to recenter your rc transmitter trims. + get an estimation of contrast\_gain (ir\_gain = contrast\_gain/contrast) comparing your contrast measure and lls + get an estimation of infrared neutrals (pitch and roll) (play the telemetry record during straight lines or maybe write a tool). + get an estimation of throttle for level flight + watch the record for anomalies (describe) + plot parameters (like airspeed, climb rate, current consumption...). This is a great tool for tunning an airframe. + + + +\subsection{adjusting low level loop (attitude loop)} +In this flight you will adjust the infrared neutrals and low level loop gains. +The number of parameter that you are able to tune in a single flight depends on how many switches and sliders are available on your rc transmitter. With the Multiplex MC3030 (9 channels) we have two spare sliders and one three positions +switch. It allows us to tune 4 parameters at a time. + +Update you airframe description. Use the values from the previous flight for infrared neutral and contrast\_gain. Use low values for the low level loop P gains. Reflash your airplane. + +For this flight we will programm an autopilot mode in auto1 which will hold the plane in an attitude described by the +roll and pitch sticks. If you leave your sticks centered, the plane will fly level. If you push your roll stick, the plane will bank to a given value (full travel -> 30°) and stay in this attitude. + + +Take off in manual mode. Gain altitude . +Check that the plane is flying level and that your tranmistter trims are centered. +If this is not the case, redo the programm of the previous flight. + +Engage auto1 mode. Be ready to switch back to manual if the plane doesn't react like you expect. +Switch to neutral calibration and fine tune values so that the plane flies level. +This stage is very important for the navigation to work. +Raise the values of the low level loop P gains until the plane reacts quickly to an attitude change but +without oscillating. + +Now your plane should be capable of holding attitude. This is a very strange feeling for the pilote. + + +After the flight: + plot params + fine tune your contrast gain with LLS measure + update low level P gains and infrared neutrals in model description. + + +\subsection{adjusting autopilot gains} + +In this flight, we will programm a navigation mode in the auto2 bank which will navigate the airplane around waypoints. + +describe a mission: home and waypoints + You can click on a map (needs calibration) or walk with the plane GPS to find the position of your points. + + For our twinstar, the typical missions are two waypoints distant from 300m and at 80m above ground level. + + Flash your airplane. If you have a map, check that the mission that the airplane transmits on boot shows up on the map at the right place. + + After the checklist, check that the position transmitted is near your home (relative position - NAV message) and that the coordinates go in the right direction (X->NORTH Y->EAST FIXME:check!!) + +Take of in manual mode - check plane level and rc trim centered +Switch to auto1 - check plane level +Switch to auto2 - adjust nav P gain and max bank angle for smooth nav + + + +Make more flights taking off in manual - +When your are confident you can take off in auto1 or auto2. Specify desired climb rate and security altitude + + + +\section{Part list and supplyers} + +\subsection{power supply} + +\subsection{controller board} + + +\subsection{infrared sensor} +\begin{tabular}{| l | l | l | l |} +\hline +4 & thermopiles MLX90247ESF-B & www.digikey.com (PN: MLX90247ESF-B-ND) & 16,5 euros each, 12.5 euros >= 10 \\ +\hline +1 & op amp AD8552 TSSOP case & le fabriquant envoie des échantillons - formulaire sur leur site.& \\ +\hline +1 & MOLEX 6 contacts connector 1.25mm pitch & radiospares (PN: 53047-0610) & by 10 - 2,34 euros \\ +\hline +\end{tabular} + + +résistances et condensateurs CMS + + + + + +\section{glossaire} +\begin{description} + \item[ADC] Analog to Digital Converter: A chip or MCU peripheral that converts an analog voltage to its binary representation. + + \item[CPU] Control Processor Unit: + + + \item[MCU] Micro Controller Unit: A chip containing a CPU and varous peripherals like memory, io ports, timer, ADCs etc.. + The paparazzi controller board uses two of these chip. + + +\item[LLS] Linear Least Square: + +\end{description} + +\printindex + + +\end{document} diff --git a/sw/README b/sw/README new file mode 100644 index 0000000000..850deeecdb --- /dev/null +++ b/sw/README @@ -0,0 +1,25 @@ +# +# $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. +# + + + + diff --git a/sw/airborne/autopilot/Makefile b/sw/airborne/autopilot/Makefile new file mode 100644 index 0000000000..51a5943602 --- /dev/null +++ b/sw/airborne/autopilot/Makefile @@ -0,0 +1,92 @@ +# +# $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. +# + + +FBW=../fly_by_wire + + +LOCAL_CFLAGS= $(CTL_BRD_FLAGS) $(GPS_FLAGS) $(SIMUL_FLAGS) + +VARINCLUDE=$(PAPARAZZI_HOME)/var/include +ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) + +ARCH = atmega128 +TARGET = autopilot + +LOW_FUSE = e0 +HIGH_FUSE = 99 + +ifeq ($(CTL_BRD_VERSION),V1_1) +LOW_FUSE = ff +HIGH_FUSE = 89 +CTL_BRD_FLAGS=-DCTL_BRD_V1_1 +endif + +ifeq ($(SIMUL),1) +SIMUL_FLAGS= -DSIMUL +endif + +EXT_FUSE = ff +LOCK_FUSE = ff +INCLUDES = -I $(FBW) -I ../../include -I $(VARINCLUDE) -I $(ACINCLUDE) + +GPS = gps_ubx.c +GPS_FLAGS=-DUBX + +$(TARGET).srcs = \ + main.c \ + modem.c \ + link_fbw.c \ + spi.c \ + adc.c \ + $(GPS) \ + infrared.c \ + pid.c \ + nav.c \ + uart.c \ + estimator.c \ + if_calib.c \ + mainloop.c + +include ../../../conf/Makefile.local +include ../../../conf/Makefile.avr + +autopilot.install : warn_conf + +warn_conf : + @echo + @echo '###########################################################' + @grep AIRFRAME_NAME $(ACINCLUDE)/airframe.h + @grep RADIO_NAME $(ACINCLUDE)/radio.h + @grep FLIGHT_PLAN_NAME $(ACINCLUDE)/flight_plan.h + @echo '###########################################################' + @echo + + +.depend : $(VARINCLUDE)/messages.h $(ACINCLUDE)/flight_plan.h $(VARINCLUDE)/ubx_protocol.h $(ACINCLUDE)/inflight_calib.h $(ACINCLUDE)/airframe.h $(ACINCLUDE)/radio.h +main.o : $(VARINCLUDE)/messages.h +nav.o : $(ACINCLUDE)/flight_plan.h +gps_ubx.o : $(VARINCLUDE)/ubx_protocol.h +if_calib.o : $(ACINCLUDE)/inflight_calib.h + +clean : avr_clean + rm -f *.out *.cm* messages.h flight_plan.h ubx_protocol.h inflight_calib.h diff --git a/sw/airborne/autopilot/README b/sw/airborne/autopilot/README new file mode 100644 index 0000000000..d56db63899 --- /dev/null +++ b/sw/airborne/autopilot/README @@ -0,0 +1,24 @@ +# $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. +# + + + + diff --git a/sw/airborne/autopilot/adc.c b/sw/airborne/autopilot/adc.c new file mode 100644 index 0000000000..a15592f15e --- /dev/null +++ b/sw/airborne/autopilot/adc.c @@ -0,0 +1,126 @@ +/* + * Paparazzi mcu0 adc functions + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * Copyright (C) 2002 Trammell Hudson + * 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. + * + */ + + +#include +#include +#include +#include "airframe.h" +#include "adc.h" + + +/************************************************************************* + * + * Analog to digital conversion code. + * + * We allow interrupts during the 2048 usec windows. If we run the + * ADC clock faster than Clk/64 we have too much overhead servicing + * the interrupts from it and end up with servo jitter. + * + * For now we've slowed the clock to Clk/128 because it lets us + * be lazy in the interrupt routine. + */ +#define VOLTAGE_TIME 0x07 +#define ANALOG_PORT PORTF +#define ANALOG_PORT_DIR DDRF + + +#ifdef CTL_BRD_V1_1 +#define ANALOG_VREF 0 +#endif + +#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1 +#define ANALOG_VREF _BV(REFS0) +#endif + +uint16_t adc_samples[ NB_ADC ]; + +static struct adc_buf* buffers[NB_ADC]; + +void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s) { + buffers[adc_channel] = s; +} + +void +adc_init( void ) +{ + uint8_t i; + /* Ensure that our port is for input with no pull-ups */ + ANALOG_PORT = 0x00; + ANALOG_PORT_DIR = 0x00; + + /* Select our external voltage ref, which is tied to Vcc */ + ADMUX = ANALOG_VREF; + + /* Turn off the analog comparator */ + sbi( ACSR, ACD ); + + /* Select out clock, turn on the ADC interrupt and start conversion */ + ADCSR = 0 + | VOLTAGE_TIME + | ( 1 << ADEN ) + | ( 1 << ADIE ) + | ( 1 << ADSC ); + + /* Init to 0 (usefull ?) */ + for(i = 0; i < NB_ADC; i++) + buffers[i] = (struct adc_buf*)0; +} + +/** + * Called when the voltage conversion is finished + * + * 8.913kHz on mega128@16MHz 1kHz/channel ?? +*/ + + +SIGNAL( SIG_ADC ) +{ + uint8_t adc_input = ADMUX & 0x7; + struct adc_buf* buf = buffers[adc_input]; + uint16_t adc_value = ADCW; + /* Store result */ + adc_samples[ adc_input ] = adc_value; + + if (buf) { + uint8_t new_head = buf->head + 1; + if (new_head >= AV_NB_SAMPLE) new_head = 0; + buf->sum -= buf->values[new_head]; + buf->values[new_head] = adc_value; + buf->sum += adc_value; + buf->head = new_head; + } + + /* Find the next input */ + adc_input++; + if( adc_input >= 8 ) + adc_input = 0; + /* Select it */ + ADMUX = adc_input | ANALOG_VREF; + /* Restart the conversion */ + sbi( ADCSR, ADSC ); +} diff --git a/sw/airborne/autopilot/adc.h b/sw/airborne/autopilot/adc.h new file mode 100644 index 0000000000..c64a7719f8 --- /dev/null +++ b/sw/airborne/autopilot/adc.h @@ -0,0 +1,53 @@ +/* + * Paparazzi mcu0 adc functions + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * Copyright (C) 2002 Trammell Hudson + * 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. + * + */ + +#ifndef _ADC_H_ +#define _ADC_H_ + +#include + + +#define NB_ADC 8 + +/* Array containing the last measured value */ +extern uint16_t adc_samples[ NB_ADC ]; + +void adc_init( void ); + +#define AV_NB_SAMPLE 0x20 + +struct adc_buf { + uint16_t sum; + uint16_t values[AV_NB_SAMPLE]; + uint8_t head; +}; + +/* Facility to store last values in a circular buffer for a specific + channel: allocate a (struct adc_buf) and register it with the following + function */ +void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s); +#endif diff --git a/sw/airborne/autopilot/autopilot.h b/sw/airborne/autopilot/autopilot.h new file mode 100644 index 0000000000..722a2694b4 --- /dev/null +++ b/sw/airborne/autopilot/autopilot.h @@ -0,0 +1,110 @@ +/* + * $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. + * + */ + +#ifndef AUTOPILOT_H +#define AUTOPILOT_H + +#include "link_autopilot.h" + +#define TRESHOLD1 TRESHOLD_MANUAL_PPRZ +#define TRESHOLD2 200 * CLOCK + + +#define PPRZ_MODE_MANUAL 0 +#define PPRZ_MODE_AUTO1 1 +#define PPRZ_MODE_AUTO2 2 +#define PPRZ_MODE_HOME 3 +#define PPRZ_MODE_NB 4 + +#define PPRZ_MODE_OF_PULSE(pprz, mega8_status) \ + (pprz > TRESHOLD2 ? PPRZ_MODE_AUTO2 : \ + (pprz > TRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL)) + +extern uint8_t pprz_mode; + + +#define VERTICAL_MODE_MANUAL 0 +#define VERTICAL_MODE_AUTO_GAZ 1 +#define VERTICAL_MODE_AUTO_CLIMB 2 +#define VERTICAL_MODE_AUTO_ALT 3 +#define VERTICAL_MODE_NB 4 + +#define VERTICAL_MODE_OF_PULSE(pprz) (pprz < TRESHOLD2 ? VERTICAL_MODE_MANUAL: \ + VERTICAL_MODE_AUTO_ALT) + +#define IR_ESTIM_MODE_OFF 0 +#define IR_ESTIM_MODE_ON 1 + +#define IR_ESTIM_MODE_OF_PULSE(pprz) (pprz < TRESHOLD2 ? IR_ESTIM_MODE_OFF: \ + IR_ESTIM_MODE_ON) + +extern uint8_t ir_estim_mode; + +#define STICK_PUSHED(pprz) (pprz < TRESHOLD1 || pprz > TRESHOLD2) + + +#define TRIM_PPRZ(pprz) (pprz < MIN_PPRZ ? MIN_PPRZ : \ + (pprz > MAX_PPRZ ? MAX_PPRZ : \ + pprz)) + +#define TRIM_UPPRZ(pprz) (pprz < 0 ? 0 : \ + (pprz > MAX_PPRZ ? MAX_PPRZ : \ + pprz)) + + +#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ * travel + center) + +extern uint8_t fatal_error_nb; + +#define GAZ_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9) + +extern uint8_t inflight_calib_mode; +//extern uint16_t flight_time; +extern uint8_t vertical_mode; +extern uint8_t vsupply; + +extern bool_t rc_event_1, rc_event_2; + +extern float slider_1_val, slider_2_val; + +extern bool_t launch; + + +#define ModeUpdate(_mode, _value) { \ + uint8_t new_mode = _value; \ + if (_mode != new_mode) { _mode = new_mode; return TRUE; } \ + return FALSE; \ +} + +#define CheckEvent(_event) (_event ? _event = FALSE, TRUE : FALSE) + +#ifdef CTL_BRD_V1_1 +extern struct adc_buf buf_bat; +#endif + +void periodic_task( void ); +void use_gps_pos(void); +void radio_control_task(void); + +#endif /* AUTOPILOT_H */ diff --git a/sw/airborne/autopilot/downlink.h b/sw/airborne/autopilot/downlink.h new file mode 100644 index 0000000000..eee7acd783 --- /dev/null +++ b/sw/airborne/autopilot/downlink.h @@ -0,0 +1,35 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +#ifndef DOWNLINK_H +#define DOWNLINK_H + +#include "modem.h" + +#define STX 0x05 +#define ETX 0x06 + +#include "messages.h" + +#endif /* DOWNLINK_H */ diff --git a/sw/airborne/autopilot/estimator.c b/sw/airborne/autopilot/estimator.c new file mode 100644 index 0000000000..370996ce43 --- /dev/null +++ b/sw/airborne/autopilot/estimator.c @@ -0,0 +1,170 @@ +/* + * Paparazzi autopilot $Id$ + * + * Copyright (C) 2004 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. + * + */ + +#include +#include + +#include "estimator.h" +#include "gps.h" +#include "pid.h" +#include "infrared.h" +#include "autopilot.h" + + +/* position in meters */ +float estimator_x; +float estimator_y; +float estimator_z; + +/* attitude in radian */ +float estimator_phi; +float estimator_psi; +float estimator_theta; + +/* speed in meters per second */ +float estimator_x_dot; +float estimator_y_dot; +float estimator_z_dot; + +/* rotational speed in radians per second */ +float estimator_phi_dot; +float estimator_psi_dot; +float estimator_theta_dot; + +/* flight time in seconds */ +uint16_t estimator_flight_time; +/* flight time in seconds */ +float estimator_t; + +/* horizontal speed in module and dir */ +float estimator_hspeed_mod; +float estimator_hspeed_dir; + +float estimator_rad_of_ir, estimator_ir, estimator_rad; + +#define EstimatorSetPos(x, y, z) { estimator_x = x; estimator_y = y; estimator_z = z; } +#define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; } + + +// FIXME maybe vz = -climb for NED?? +#define EstimatorSetSpeedCart(vx, vy, vz) { \ + estimator_vx = vx; \ + estimator_vy = vy; \ + estimator_vz = vz; \ +} +// estimator_hspeed_mod = sqrt( estimator_vx * estimator_vx + estimator_vy * estimator_vy); +// estimator_hspeed_dir = atan2(estimator_vy, estimator_vx); + + +#define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \ + estimator_hspeed_mod = vhmod; \ + estimator_hspeed_dir = vhdir; \ + estimator_z_dot = vz; \ +} +//FIXME is this true ?? estimator_vx = estimator_hspeed_mod * cos(estimator_hspeed_dir); +//FIXME is this true ?? estimator_vy = estimator_hspeed_mod * sin(estimator_hspeed_dir); + +#define EstimatorSetRotSpeed(phi_dot, psi_dot, theta_dot) { \ + estimator_phi_dot = phi_dot; \ + estimator_psi_dot = psi_dot; \ + estimator_theta_dot = theta_dot; \ +} + +inline void estimator_update_lls( void ); + +void estimator_init( void ) { + + EstimatorSetPos (0., 0., 0.); + + EstimatorSetAtt (0., 0., 0); + + EstimatorSetSpeedPol ( 0., 0., 0.); + + EstimatorSetRotSpeed (0., 0., 0.); + + estimator_flight_time = 0; + + estimator_rad_of_ir = ir_rad_of_ir; +} + +#define EstimatorIrGainIsCorrect() (TRUE) + +void estimator_update_state_infrared( void ) { + float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON && EstimatorIrGainIsCorrect()) ? + estimator_rad_of_ir : ir_rad_of_ir; + estimator_phi = rad_of_ir * ir_roll; + + estimator_theta = rad_of_ir * ir_pitch; +} + +#define INIT_WEIGHT 100. /* The number of times the initial value has to be taken */ +#define INIT_IR2 (50.*50.)/* Ir value used for initialization */ +#define RHO 0.999 /* The higher, the slower the estimation is changing */ + +#define g 9.81 + +void estimator_update_ir_estim( void ) { + static float last_hspeed_dir; + static float last_t; + static bool_t initialized = FALSE; + static float sum_xy, sum_xx; + + if (initialized) { + float dt = gps_ftow - last_t; + if (dt > 0.1) { // Against division by zero + float phi = (estimator_hspeed_dir - last_hspeed_dir)/dt*NOMINAL_AIRSPEED/g; /* tan linearized */ + NORM_RAD_ANGLE(phi); + estimator_ir = (float)ir_roll; + estimator_rad = phi; + float absphi = fabs(phi); + if (absphi < 1.0 && absphi > 0.05 && (- ir_contrast/2 < ir_roll && ir_roll < ir_contrast/2)) { + sum_xy = estimator_rad * estimator_ir + RHO * sum_xy; + sum_xx = estimator_ir * estimator_ir + RHO * sum_xx; + estimator_rad_of_ir = sum_xy / sum_xx; + } + } + } else { + initialized = TRUE; + sum_xy = INIT_WEIGHT * estimator_rad_of_ir * INIT_IR2; + sum_xx = INIT_WEIGHT * INIT_IR2; + } + + last_hspeed_dir = estimator_hspeed_dir; + last_t = gps_ftow; +} + + +void estimator_update_state_gps( void ) { + if (GPS_FIX_VALID(gps_mode)) { + EstimatorSetPos(gps_east, gps_north, gps_falt); + EstimatorSetSpeedPol(gps_fspeed, gps_fcourse, gps_fclimb); + + if (estimator_flight_time) + estimator_update_ir_estim(); + } +} + +void estimator_propagate_state( void ) { + +} diff --git a/sw/airborne/autopilot/estimator.h b/sw/airborne/autopilot/estimator.h new file mode 100644 index 0000000000..4aa40438ba --- /dev/null +++ b/sw/airborne/autopilot/estimator.h @@ -0,0 +1,67 @@ +/* + * $Id$ + * + * Copyright (C) 2004 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. + * + */ + +#ifndef ESTIMATOR_H +#define ESTIMATOR_H + +#include + +/* position in meters */ +extern float estimator_x; +extern float estimator_y; +extern float estimator_z; + +/* attitude in radians */ +extern float estimator_phi; +extern float estimator_psi; +extern float estimator_theta; + +/* speed in meters per second */ +extern float estimator_x_dot; +extern float estimator_y_dot; +extern float estimator_z_dot; + +/* rotational speed in radians per second */ +extern float estimator_phi_dot; +extern float estimator_psi_dot; +extern float estimator_teta_dot; + +/* flight time in seconds */ +extern uint16_t estimator_flight_time; +extern float estimator_t; + +/* horizontal speed in module and dir (m/s, rad) */ +extern float estimator_hspeed_mod; +extern float estimator_hspeed_dir; + +void estimator_init( void ); +void estimator_update_state_infrared( void ); +void estimator_update_state_gps( void ); +void estimator_propagate_state( void ); + +extern float estimator_rad_of_ir, estimator_ir, estimator_rad; + + + +#endif /* ESTIMATOR_H */ diff --git a/sw/airborne/autopilot/gps.h b/sw/airborne/autopilot/gps.h new file mode 100644 index 0000000000..ad873dfefa --- /dev/null +++ b/sw/airborne/autopilot/gps.h @@ -0,0 +1,58 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +/* + * Parse SIRF protocol from ublox SAM module + * +*/ + + +#ifndef GPS_H +#define GPS_H + +#include "std.h" + +extern uint8_t gps_mode; +extern float gps_ftow; /* ms */ +extern float gps_falt; /* m */ +extern float gps_fspeed; /* m/s */ +extern float gps_fclimb; /* m/s */ +extern float gps_fcourse; /* rad */ +extern int32_t gps_utm_east, gps_utm_north; +extern float gps_east, gps_north; /* m */ + +void gps_init( void ); +void parse_gps_msg( void ); +extern volatile uint8_t gps_msg_received; +extern bool_t gps_pos_available; +extern uint8_t gps_nb_ovrn; + +#ifdef UBX +#include "ubx.h" +#else +#include "sirf.h" +#endif + + +#endif /* GPS_H */ diff --git a/sw/airborne/autopilot/gps_sirf.c b/sw/airborne/autopilot/gps_sirf.c new file mode 100644 index 0000000000..50ba3ca99e --- /dev/null +++ b/sw/airborne/autopilot/gps_sirf.c @@ -0,0 +1,235 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +#include +#include +#include +#include +#include + + +#include "uart.h" +#include "gps.h" + +float gps_falt; +float gps_fspeed; +float gps_fclimb; +float gps_fcourse; +uint8_t gps_mode; +volatile bool_t gps_msg_received; +bool_t gps_pos_available; + + +#define SIRF_MAX_PAYLOAD 255 +uint8_t sirf_msg_buf[SIRF_MAX_PAYLOAD]; + +#define READ_INT32_AT_OFFSET(offset, dest) \ +{ \ + dest[0] = sirf_msg_buf[offset+3]; \ + dest[1] = sirf_msg_buf[offset+2]; \ + dest[2] = sirf_msg_buf[offset+1]; \ + dest[3] = sirf_msg_buf[offset]; \ +} \ +/* ext nav type = 0x62 + offset len + type 0 1 + lat 1 4 + lon 5 4 + alt 9 4 + speed 13 4 + climb 17 4 + course 21 4 + mode 25 1 +*/ +void parse_gps_msg( void ) { + static int32_t tmp_int32; + uint8_t *tmp = (uint8_t*)&tmp_int32; + + READ_INT32_AT_OFFSET(1, tmp); + gps_lat = tmp_int32; + + READ_INT32_AT_OFFSET(5, tmp); + gps_lon = tmp_int32; + + READ_INT32_AT_OFFSET(9, tmp); + gps_falt = (float)tmp_int32 / 1e3; + + READ_INT32_AT_OFFSET(13, tmp); + gps_fspeed = (float)tmp_int32 / 1e3; + + READ_INT32_AT_OFFSET(17, tmp); + gps_fclimb = (float)tmp_int32 / 1e3; + + READ_INT32_AT_OFFSET(21, tmp); + gps_fcourse = (float)tmp_int32 / 1e8; + + gps_mode = sirf_msg_buf[25]; + + gps_pos_available = TRUE; +} + + + + + + +void gps_init( void ) { + /* Enable uart */ +#ifdef SIMUL + uart0_init(); +#else + uart1_init(); +#endif +} + +#define SIRF_START1 0xA0 +#define SIRF_START2 0xA2 +#define SIRF_END1 0xB0 +#define SIRF_END2 0xB3 + +#ifdef SIMUL +#define IR_START 0xA1 /* simulator/mc.ml */ +volatile int16_t simul_ir_roll; +volatile int16_t simul_ir_pitch; +#endif + +#define SIRF_TYP_NAV 0x02 +#define SIRF_TYP_EXT_NAV 0x62 + +#define UNINIT 0 +#define GOT_START1 1 +#define GOT_START2 2 +#define GOT_LEN1 3 +#define GOT_LEN2 4 +#define GOT_PAYLOAD 5 +#define GOT_CHECKSUM1 6 +#define GOT_CHECKSUM2 7 +#define GOT_END1 8 +#ifdef SIMUL +#define GOT_IR_START 9 +#define GOT_IR1 10 +#define GOT_IR2 11 +#define GOT_IR3 12 +#endif + +static uint8_t sirf_status; +static uint16_t sirf_len; +static uint16_t sirf_checksum; +static uint8_t sirf_type; +static uint8_t sirf_msg_idx; + + +static inline void parse_sirf( uint8_t c ) { + switch (sirf_status) { + case UNINIT: + if (c == SIRF_START1) + sirf_status++; +#ifdef SIMUL + if (c == IR_START) + sirf_status = GOT_IR_START; +#endif + break; + case GOT_START1: + if (c != SIRF_START2) + goto error; + sirf_status++; + break; + case GOT_START2: + sirf_len = (c<<8) & 0xFF00; + sirf_status++; + break; + case GOT_LEN1: + sirf_len += (c & 0x00FF); + if (sirf_len > SIRF_MAX_PAYLOAD) + goto error; + sirf_msg_idx = 0; + sirf_status++; + break; + case GOT_LEN2: + if (sirf_msg_idx==0) { + sirf_type = c; + } + if (sirf_type == SIRF_TYP_EXT_NAV) + sirf_msg_buf[sirf_msg_idx] = c; + sirf_msg_idx++; + if (sirf_msg_idx >= sirf_len) { + sirf_status++; + } + break; + case GOT_PAYLOAD: + sirf_checksum = (c<<8) & 0xFF00; + sirf_status++; + break; + case GOT_CHECKSUM1: + sirf_checksum += (c & 0x00FF); + /* fixme: check correct */ + sirf_status++; + break; + case GOT_CHECKSUM2: + if (c != SIRF_END1) + goto error; + sirf_status++; + break; + case GOT_END1: + if (c != SIRF_END2) + goto error; + + if (sirf_type == SIRF_TYP_EXT_NAV) + gps_msg_received = TRUE; + goto restart; + break; +#ifdef SIMUL + case GOT_IR_START: + simul_ir_roll = c << 8; + sirf_status++; + break; + case GOT_IR1: + simul_ir_roll |= c; + sirf_status++; + break; + case GOT_IR2: + simul_ir_pitch = c << 8; + sirf_status++; + break; + case GOT_IR3: + simul_ir_pitch |= c; + goto restart; + break; +#endif + } + return; + error: + // modem_putc('r'); + restart: + // modem_putc('\n'); + sirf_status = UNINIT; + sirf_checksum = 0; + return; +} + +#ifdef SIMUL +ReceiveUart0(parse_sirf); +#else +ReceiveUart1(parse_sirf); +#endif diff --git a/sw/airborne/autopilot/gps_ubx.c b/sw/airborne/autopilot/gps_ubx.c new file mode 100644 index 0000000000..3d952cd53e --- /dev/null +++ b/sw/airborne/autopilot/gps_ubx.c @@ -0,0 +1,207 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +#include +#include +#include +#include +#include +#include + + +#include "flight_plan.h" +#include "uart.h" +#include "gps.h" +#include "ubx_protocol.h" +#include "flight_plan.h" + +float gps_ftow; +float gps_falt; +float gps_fspeed; +float gps_fclimb; +float gps_fcourse; +int32_t gps_utm_east, gps_utm_north; +float gps_east, gps_north; +uint8_t gps_mode; +volatile bool_t gps_msg_received; +bool_t gps_pos_available; +const int32_t utm_east0 = NAV_UTM_EAST0; +const int32_t utm_north0 = NAV_UTM_NORTH0; + +#define UBX_MAX_PAYLOAD 255 +static uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD]; + +#define RadianOfDeg(d) ((d)/180.*3.1415927) + +#ifdef SIMUL +#include "infrared.h" +#define IR_START 0xA1 /* simulator/mc.ml */ +volatile int16_t simul_ir_roll; +volatile int16_t simul_ir_pitch; +#endif + +#define UNINIT 0 +#define GOT_SYNC1 1 +#define GOT_SYNC2 2 +#define GOT_CLASS 3 +#define GOT_ID 4 +#define GOT_LEN1 5 +#define GOT_LEN2 6 +#define GOT_PAYLOAD 7 +#define GOT_CHECKSUM1 8 +#ifdef SIMUL +#define GOT_IR_START 20 +#define GOT_IR1 21 +#define GOT_IR2 22 +#define GOT_IR3 23 +#endif + +static uint8_t ubx_status; +static uint16_t ubx_len; +static uint8_t ubx_msg_idx; +static uint8_t ck_a, ck_b, ubx_id, ubx_class; + +void gps_init( void ) { + /* Enable uart */ +#ifdef SIMUL + uart0_init(); + simul_ir_roll = ir_roll_neutral; + simul_ir_pitch = ir_pitch_neutral; +#else + uart1_init(); +#endif + ubx_status = UNINIT; +} + +void parse_gps_msg( void ) { + if (ubx_class == UBX_NAV_ID) { + if (ubx_id == UBX_NAV_POSUTM_ID) { + gps_utm_east = UBX_NAV_POSUTM_EAST(ubx_msg_buf); + gps_utm_north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf); + gps_falt = (float)(UBX_NAV_POSUTM_ALT(ubx_msg_buf)/100); + } else if (ubx_id == UBX_NAV_STATUS_ID) { + gps_mode = UBX_NAV_STATUS_GPSfix(ubx_msg_buf); + } else if (ubx_id == UBX_NAV_VELNED_ID) { + gps_fspeed = ((float)UBX_NAV_VELNED_GSpeed(ubx_msg_buf)) / 1e2; + gps_fclimb = ((float)UBX_NAV_VELNED_VEL_D(ubx_msg_buf)) / -1e2; + gps_fcourse = RadianOfDeg(((float)UBX_NAV_VELNED_Heading(ubx_msg_buf)) / 1e5); + gps_ftow = ((float)UBX_NAV_VELNED_ITOW(ubx_msg_buf)) / 1e3; + + gps_east = gps_utm_east / 100 - NAV_UTM_EAST0; + gps_north = gps_utm_north / 100 - NAV_UTM_NORTH0; + + + gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */ + } + } +#ifdef SIMUL + if (ubx_class == UBX_USR_ID) { + if (ubx_id == UBX_USR_IRSIM_ID) { + simul_ir_roll = UBX_USR_IRSIM_ROLL(ubx_msg_buf); + simul_ir_pitch = UBX_USR_IRSIM_PITCH(ubx_msg_buf); + } + } +#endif + + + +} + + +uint8_t gps_nb_ovrn; + + +static inline void parse_ubx( uint8_t c ) { + if (ubx_status < GOT_PAYLOAD) { + ck_a += c; + ck_b += ck_a; + } + switch (ubx_status) { + case UNINIT: + if (c == UBX_SYNC1) + ubx_status++; + break; + case GOT_SYNC1: + if (c != UBX_SYNC2) + goto error; + ck_a = 0; + ck_b = 0; + ubx_status++; + break; + case GOT_SYNC2: + if (gps_msg_received) { + /* Previous message has not yet been parsed: discard this one */ + gps_nb_ovrn++; + goto error; + } + ubx_class = c; + ubx_status++; + break; + case GOT_CLASS: + ubx_id = c; + ubx_status++; + break; + case GOT_ID: + ubx_len = c; + ubx_status++; + break; + case GOT_LEN1: + ubx_len |= (c<<8); + if (ubx_len > UBX_MAX_PAYLOAD) + goto error; + ubx_msg_idx = 0; + ubx_status++; + break; + case GOT_LEN2: + ubx_msg_buf[ubx_msg_idx] = c; + ubx_msg_idx++; + if (ubx_msg_idx >= ubx_len) { + ubx_status++; + } + break; + case GOT_PAYLOAD: + if (c != ck_a) + goto error; + ubx_status++; + break; + case GOT_CHECKSUM1: + if (c != ck_b) + goto error; + gps_msg_received = TRUE; + goto restart; + break; + } + return; + error: + restart: + ubx_status = UNINIT; + return; +} + +#ifdef SIMUL +ReceiveUart0(parse_ubx); +#else +ReceiveUart1(parse_ubx); +#endif + diff --git a/sw/airborne/autopilot/if_calib.c b/sw/airborne/autopilot/if_calib.c new file mode 100644 index 0000000000..e13535365d --- /dev/null +++ b/sw/airborne/autopilot/if_calib.c @@ -0,0 +1,94 @@ +/* + * $Id$ + * Flight-time calibration facility + * + * 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. + * + */ + + +#include +#include "radio.h" +#include "autopilot.h" +#include "if_calib.h" +#include "infrared.h" +#include "pid.h" +#include "nav.h" + + +#define ParamValInt16(param_init_val, param_travel, cur_pulse, init_pulse) \ +(param_init_val + (int16_t)(((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ)) + +#define ParamValFloat(param_init_val, param_travel, cur_pulse, init_pulse) \ +(param_init_val + ((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ) + + + +uint8_t inflight_calib_mode = IF_CALIB_MODE_NONE; + +static int16_t slider1_init, slider2_init; + +#include "inflight_calib.h" + + +/*** +inline uint8_t inflight_calib(void) { + static int16_t slider1_init, slider2_init; + //static float ir_gain_init; + //static float roll_pgain_init; + static float course_pgain_init; + static int16_t roll_neutral_init; + static float pitch_pgain_init; + static int16_t pitch_neutral_init; + + int8_t mode_changed = inflight_calib_mode_update(); + + if (inflight_calib_mode == IF_CALIB_MODE_NEUTRAL) { + if (mode_changed) { + pitch_neutral_init = ir_pitch_neutral; + roll_neutral_init = ir_roll_neutral; + slider1_init = from_fbw.channels[RADIO_GAIN1]; + slider2_init = from_fbw.channels[RADIO_GAIN2]; + } + ir_pitch_neutral = PARAM_VAL_INT16( pitch_neutral_init, -60., from_fbw.channels[RADIO_GAIN1], slider1_init); + ir_roll_neutral = PARAM_VAL_INT16( roll_neutral_init, 60., from_fbw.channels[RADIO_GAIN2], slider2_init); + } + else if (inflight_calib_mode == IF_CALIB_MODE_GAIN) { + if (mode_changed) { + // ir_gain_init = ir_gain; + course_pgain_init = course_pgain; + // roll_pgain_init = roll_pgain; + pitch_pgain_init = pitch_pgain; + slider1_init = from_fbw.channels[RADIO_GAIN1]; + slider2_init = from_fbw.channels[RADIO_GAIN2]; + } + course_pgain = PARAM_VAL_FLOAT( course_pgain_init, -0.1, from_fbw.channels[RADIO_GAIN1], slider1_init); + // ir_gain = PARAM_VAL_FLOAT( ir_gain_init, 0.0015, from_fbw.channels[RADIO_GAIN2], slider2_init); + // roll_pgain = PARAM_VAL_FLOAT( roll_pgain_init, -5000., from_fbw.channels[RADIO_GAIN2], slider1_init); + pitch_pgain = PARAM_VAL_FLOAT( pitch_pgain_init, -5000., from_fbw.channels[RADIO_GAIN1], slider1_init); + } + return (mode_changed); +} +***/ + + + + + diff --git a/sw/airborne/autopilot/if_calib.h b/sw/airborne/autopilot/if_calib.h new file mode 100644 index 0000000000..9a89799410 --- /dev/null +++ b/sw/airborne/autopilot/if_calib.h @@ -0,0 +1,18 @@ +#ifndef IF_CALIB_H + +#include "link_fbw.h" + +extern uint8_t inflight_calib_mode; +void inflight_calib(bool_t calib_mode_changed); + + +#define IF_CALIB_MODE_NONE 0 +#define IF_CALIB_MODE_DOWN 1 +#define IF_CALIB_MODE_UP 2 + +#define IF_CALIB_MODE_OF_PULSE(pprz) (pprz < TRESHOLD1 ? IF_CALIB_MODE_UP : \ + (pprz < TRESHOLD2 ? IF_CALIB_MODE_NONE : \ + IF_CALIB_MODE_DOWN)) + + +#endif // IF_CALIB_H diff --git a/sw/airborne/autopilot/infrared.c b/sw/airborne/autopilot/infrared.c new file mode 100644 index 0000000000..0c120c13d1 --- /dev/null +++ b/sw/airborne/autopilot/infrared.c @@ -0,0 +1,71 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +#include "adc.h" +#include "infrared.h" +#include "autopilot.h" +#include "estimator.h" + +int16_t ir_roll; +int16_t ir_pitch; + +int16_t ir_contrast = IR_DEFAULT_CONTRAST; +int16_t ir_roll_neutral = IR_ROLL_NEUTRAL_DEFAULT; +int16_t ir_pitch_neutral = IR_PITCH_NEUTRAL_DEFAULT; + +#define RadOfIrFromConstrast(c) ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / c; + +float ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / IR_DEFAULT_CONTRAST; + +static struct adc_buf buf_ir1; +static struct adc_buf buf_ir2; + +void ir_init(void) { + RadOfIrFromConstrast(IR_DEFAULT_CONTRAST); + adc_buf_channel(ADC_CHANNEL_IR1, &buf_ir1); + adc_buf_channel(ADC_CHANNEL_IR2, &buf_ir2); +} + +void ir_update(void) { +#ifndef SIMUL + int16_t x1_mean = buf_ir1.sum/AV_NB_SAMPLE; + int16_t x2_mean = buf_ir2.sum/AV_NB_SAMPLE; + ir_roll = IR_RollOfIrs(x1_mean, x2_mean) - ir_roll_neutral; + ir_pitch = IR_PitchOfIrs(x1_mean, x2_mean) - ir_pitch_neutral; +#else + extern volatile int16_t simul_ir_roll, simul_ir_pitch; + ir_roll = simul_ir_roll - ir_roll_neutral; + ir_pitch = simul_ir_pitch - ir_pitch_neutral; +#endif +} + +/* + Contrast measurement +*/ + +void ir_gain_calib(void) { // Plane nose down + /* plane nose down -> negativ value */ + ir_contrast = - ir_pitch; + RadOfIrFromConstrast(ir_contrast); +} diff --git a/sw/airborne/autopilot/infrared.h b/sw/airborne/autopilot/infrared.h new file mode 100644 index 0000000000..2823e8cd11 --- /dev/null +++ b/sw/airborne/autopilot/infrared.h @@ -0,0 +1,44 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +#ifndef INFRARED_H +#define INFRARED_H + +#define AXIS_1_CHANNEL 4 /* P */ +#define AXIS_2_CHANNEL 5 /* other one */ + +extern int16_t ir_roll; /* averaged roll adc */ +extern int16_t ir_pitch; /* averaged pitch adc */ + + +extern float ir_rad_of_ir; +extern int16_t ir_contrast; +extern int16_t ir_roll_neutral; +extern int16_t ir_pitch_neutral; + +void ir_init(void); +void ir_update(void); +void ir_gain_calib(void); + +#endif /* INFRARED_H */ diff --git a/sw/airborne/autopilot/link_fbw.c b/sw/airborne/autopilot/link_fbw.c new file mode 100644 index 0000000000..a64f92a881 --- /dev/null +++ b/sw/airborne/autopilot/link_fbw.c @@ -0,0 +1,122 @@ +/* + * $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. + * + */ + +#include +#include +#include + +#include "link_fbw.h" +#include "spi.h" + +struct inter_mcu_msg from_fbw; +struct inter_mcu_msg to_fbw; +volatile uint8_t link_fbw_receive_complete = TRUE; +volatile uint8_t link_fbw_receive_valid = FALSE; +volatile uint8_t link_fbw_nb_err; +uint8_t link_fbw_fbw_nb_err; + +static uint8_t idx_buf; +static uint8_t xor_in, xor_out; + +void link_fbw_init(void) { + link_fbw_nb_err; + link_fbw_receive_complete = FALSE; +} + +void link_fbw_send(void) { + if (spi_cur_slave != SPI_NONE) { + spi_nb_ovrn++; + return; + } + + /* Enable SPI, Master, set clock rate fck/16 */ + SPI_START(_BV(SPE) | _BV(MSTR) | _BV(SPR0)); // | _BV(SPR1); + SPI_SELECT_SLAVE0(); + + idx_buf = 0; + xor_in = 0; + xor_out = ((uint8_t*)&to_fbw)[idx_buf]; + SPDR = xor_out; + link_fbw_receive_valid = FALSE; + // Other bytes will follow SIG_SPI interrupts +} + +void link_fbw_on_spi_it( void ) { + /* setup OCR1A to pop in 200 clock cycles */ + /* this leaves time for the slave (fbw) */ + /* to process the byte we've sent and to */ + /* prepare a new one to be sent */ + OCR1A = TCNT1 + 200; + /* clear interrupt flag */ + sbi(TIFR, OCF1A); + /* enable OC1A interrupt */ + sbi(TIMSK, OCIE1A); +} + + +/* send the next byte */ +SIGNAL(SIG_OUTPUT_COMPARE1A) { + uint8_t tmp; + + /* disable OC1A interrupt */ + cbi(TIMSK, OCIE1A); + + idx_buf++; + + /* we have sent/received a complete frame */ + if (idx_buf == FRAME_LENGTH) { + /* read checksum from receive register */ + tmp = SPDR; + /* notify valid frame */ + if (tmp == xor_in) { + link_fbw_receive_valid = TRUE; + link_fbw_fbw_nb_err = from_fbw.nb_err; + } + else + link_fbw_nb_err++; + link_fbw_receive_complete = TRUE; + /* unselect slave0 */ + SPI_UNSELECT_SLAVE0(); + SPI_STOP(); + return; + } + + /* we are sending/receiving payload */ + if (idx_buf < FRAME_LENGTH - 1) { + /* place new payload byte in send register */ + tmp = ((uint8_t*)&to_fbw)[idx_buf]; + SPI_SEND(tmp); + xor_out ^= tmp; + } + /* we are done sending the payload */ + else { // idx_buf == FRAME_LENGTH - 1 + /* place checksum in send register */ + SPI_SEND(xor_out); + } + + /* read the byte from receive register */ + tmp = SPDR; + ((uint8_t*)&from_fbw)[idx_buf-1] = tmp; + xor_in ^= tmp; +} diff --git a/sw/airborne/autopilot/link_fbw.h b/sw/airborne/autopilot/link_fbw.h new file mode 100644 index 0000000000..ce7acf52ad --- /dev/null +++ b/sw/airborne/autopilot/link_fbw.h @@ -0,0 +1,44 @@ +/* + * $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. + * + */ + +#ifndef LINK_FBW_H +#define LINK_FBW_H + +#include + +#include "link_autopilot.h" + +void link_fbw_init(void); +void link_fbw_send(void); +void link_fbw_on_spi_it(void); + +extern volatile uint8_t link_fbw_nb_err; +extern uint8_t link_fbw_fbw_nb_err; + +extern struct inter_mcu_msg from_fbw; +extern struct inter_mcu_msg to_fbw; +extern volatile uint8_t link_fbw_receive_complete; +extern volatile uint8_t link_fbw_receive_valid; + +#endif /* LINK_FBW_H */ diff --git a/sw/airborne/autopilot/lls.c b/sw/airborne/autopilot/lls.c new file mode 100644 index 0000000000..85100f9d91 --- /dev/null +++ b/sw/airborne/autopilot/lls.c @@ -0,0 +1,68 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +/* Linear Least Square regression */ + +#include "lls.h" +#include "infrared.h" + +float lls_a; +float lls_b; +float lls_x; +float lls_y; + +void lls_init() { + float lls_a_init = ir_gain; + float lls_b_init = (float)(-ir_roll_neutral) * lls_a_init; + lls_x = ir_roll_neutral + LLS_IR_HALF_INTERVAL; + lls_y = lls_a_init * lls_x + lls_b_init; + lls_update(); + lls_x = ir_roll_neutral - LLS_IR_HALF_INTERVAL; + lls_y = lls_a_init * lls_x + lls_b_init; + lls_update(); +} + +void lls_update() { + static float sum_x = 0.; + static float sum_y = 0.; + static float sum_xy = 0.; + static float sum_x2 = 0.; + static uint16_t n = 0; + float fn; + float mean_x, mean_y, c_xy, s2_x; + + n++; + sum_x += lls_x; + sum_y += lls_y; + sum_xy += lls_x * lls_y; + sum_x2 += lls_x * lls_x; + fn = (float)n; + + mean_x = sum_x / fn; + mean_y = sum_y / fn; + c_xy = mean_x * mean_y + (sum_xy - mean_x * sum_y - mean_y * sum_x ) / fn; + s2_x = mean_x * mean_x + (sum_x2 - 2* mean_x * sum_x) / fn; + lls_a = c_xy / s2_x; + lls_b = mean_y - lls_a * mean_x; +} diff --git a/sw/airborne/autopilot/lls.h b/sw/airborne/autopilot/lls.h new file mode 100644 index 0000000000..f57c301844 --- /dev/null +++ b/sw/airborne/autopilot/lls.h @@ -0,0 +1,42 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +/* Linear Least Square regression : y = lls_a * x + lls_b */ + +#ifndef LLS_H +#define LLS_H + +#include + +#define LLS_IR_HALF_INTERVAL 150 + +extern float lls_a; +extern float lls_b; +extern float lls_x; +extern float lls_y; + +void lls_update(void); +void lls_init(void); + +#endif diff --git a/sw/airborne/autopilot/main.c b/sw/airborne/autopilot/main.c new file mode 100644 index 0000000000..d4cdae1e65 --- /dev/null +++ b/sw/airborne/autopilot/main.c @@ -0,0 +1,377 @@ +/* + * $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. + * + */ + +#include +#include + + +#include "link_autopilot.h" + +#include "timer.h" +#include "adc.h" +#include "pid.h" +#include "gps.h" +#include "infrared.h" +#include "downlink.h" +#include "nav.h" +#include "autopilot.h" +#include "estimator.h" +#include "if_calib.h" + +// +// +// FIXME estimator_flight_time should not be manipuled here anymore +// +#define MIN_SPEED_FOR_TAKEOFF 5. // m/s + + +uint8_t fatal_error_nb = 0; +static const uint16_t version = 1; + +static uint16_t cputime = 0; // seconds + +uint8_t pprz_mode = PPRZ_MODE_MANUAL; +uint8_t vertical_mode = VERTICAL_MODE_MANUAL; +uint8_t ir_estim_mode = IR_ESTIM_MODE_ON; + +bool_t rc_event_1, rc_event_2; + +uint8_t vsupply; + +static uint8_t mcu1_status, mcu1_ppm_cpt; + +static bool_t low_battery = FALSE; + +#ifdef CTL_BRD_V1_1 +struct adc_buf buf_bat; +#endif + +float slider_1_val, slider_2_val; + +bool_t launch = FALSE; + + +#define Min(x, y) (x < y ? x : y) +#define Max(x, y) (x > y ? x : y) + + +#define NO_CALIB 0 +#define WAITING_CALIB_CONTRAST 1 +#define CALIB_DONE 2 + +#define MAX_DELAY_FOR_CALIBRATION 10 + + +inline void ground_calibrate(void) { + static uint8_t calib_status = NO_CALIB; + switch (calib_status) { + case NO_CALIB: + if (cputime < MAX_DELAY_FOR_CALIBRATION && pprz_mode == PPRZ_MODE_AUTO1 ) { + calib_status = WAITING_CALIB_CONTRAST; + DOWNLINK_SEND_CALIB_START(); + } + break; + case WAITING_CALIB_CONTRAST: + if (STICK_PUSHED(from_fbw.channels[RADIO_ROLL])) { + ir_gain_calib(); + estimator_rad_of_ir = ir_rad_of_ir; + DOWNLINK_SEND_RAD_OF_IR(&estimator_ir, &estimator_rad, &estimator_rad_of_ir, &ir_roll_neutral, &ir_pitch_neutral); + calib_status = CALIB_DONE; + DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast); + } + break; + case CALIB_DONE: + break; + } +} + + + +inline uint8_t pprz_mode_update( void ) { + /* We remain in home mode until explicit reset from the RC */ + if (pprz_mode != PPRZ_MODE_HOME || CheckEvent(rc_event_1)) { + ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(from_fbw.channels[RADIO_MODE], from_fbw.status)); + nav_stage = 0; /* Restart the last current block */ + } else + return FALSE; +} + +#ifdef RADIO_LLS +inline uint8_t ir_estim_mode_update( void ) { + ModeUpdate(ir_estim_mode, IR_ESTIM_MODE_OF_PULSE(from_fbw.channels[RADIO_LLS])); +} +#endif + + +inline uint8_t mcu1_status_update( void ) { + uint8_t new_mode = from_fbw.status; + if (mcu1_status != new_mode) { + bool_t changed = ((mcu1_status&MASK_FBW_CHANGED) != (new_mode&MASK_FBW_CHANGED)); + mcu1_status = new_mode; + return changed; + } + return FALSE; +} + +#define EVENT_DELAY 20 + +#define EventUpdate(_cpt, _cond, _event) \ + if (_cond) { \ + if (_cpt < EVENT_DELAY) { \ + _cpt++; \ + if (_cpt == EVENT_DELAY) \ + _event = TRUE; \ + } \ + } else { \ + _cpt = 0; \ + _event = FALSE; \ + } +#define EventPos(_cpt, _channel, _event) \ + EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[_channel]>MAX_PPRZ/2), _event) + +#define EventNeg(_cpt, _channel, _event) \ + EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[_channel]<-MAX_PPRZ/2), _event) + +static inline void events_update(void) { + static uint16_t event1_cpt = 0; + EventPos(event1_cpt, RADIO_GAIN1, rc_event_1); + static uint16_t event2_cpt = 0; + EventNeg(event2_cpt, RADIO_GAIN1, rc_event_2); +} + + +/* Send back uncontrolled channels (only rudder) */ +inline void copy_from_to_fbw ( void ) { + to_fbw.channels[RADIO_YAW] = from_fbw.channels[RADIO_YAW]; + to_fbw.status = 0; +} + +#ifdef EST_TEST +float est_pos_x; +float est_pos_y; +float est_fcourse; +uint8_t ticks_last_est; // 20Hz +#endif /* EST_TEST */ + + + +/* + called at 20Hz. + sends a serie of initialisation messages followed by a stream of periodic ones +*/ + +#define INIT_MSG_NB 2 +#define HI_FREQ_PHASE_NB 5 + +static char ac_ident[16] = AIRFRAME_NAME; + +#define PERIODIC_SEND_BAT() DOWNLINK_SEND_BAT(&vsupply, &estimator_flight_time, &low_battery, &block_time, &stage_time) +#define PERIODIC_SEND_DEBUG() DOWNLINK_SEND_DEBUG(&link_fbw_nb_err, &link_fbw_fbw_nb_err, &modem_nb_ovrn, &gps_nb_ovrn, &mcu1_ppm_cpt); +#define PERIODIC_SEND_ATTITUDE() DOWNLINK_SEND_ATTITUDE(&estimator_phi, &estimator_psi, &estimator_theta); +#define PERIODIC_SEND_ADC() DOWNLINK_SEND_ADC(&ir_roll, &ir_pitch); +#define PERIODIC_SEND_STABILISATION() DOWNLINK_SEND_STABILISATION(&roll_pgain, &pitch_pgain); +#define PERIODIC_SEND_CLIMB_PID() DOWNLINK_SEND_CLIMB_PID(&desired_gaz, &desired_climb, &climb_sum_err, &climb_pgain); +#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode); +#define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude); +#define PERIODIC_SEND_PITCH() DOWNLINK_SEND_PITCH(&ir_pitch, &ir_pitch_neutral, &ir_gain); +#define PERIODIC_SEND_NAVIGATION_REF() DOWNLINK_SEND_NAVIGATION_REF(&utm_east0, &utm_north0); +#define PERIODIC_SEND_IDENT() DOWNLINK_SEND_IDENT(ac_ident); + +#ifdef RADIO_CALIB +#define PERIODIC_SEND_SETTINGS() if (inflight_calib_mode != IF_CALIB_MODE_NONE) DOWNLINK_SEND_SETTINGS(&inflight_calib_mode, &slider_1_val, &slider_2_val); +#else +#define PERIODIC_SEND_SETTINGS() +#endif + + +inline void reporting_task( void ) { + static uint8_t boot = TRUE; + + /* initialisation phase */ + if (boot) { + DOWNLINK_SEND_BOOT(&version); + DOWNLINK_SEND_RAD_OF_IR(&estimator_ir, &estimator_rad, &estimator_rad_of_ir, &ir_roll_neutral, &ir_pitch_neutral); + boot = FALSE; + } + /* periodic reporting */ + else { + PeriodicSend(); + } +} + +inline uint8_t inflight_calib_mode_update (void) { + ModeUpdate(inflight_calib_mode, IF_CALIB_MODE_OF_PULSE(from_fbw.channels[RADIO_CALIB])); +} + + +inline void radio_control_task( void ) { + if (link_fbw_receive_valid) { + uint8_t mode_changed = FALSE; + copy_from_to_fbw(); + if ((bit_is_set(from_fbw.status, RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL)) || too_far_from_home) { + pprz_mode = PPRZ_MODE_HOME; + mode_changed = TRUE; + } + if (bit_is_set(from_fbw.status, AVERAGED_CHANNELS_SENT)) { + bool_t pprz_mode_changed = pprz_mode_update(); + mode_changed |= pprz_mode_changed; +#ifdef RADIO_LLS + mode_changed |= ir_estim_mode_update(); +#endif +#ifdef RADIO_CALIB + bool_t calib_mode_changed = inflight_calib_mode_update(); + inflight_calib(calib_mode_changed || pprz_mode_changed); + mode_changed |= calib_mode_changed; +#endif + } + mode_changed |= mcu1_status_update(); + if ( mode_changed ) + DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode); + + if (pprz_mode == PPRZ_MODE_AUTO1) { + desired_roll = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_ROLL], 0., -0.6); + desired_pitch = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_PITCH], 0., 0.5); + } // else asynchronously set by course_pid_run() + if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) + desired_gaz = from_fbw.channels[RADIO_THROTTLE]; + // else asynchronously set by climb_pid_run(); + + mcu1_ppm_cpt = from_fbw.ppm_cpt; +#ifndef CTL_BRD_V1_1 + vsupply = from_fbw.vsupply; +#endif + + events_update(); + + if (!estimator_flight_time) { + ground_calibrate(); + if (pprz_mode == PPRZ_MODE_AUTO2 && from_fbw.channels[RADIO_THROTTLE] > GAZ_THRESHOLD_TAKEOFF) { + launch = TRUE; + } + } + } + +} + +void navigation_task( void ) { + + /* Compute desired_course */ + if (pprz_mode == PPRZ_MODE_HOME) + nav_home(); + else + nav_update_desired_course(); + + DOWNLINK_SEND_NAVIGATION(&nav_block, &nav_stage, &estimator_x, &estimator_y, &desired_course, &dist2_to_wp, &course_pgain, &dist2_to_home); + + if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { + course_pid_run(); /* aka compute desired_roll */ + desired_pitch = nav_pitch; + + if (vertical_mode == VERTICAL_MODE_AUTO_ALT) + altitude_pid_run(); + if (vertical_mode >= VERTICAL_MODE_AUTO_CLIMB) + climb_pid_run(); + if (vertical_mode == VERTICAL_MODE_AUTO_GAZ) + desired_gaz = nav_desired_gaz; + if (low_battery || (!estimator_flight_time && !launch)) + desired_gaz = 0.; + } + + + +} + +#define PERIOD (256. * 1024. / CLOCK / 1000000.) +inline void periodic_task( void ) { // 61 Hz + static uint8_t _20Hz = 0; + static uint8_t _4Hz = 0; + static uint8_t _1Hz = 0; + + estimator_t += PERIOD; + + _20Hz++; + if (_20Hz>=3) _20Hz=0; + _4Hz++; + if (_4Hz>=15) _4Hz=0; + _1Hz++; + if (_1Hz>=61) _1Hz=0; + + if (!_1Hz) { + if (estimator_flight_time) estimator_flight_time++; + cputime++; + stage_time++; + block_time++; + +#ifdef CTL_BRD_V1_1 + uint16_t av_bat_value = buf_bat.sum/AV_NB_SAMPLE; + vsupply = VoltageOfAdc(av_bat_value) * 10.; +#endif + low_battery |= (vsupply < LOW_BATTERY); + } + switch(_4Hz) { + case 0: + estimator_propagate_state(); + navigation_task(); + break; + // default: + } + switch (_20Hz) { + case 0: + break; + case 1: { + static uint8_t odd; + odd++; + if (odd & 0x01) + reporting_task(); + break; + } + case 2: + ir_update(); + estimator_update_state_infrared(); + roll_pitch_pid_run(); // Set desired_aileron & desired_elevator + to_fbw.channels[RADIO_THROTTLE] = desired_gaz; // desired_gaz is set upon GPS message reception + to_fbw.channels[RADIO_ROLL] = desired_aileron; + to_fbw.channels[RADIO_PITCH] = desired_elevator; + + // Code for camera stabilization, FIXME put that elsewhere + to_fbw.channels[RADIO_GAIN1] = TRIM_PPRZ(MAX_PPRZ/0.75*(-estimator_phi)); + + link_fbw_send(); + break; + default: + fatal_error_nb++; + } +} + + +void use_gps_pos(void) { + DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_fcourse, &gps_falt, &gps_fspeed,&gps_fclimb, &gps_ftow); + estimator_update_state_gps(); + DOWNLINK_SEND_RAD_OF_IR(&estimator_ir, &estimator_rad, &estimator_rad_of_ir, &ir_roll_neutral, &ir_pitch_neutral); + if (!estimator_flight_time && (estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF)) { + estimator_flight_time = 1; + launch = TRUE; /* Not set in non auto launch */ + DOWNLINK_SEND_TAKEOFF(&cputime); + } +} diff --git a/sw/airborne/autopilot/mainloop.c b/sw/airborne/autopilot/mainloop.c new file mode 100644 index 0000000000..baf336c33b --- /dev/null +++ b/sw/airborne/autopilot/mainloop.c @@ -0,0 +1,85 @@ +/* + * $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. + * + */ + +#include +#include "std.h" + +#include "timer.h" +#include "modem.h" +#include "adc.h" +#include "airframe.h" +#include "autopilot.h" +#include "spi.h" +#include "link_fbw.h" +#include "gps.h" +#include "nav.h" +#include "infrared.h" +#include "estimator.h" +#include "downlink.h" + + +int main( void ) { + /* init peripherals */ + timer_init(); + modem_init(); + adc_init(); +#ifdef CTL_BRD_V1_1 + adc_buf_channel(ADC_CHANNEL_BAT, &buf_bat); +#endif + spi_init(); + link_fbw_init(); + gps_init(); + nav_init(); + ir_init(); + estimator_init(); + + /* start interrupt task */ + sei(); + + /* Wait 0.5s (for modem init ?) */ + uint8_t init_cpt = 30; + while (init_cpt) { + if (timer_periodic()) + init_cpt--; + } + + /* enter mainloop */ + while( 1 ) { + if(timer_periodic()) + periodic_task(); + if (gps_msg_received) { + parse_gps_msg(); + gps_msg_received = FALSE; + if (gps_pos_available) { + use_gps_pos(); + gps_pos_available = FALSE; + } + } + if (link_fbw_receive_complete) { + link_fbw_receive_complete = FALSE; + radio_control_task(); + } + } + return 0; +} diff --git a/sw/airborne/autopilot/modem.c b/sw/airborne/autopilot/modem.c new file mode 100644 index 0000000000..5c6039316f --- /dev/null +++ b/sw/airborne/autopilot/modem.c @@ -0,0 +1,88 @@ +/* + * Paparazzi mcu0 cmx469 modem functions + * + * 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. + * + */ + +#include +#include +#include +#include +#include +#include "modem.h" + +uint8_t modem_nb_ovrn; + +uint8_t tx_head; +volatile uint8_t tx_tail; +uint8_t tx_buf[ TX_BUF_SIZE ]; + +uint8_t tx_byte; +uint8_t tx_byte_idx; + +uint8_t ck_a, ck_b; + +void modem_init( void ) { +#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1 + MODEM_OSC_DDR |= _BV(MODEM_OSC); + OCR0 = 1; /* 4MhZ */ + TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); +#endif + + /* setup TX_EN and TX_DATA pin as output */ + MODEM_TX_DDR |= _BV(MODEM_TX_EN) | _BV(MODEM_TX_DATA); + /* data idles hight */ + sbi(MODEM_TX_PORT, MODEM_TX_DATA); + /* enable transmitter */ + cbi(MODEM_TX_PORT, MODEM_TX_EN); + /* set interrupt on failing edge of clock */ + MODEM_CLK_INT_REG |= MODEM_CLK_INT_CFG; +} + +SIGNAL( MODEM_CLK_INT_SIG ) { + /* start bit */ + if (tx_byte_idx == 0) + cbi(MODEM_TX_PORT, MODEM_TX_DATA); + /* 8 data bits */ + else if (tx_byte_idx < 9) { + if (tx_byte & 0x01) + sbi(MODEM_TX_PORT, MODEM_TX_DATA); + else + cbi(MODEM_TX_PORT, MODEM_TX_DATA); + tx_byte >>= 1; + } + /* stop_bit */ + else { + sbi(MODEM_TX_PORT, MODEM_TX_DATA); + } + tx_byte_idx++; + /* next byte */ + if (tx_byte_idx >= 10) { + /* if we have nothing left to transmit */ + if( tx_head == tx_tail ) { + /* disable clock interrupt */ + cbi( EIMSK, MODEM_CLK_INT ); + } else { + /* else load next byte */ + MODEM_LOAD_NEXT_BYTE(); + } + } +} diff --git a/sw/airborne/autopilot/modem.h b/sw/airborne/autopilot/modem.h new file mode 100644 index 0000000000..2d84056e16 --- /dev/null +++ b/sw/airborne/autopilot/modem.h @@ -0,0 +1,140 @@ +/* + * Paparazzi mcu0 cmx469 modem functions + * + * 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. + * + */ + +#ifndef MODEM_H +#define MODEM_H + +#include "airframe.h" + +void modem_init( void ); + +extern uint8_t modem_nb_ovrn; + +#define TX_BUF_SIZE 255 +extern uint8_t tx_head; +extern volatile uint8_t tx_tail; +extern uint8_t tx_buf[ TX_BUF_SIZE ]; + +extern uint8_t tx_byte; +extern uint8_t tx_byte_idx; + +extern uint8_t ck_a, ck_b; + +#define ModemStartMessage(id) \ + { MODEM_PUT_1_BYTE(STX); MODEM_PUT_1_BYTE(id); ck_a = id; ck_b = id; } + +#define ModemEndMessage() \ + { MODEM_PUT_1_BYTE(ck_a); MODEM_PUT_1_BYTE(ck_b); MODEM_CHECK_RUNNING(); } + + +#define MODEM_TX_PORT PORTD +#define MODEM_TX_DDR DDRD +#define MODEM_TX_EN 7 +#define MODEM_TX_DATA 6 + +#ifdef CTL_BRD_V1_1 +#define MODEM_CLK_DDR DDRD +#define MODEM_CLK_PORT PORTD +#define MODEM_CLK 0 +#define MODEM_CLK_INT INT0 +#define MODEM_CLK_INT_REG EICRA +#define MODEM_CLK_INT_CFG _BV(ISC01) +#define MODEM_CLK_INT_SIG SIG_INTERRUPT0 +#endif /* CTL_BRD_V1_1 */ + +#ifdef CTL_BRD_V1_2 +#define MODEM_CLK_DDR DDRD +#define MODEM_CLK_PORT PORTD +#define MODEM_CLK 0 +#define MODEM_CLK_INT INT0 +#define MODEM_CLK_INT_REG EICRA +#define MODEM_CLK_INT_CFG _BV(ISC01) +#define MODEM_CLK_INT_SIG SIG_INTERRUPT0 + +#define MODEM_OSC_DDR DDRB +#define MODEM_OSC_PORT PORTB +#define MODEM_OSC 4 +#endif /* CTL_BRD_V1_2 */ + +#ifdef CTL_BRD_V1_2_1 +#define MODEM_CLK_DDR DDRE +#define MODEM_CLK_PORT PORTE +#define MODEM_CLK 4 +#define MODEM_CLK_INT INT4 +#define MODEM_CLK_INT_REG EICRB +#define MODEM_CLK_INT_CFG _BV(ISC41) +#define MODEM_CLK_INT_SIG SIG_INTERRUPT4 +#define MODEM_OSC_DDR DDRB +#define MODEM_OSC_PORT PORTB +#define MODEM_OSC 4 +#endif /* CTL_BRD_V1_2_1 */ + + + +#define MODEM_CHECK_FREE_SPACE(_space) (tx_head>=tx_tail? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head)) + +#define MODEM_PUT_1_BYTE(_byte) { \ + tx_buf[tx_head] = _byte; \ + tx_head++; \ + if (tx_head >= TX_BUF_SIZE) tx_head = 0; \ +} + +#define MODEM_PUT_1_BYTE_BY_ADDR(_byte) { \ + tx_buf[tx_head] = *(_byte); \ + ck_a += *(_byte); \ + ck_b += ck_a; \ + tx_head++; \ + if (tx_head >= TX_BUF_SIZE) tx_head = 0; \ +} + +#define MODEM_PUT_2_BYTE_BY_ADDR(_byte) { \ + MODEM_PUT_1_BYTE_BY_ADDR(_byte); \ + MODEM_PUT_1_BYTE_BY_ADDR(_byte+1); \ +} + +#define MODEM_PUT_4_BYTE_BY_ADDR(_byte) { \ + MODEM_PUT_1_BYTE_BY_ADDR(_byte); \ + MODEM_PUT_1_BYTE_BY_ADDR(_byte+1); \ + MODEM_PUT_1_BYTE_BY_ADDR(_byte+2); \ + MODEM_PUT_1_BYTE_BY_ADDR(_byte+3); \ +} + +#define MODEM_LOAD_NEXT_BYTE() { \ + tx_byte = tx_buf[tx_tail]; \ + tx_byte_idx = 0; \ + tx_tail++; \ + if( tx_tail >= TX_BUF_SIZE ) \ + tx_tail = 0; \ +} + +#define MODEM_CHECK_RUNNING() { \ + if (!(EIMSK & _BV(MODEM_CLK_INT))) { \ + MODEM_LOAD_NEXT_BYTE() \ + sbi(EIFR, INTF0); \ + sbi(EIMSK, MODEM_CLK_INT); \ + } \ +} + + +#endif /* MODEM_H */ diff --git a/sw/airborne/autopilot/nav.c b/sw/airborne/autopilot/nav.c new file mode 100644 index 0000000000..3c0227bb55 --- /dev/null +++ b/sw/airborne/autopilot/nav.c @@ -0,0 +1,204 @@ +/* + * $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. + * + */ + +#define NAV_C + +#include + +#include "nav.h" +#include "estimator.h" +#include "pid.h" +#include "autopilot.h" +#include "link_fbw.h" +#include "airframe.h" + +uint8_t nav_stage, nav_block; +uint8_t excpt_stage; /*To save the current stage when an exception is raised */ +static float last_x, last_y; +static uint8_t last_wp; +float rc_pitch; +uint16_t stage_time, block_time; + +#define RcEvent1() CheckEvent(rc_event_1) +#define RcEvent2() CheckEvent(rc_event_2) +#define Block(x) case x: nav_block=x; +#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); } +#define NextBlock() { nav_block++; InitBlock(); } +#define GotoBlock(b) { nav_block=b; InitBlock(); } + +#define Stage(s) case s: nav_stage=s; +#define InitStage() { last_x = estimator_x; last_y = estimator_y; stage_time = 0; return; } +#define NextStage() { nav_stage++; InitStage() } +#define NextStageFrom(wp) { last_wp = wp; NextStage() } +#define GotoStage(s) { nav_stage = s; InitStage() } + +#define Label(x) label_ ## x: +#define Goto(x) { goto label_ ## x; } + +#define Exception(x) { excpt_stage = nav_stage; goto label_ ## x; } +#define ReturnFromException(_) { GotoStage(excpt_stage) } + +static bool_t approaching(uint8_t); +static inline void fly_to_xy(float x, float y); +static void fly_to(uint8_t wp); +static void route_to(uint8_t last_wp, uint8_t wp); +static void glide_to(uint8_t last_wp, uint8_t wp); + +#define MIN_DX ((int16_t)(MAX_PPRZ * 0.05)) + +#define DegOfRad(x) ((x) / M_PI * 180.) +#define RadOfDeg(x) ((x)/180. * M_PI) +#define NormCourse(x) { \ + while (x < 0) x += 360; \ + while (x >= 360) x -= 360; \ +} + +static float qdr; /* Degrees from 0 to 360 */ +#define CircleXY(x, y, radius) { \ + float alpha = atan2(estimator_y - y, \ + estimator_x - x); \ + float alpha_carrot = alpha + CARROT / -radius * estimator_hspeed_mod; \ + fly_to_xy(x+cos(alpha_carrot)*fabs(radius), \ + y+sin(alpha_carrot)*fabs(radius)); \ + qdr = DegOfRad(M_PI/2 - alpha_carrot); \ + NormCourse(qdr); \ +} + +static float carrot_x, carrot_y; + +#define Goto3D(radius) { \ + int16_t yaw = from_fbw.channels[RADIO_YAW]; \ + if (yaw > MIN_DX || yaw < -MIN_DX) \ + carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \ + int16_t pitch = from_fbw.channels[RADIO_PITCH]; \ + if (pitch > MIN_DX || pitch < -MIN_DX) \ + carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \ + vertical_mode = VERTICAL_MODE_AUTO_ALT; \ + int16_t roll = from_fbw.channels[RADIO_ROLL]; \ + if (roll > MIN_DX || roll < -MIN_DX) \ + desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \ + CircleXY(carrot_x, carrot_y, radius); \ +} +#define Circle(wp, radius) \ + CircleXY(waypoints[wp].x, waypoints[wp].y, radius) + +#define And(x, y) ((x) && (y)) +#define Or(x, y) ((x) || (y)) +#define Min(x,y) (x < y ? x : y) +#define Max(x,y) (x > y ? x : y) +#define Qdr(x) (Min(x, 350) < qdr && qdr < x+10) + +#include "flight_plan.h" + + +#define MIN_DIST2_WP (15.*15.) + +#define DISTANCE2(p1_x, p1_y, p2) ((p1_x-p2.x)*(p1_x-p2.x)+(p1_y-p2.y)*(p1_y-p2.y)) + +const int32_t nav_east0 = NAV_UTM_EAST0; +const int32_t nav_north0 = NAV_UTM_NORTH0; + +float desired_altitude, desired_x, desired_y; +uint16_t nav_desired_gaz; +float nav_pitch = NAV_PITCH; + +float dist2_to_wp, dist2_to_home; +bool_t too_far_from_home; +const uint8_t nb_waypoint = NB_WAYPOINT; + +struct point waypoints[NB_WAYPOINT+1] = WAYPOINTS; + +static float carrot; +static bool_t approaching(uint8_t wp) { + float pw_x = waypoints[wp].x - estimator_x; + float pw_y = waypoints[wp].y - estimator_y; + + dist2_to_wp = pw_x*pw_x + pw_y *pw_y; + carrot = CARROT * estimator_hspeed_mod; + carrot = (carrot < 40 ? 40 : carrot); + if (dist2_to_wp < carrot*carrot) + return TRUE; + + float scal_prod = (waypoints[wp].x - last_x) * pw_x + (waypoints[wp].y - last_y) * pw_y; + + return (scal_prod < 0); +} + +static inline void fly_to_xy(float x, float y) { + desired_x = x; + desired_y = y; + desired_course = M_PI/2.-atan2(y - estimator_y, x - estimator_x); +} + +static void fly_to(uint8_t wp) { + fly_to_xy(waypoints[wp].x, waypoints[wp].y); +} + +static float alpha, leg; +static void route_to(uint8_t _last_wp, uint8_t wp) { + float last_wp_x = waypoints[_last_wp].x; + float last_wp_y = waypoints[_last_wp].y; + float leg_x = waypoints[wp].x - last_wp_x; + float leg_y = waypoints[wp].y - last_wp_y; + float leg2 = leg_x * leg_x + leg_y * leg_y; + alpha = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2; + alpha = Max(alpha, 0.); + leg = sqrt(leg2); + alpha += Max(carrot / leg, 0.); /* carrot computed in approaching() */ + alpha = Min(1., alpha); + fly_to_xy(last_wp_x + alpha*leg_x, last_wp_y + alpha*leg_y); +} + +static void glide_to(uint8_t _last_wp, uint8_t wp) { + float last_alt = waypoints[_last_wp].a; + desired_altitude = last_alt + alpha * (waypoints[wp].a - last_alt); + pre_climb = NOMINAL_AIRSPEED * (waypoints[wp].a - last_alt) / leg; +} + +static inline void compute_dist2_to_home(void) { + float ph_x = waypoints[WP_HOME].x - estimator_x; + float ph_y = waypoints[WP_HOME].y - estimator_y; + dist2_to_home = ph_x*ph_x + ph_y *ph_y; + too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME); +} + +void nav_home(void) { + Circle(WP_HOME, 50); /* FIXME: radius should be defined elsewhere */ + nav_pitch = 0.; /* Nominal speed */ + vertical_mode = VERTICAL_MODE_AUTO_ALT; + desired_altitude = GROUND_ALT+50; + compute_dist2_to_home(); + dist2_to_wp = dist2_to_home; +} + +void nav_update_desired_course(void) { + compute_dist2_to_home(); + auto_nav(); +} + + +void nav_init(void) { + nav_block = 0; + nav_stage = 0; +} diff --git a/sw/airborne/autopilot/nav.h b/sw/airborne/autopilot/nav.h new file mode 100644 index 0000000000..93ac4ef428 --- /dev/null +++ b/sw/airborne/autopilot/nav.h @@ -0,0 +1,56 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +#ifndef NAV_H +#define NAV_H + +#include + +struct point { + float x; + float y; + float a; +}; +extern float cur_pos_x; +extern float cur_pos_y; +extern uint8_t nav_stage, nav_block; +extern float dist2_to_wp, dist2_to_home; + +extern const int32_t nav_east0; +extern const int32_t nav_north0; + +extern const uint8_t nb_waypoint; +extern struct point waypoints[]; +extern float desired_altitude, desired_x, desired_y; + +extern uint16_t nav_desired_gaz; +extern float nav_pitch, rc_pitch; +extern bool_t too_far_from_home; +extern uint16_t stage_time, block_time; /* s */ + +void nav_update_desired_course(void); +void nav_home(void); +void nav_init(void); + +#endif /* NAV_H */ diff --git a/sw/airborne/autopilot/pid.c b/sw/airborne/autopilot/pid.c new file mode 100644 index 0000000000..bd6be86557 --- /dev/null +++ b/sw/airborne/autopilot/pid.c @@ -0,0 +1,88 @@ +/* + * $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. + * + */ + +#include +#include + +#include "pid.h" + +#include "autopilot.h" +#include "infrared.h" +//#include "gps.h" +#include "estimator.h" +#include "nav.h" + +float desired_roll = 0.; +float desired_pitch = 0.; +int16_t desired_gaz, desired_aileron, desired_elevator; +float roll_pgain = ROLL_PGAIN; +float pitch_pgain = PITCH_PGAIN; +float pitch_of_roll = PITCH_OF_ROLL; + +void roll_pitch_pid_run( void ) { + float err = estimator_phi - desired_roll; + desired_aileron = TRIM_PPRZ(roll_pgain * err); + if (pitch_of_roll <0.) + pitch_of_roll = 0.; + err = -(estimator_theta - desired_pitch - pitch_of_roll * fabs(estimator_phi)); + desired_elevator = TRIM_PPRZ(pitch_pgain * err); +} + +float course_pgain = COURSE_PGAIN; +float desired_course = 0.; +float max_roll = MAX_ROLL; + +void course_pid_run( void ) { + float err = estimator_hspeed_dir - desired_course; + NORM_RAD_ANGLE(err); + float roll = course_pgain * err; // * fspeed / AIR_SPEED; + if (roll > max_roll) + desired_roll = max_roll; + else if (roll < -max_roll) + desired_roll = -max_roll; + else desired_roll = roll; +} + +const float climb_pgain = CLIMB_PGAIN; +const float climb_igain = CLIMB_IGAIN; +float desired_climb = 0., pre_climb = 0.; +static const float level_gaz = CLIMB_LEVEL_GAZ; +float climb_sum_err = 0; + +void climb_pid_run ( void ) { + float err = estimator_z_dot - desired_climb; + float fgaz = climb_pgain * (err + climb_igain * climb_sum_err) + CLIMB_LEVEL_GAZ + (0.9-CLIMB_LEVEL_GAZ)/CLIMB_GAZ_MAX*desired_climb; + climb_sum_err += err; + desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ); +} + +float altitude_pgain = ALTITUDE_PGAIN; + + +void altitude_pid_run(void) { + float err = estimator_z - desired_altitude; + desired_climb = pre_climb + altitude_pgain * err; + if (desired_climb < -CLIMB_MAX) desired_climb = -CLIMB_MAX; + if (desired_climb > CLIMB_MAX) desired_climb = CLIMB_MAX; +} diff --git a/sw/airborne/autopilot/pid.h b/sw/airborne/autopilot/pid.h new file mode 100644 index 0000000000..00efbcabd5 --- /dev/null +++ b/sw/airborne/autopilot/pid.h @@ -0,0 +1,58 @@ +/* + * Paparazzi mcu0 $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. + * + */ + +#ifndef PID_H +#define PID_H + +#include + +#define NORM_RAD_ANGLE(x) { \ + while (x > M_PI) x -= 2 * M_PI; \ + while (x < -M_PI) x += 2 * M_PI; \ + } + +extern float desired_roll; +extern float max_roll; +extern float desired_pitch; +extern float roll_pgain; +extern float pitch_pgain; +extern float pitch_of_roll; +void roll_pitch_pid_run( void ); + + +extern float course_pgain; +extern float desired_course; +void course_pid_run( void ); + + +extern const float climb_pgain; +extern const float climb_igain; +extern float climb_sum_err; +extern float desired_climb, pre_climb; +extern int16_t desired_gaz, desired_aileron, desired_elevator; + +void climb_pid_run(void); +void altitude_pid_run(void); + +#endif /* PID_H */ diff --git a/sw/airborne/autopilot/sirf.h b/sw/airborne/autopilot/sirf.h new file mode 100644 index 0000000000..b0969acb56 --- /dev/null +++ b/sw/airborne/autopilot/sirf.h @@ -0,0 +1,36 @@ +/* + * Paparazzi autopilot $Id$ + * + * Copyright (C) 2004 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. + * + */ + +/* + * SIRF protocol specific code + * +*/ + + +#ifndef SIRF_H +#define SIRF_H + +#define GPS_FIX_VALID(gps_mode) (gps_mode & 1<<5) + +#endif /* SIRF_H */ diff --git a/sw/airborne/autopilot/spi.c b/sw/airborne/autopilot/spi.c new file mode 100644 index 0000000000..9466c59cf2 --- /dev/null +++ b/sw/airborne/autopilot/spi.c @@ -0,0 +1,40 @@ +#include +#include +#include +#include + + +#include "spi.h" +#include "autopilot.h" +#include "link_fbw.h" + +volatile uint8_t spi_cur_slave; +uint8_t spi_nb_ovrn; + +void spi_init( void) { + /* Set MOSI and SCK output, all others input */ + SPI_DDR |= _BV(SPI_MOSI_PIN)| _BV(SPI_SCK_PIN); + + /* enable pull up for miso */ + // SPI_PORT |= _BV(SPI_MISO_PIN); + + /* Set SS0 output */ + sbi( SPI_SS0_DDR, SPI_SS0_PIN); + /* SS0 idles high (don't select slave yet)*/ + SPI_UNSELECT_SLAVE0(); + + /* Set SS1 output */ + sbi( SPI_SS1_DDR, SPI_SS1_PIN); + /* SS1 idles high (don't select slave yet)*/ + SPI_UNSELECT_SLAVE1(); + + spi_cur_slave = SPI_NONE; +} + + +SIGNAL(SIG_SPI) { + if (spi_cur_slave == SPI_SLAVE0) + link_fbw_on_spi_it(); + else + fatal_error_nb++; +} diff --git a/sw/airborne/autopilot/spi.h b/sw/airborne/autopilot/spi.h new file mode 100644 index 0000000000..556f60c12d --- /dev/null +++ b/sw/airborne/autopilot/spi.h @@ -0,0 +1,74 @@ +#ifndef SPI_H +#define SPI_H + +//#include "link_autopilot.h" + +#define SPI_SS0_PIN 0 +#define SPI_SS0_PORT PORTB +#define SPI_SS0_DDR DDRB +#define SPI_IT0_PIN 1 +#define SPI_IT0_PORT PORTD +#define SPI_IT0_DDR DDRD + +#define SPI_SS1_PIN 7 +#define SPI_SS1_PORT PORTE +#define SPI_SS1_DDR DDRE +#define SPI_IT1_PIN 6 +#define SPI_IT1_PORT PORTE +#define SPI_IT1_DDR DDRE + +#define SPI_SCK_PIN 1 +#define SPI_MOSI_PIN 2 +#define SPI_MISO_PIN 3 +#define SPI_PORT PORTB +#define SPI_DDR DDRB + + + +#define SPI_NONE 0 +#define SPI_SLAVE0 1 +#define SPI_SLAVE1 2 + +extern volatile uint8_t spi_cur_slave; +extern uint8_t spi_nb_ovrn; + +void spi_init( void); + +#define SPI_START(_SPCR_VAL) { \ + uint8_t foo; \ + SPCR = _SPCR_VAL; \ + if (bit_is_set(SPSR, SPIF)) \ + foo = SPDR; \ + SPCR |= _BV(SPIE); \ +} + +#define SPI_SELECT_SLAVE0() { \ + spi_cur_slave = SPI_SLAVE0; \ + cbi( SPI_SS0_PORT, SPI_SS0_PIN );\ +} + +#define SPI_UNSELECT_SLAVE0() { \ + spi_cur_slave = SPI_NONE; \ + sbi( SPI_SS0_PORT, SPI_SS0_PIN );\ +} + +#define SPI_SELECT_SLAVE1() { \ + spi_cur_slave = SPI_SLAVE1; \ + cbi( SPI_SS1_PORT, SPI_SS1_PIN );\ +} + +#define SPI_UNSELECT_SLAVE1() { \ + spi_cur_slave = SPI_NONE; \ + sbi( SPI_SS1_PORT, SPI_SS1_PIN );\ +} + +#define SPI_SEND(data) { \ + SPDR = data; \ +} + +#define SPI_STOP() { \ + cbi(SPCR,SPIE); \ + cbi(SPCR, SPE); \ +} + +#endif /* SPI_H */ diff --git a/sw/airborne/autopilot/test/Makefile b/sw/airborne/autopilot/test/Makefile new file mode 100644 index 0000000000..293b3b4363 --- /dev/null +++ b/sw/airborne/autopilot/test/Makefile @@ -0,0 +1,52 @@ +# $Id$ + +all: + @echo "call with 'make TARGET=... compile (or load)'" + +TARGET=check_uart + +ARCH = atmega128 +INCLUDES = -I ../ -I ../../../include + +LOCAL_CFLAGS= $(CTL_BRD_FLAGS) + +CONF_DIR = ../../../../conf +CONF_XML = $(CONF_DIR)/conf.xml + +ifneq ($(MAKECMDGOALS),clean) + AIRFRAME_XML = $(CONF_DIR)/$(shell echo `../../../lib/ocaml/xml_get.out $(CONF_XML) "files" "airframe"`) + CTL_BRD_VERSION=$(shell echo `../../../lib/ocaml/xml_get.out $(AIRFRAME_XML) "airframe" "ctl_board"`) +endif + + + +ifeq ($(CTL_BRD_VERSION),V1_2_1) +CTL_BRD_FLAGS=-DCTL_BRD_V1_2_1 +endif + +ifeq ($(CTL_BRD_VERSION),V1_2) +CTL_BRD_FLAGS=-DCTL_BRD_V1_2 +endif + +test_modem.srcs = test_modem.c ../modem.c + +check_uart.srcs = check_uart.c ../uart.c + +tx_adcs.srcs = tx_adcs.c ../uart.c ../adc.c + +test_v2xe.srcs = test_v2xe.c + +uart_tunnel.srcs = uart_tunnel.c ../uart.c +check_spi.srcs = check_spi.c ../uart.c ../spi.c ../link_fbw.c + +check_spi.o : INCLUDES = -I ../../../include -I ../ -I ../../fly_by_wire + +test_fp: test_fp.c ../flight_plan.h + cc -o $@ -I sim_avr -I .. -I ../../fly_by_wire ../nav.c test_fp.c -lm + +include ../../../../conf/Makefile.local +include ../../../../conf/Makefile.avr + + + +clean: avr_clean diff --git a/sw/airborne/autopilot/test/check_spi.c b/sw/airborne/autopilot/test/check_spi.c new file mode 100644 index 0000000000..afe71b070d --- /dev/null +++ b/sw/airborne/autopilot/test/check_spi.c @@ -0,0 +1,46 @@ +#include +#include "timer.h" +#include "link_fbw.h" +#include "uart.h" + +uint8_t fatal_error_nb; /* Used in spi.c */ + +/* Fill the message with dummy values */ +void fill_spi_msg(void) { + static pprz_t x; + + uint8_t i; + for(i = 0; i < RADIO_CTL_NB; i++) + to_fbw.channels[i] = x++; + to_fbw.status = 0xff; + to_fbw.ppm_cpt = 0xff; + to_fbw.vsupply = 0xff; +} + +int main( void ) { + uart0_init(); + uart0_print_string("Booting AP MCU: $Id$\n"); + link_fbw_init(); + timer_init(); + sei(); + + uint8_t _1Hz = 0; + while( 1 ) { + if(timer_periodic()) { + _1Hz++; + if (_1Hz >= 60) { + _1Hz = 0; + uart0_print_string("AP MCU Alive\n"); + fill_spi_msg(); + link_fbw_send(); + } + } + if (link_fbw_receive_complete) { + link_fbw_receive_complete = FALSE; + if (link_fbw_receive_valid) + uart0_print_string("SPI OK from fbw\n"); + else + uart0_print_string("SPI error from fbw\n"); + } + } +} diff --git a/sw/airborne/autopilot/test/check_uart.c b/sw/airborne/autopilot/test/check_uart.c new file mode 100644 index 0000000000..af9229f4ef --- /dev/null +++ b/sw/airborne/autopilot/test/check_uart.c @@ -0,0 +1,51 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include +#include "timer.h" +#include "uart.h" + +int main( void ) { + uart0_init(); + uart0_print_string("Booting AP MCU: $Id$\n"); + timer_init(); + sei(); + uint8_t _1Hz = 0; + uint8_t foo = 0; + while( 1 ) { + if(timer_periodic()) { + _1Hz++; + if (_1Hz == 60) { + _1Hz = 0; + foo++; + uart0_print_string("AP MCU uart0 check : alive ["); + uart0_print_hex(foo); + uart0_print_string("]\n"); + } + } + } + return 0; +} diff --git a/sw/airborne/autopilot/test/test_modem.c b/sw/airborne/autopilot/test/test_modem.c new file mode 100644 index 0000000000..f764fa075b --- /dev/null +++ b/sw/airborne/autopilot/test/test_modem.c @@ -0,0 +1,50 @@ +/* + * Paparazzi $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. + * + */ + + +#include +#include +#include + +#include "modem.h" +#include "timer.h" + +const uint8_t *msg = "ap modem alive\n"; + +int main( void ) { + timer_init(); + modem_init(); + sei(); + while (1) { + if (timer_periodic()) { + uint8_t i = 0; + while (msg[i]) { + MODEM_PUT_1_BYTE(msg[i]); + i++; + } + MODEM_CHECK_RUNNING(); + } + } + return 0; +} diff --git a/sw/airborne/autopilot/test/test_v2xe.c b/sw/airborne/autopilot/test/test_v2xe.c new file mode 100644 index 0000000000..e58bd73621 --- /dev/null +++ b/sw/airborne/autopilot/test/test_v2xe.c @@ -0,0 +1,277 @@ +/* + * Paparazzi $Id$ + * + * Copyright (C) 2004 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. + * + */ + + +#include +#include +#include + +#include "spi.h" +#include "timer.h" + +void uart_send(uint8_t c); + +volatile uint8_t spi_cur_slave = SPI_NONE; + +void my_spi_init(void) { + /* Set MOSI and SCK output, all others input */ + SPI_DDR = _BV(SPI_MOSI_PIN)| _BV(SPI_SCK_PIN); + + /* enable pull up for miso */ + // SPI_PORT |= _BV(SPI_MISO_PIN); + + /* Set SS0 output */ + sbi( SPI_SS0_DDR, SPI_SS0_PIN); + /* SS0 idles high (don't select slave yet)*/ + SPI_UNSELECT_SLAVE0(); + + /* Set SS1 output */ + sbi( SPI_SS1_DDR, SPI_SS1_PIN); + /* SS1 idles high (don't select slave yet)*/ + SPI_UNSELECT_SLAVE1(); + +} + + +void spi_start( void ) { + uint8_t foo; + /* Enable SPI, Master, MSB first, clock idle low, sample on leading edge, clock rate fck/128 */ + SPCR = (_BV(SPE)| _BV(MSTR) | _BV(SPR1) | _BV(SPR0)); + if (bit_is_set(SPSR, SPIF)) + foo = SPDR; + SPI_SELECT_SLAVE1(); +} + +uint8_t spi_transmit(uint8_t c) { + uint8_t foo; + SPDR = c; + while (bit_is_clear(SPSR, SPIF)); + foo = inp(SPDR); + + uart_send(c); + uart_send(foo); + return foo; +} + +void spi_stop(void) { + SPI_UNSELECT_SLAVE1(); + SPI_STOP(); +} + + +inline void delay_long(uint8_t n) { + uint8_t ctx; + while (n > 0) { + ctx=0; + do {ctx++;} while (ctx); + n--; + } +} + +#define SYNC_FLAG 0xAA +#define TERMINATOR 0x00 + +#define GET_MODE_INFO 0x01 +#define MOD_INFO_RESP 0x02 +#define SET_DATA_COMPONENTS 0x03 +#define GET_DATA 0x04 +#define DATA_RESP 0x05 +#define SET_CONFIG 0x06 +#define GET_CONFIG 0x07 +#define CONFIG_RESP 0x08 +#define SAVE_CONFIG 0x09 +#define START_CAL 0x0A +#define STOP_CAL 0x0B +#define GET_CAL_DATA 0x0C +#define CAL_DATA_RESP 0x0D +#define SET_CAL_DATA 0x0E + +#define DATA_XRAW 0x01 // Slnt32 counts 32768 to 32767 +#define DATA_YRAW 0x02 // Slnt32 counts 32768 to 32767 +#define DATA_XCAL 0x03 // Float32 scaled to 1.0 +#define DATA_YCAL 0x04 // Float32 scaled to 1.0 +#define DATA_HEADING 0x05 // Float32 degrees 0.0 ° to 359.9 ° +#define DATA_MAGNITUDE 0x06 // Float32 scaled to 1.0 +#define DATA_TEMPERATURE 0x07 // Float32 ° Celsius +#define DATA_DISTORTION 0x08 // Boolean +#define DATA_CAL_STATUS 0x09 // Boolean + +uint8_t err_cnt; +uint8_t errno; + +#define DATA_FIELD_NB 3 +#define DATA_LEN 12 +void v2xe_setup_data_components(void) { + uint8_t c; + spi_start(); + c = spi_transmit(SYNC_FLAG); + c = spi_transmit(SET_DATA_COMPONENTS); + c = spi_transmit(DATA_FIELD_NB); + c = spi_transmit(DATA_XRAW); + c = spi_transmit(DATA_YRAW); + c = spi_transmit(DATA_TEMPERATURE); + c = spi_transmit(TERMINATOR); + spi_stop(); + uart_send(0xFF); +} + +void v2xe_read_data(void) { + uint8_t c, i=0; + spi_start(); + + /* querry */ + c = spi_transmit(SYNC_FLAG); + c = spi_transmit(GET_DATA); + c = spi_transmit(TERMINATOR); + + delay_long(255); + + /* answer */ + do { c = spi_transmit(0x00); i++;} + while (c!=SYNC_FLAG && i < 20); + // if (i>20) TIMEOUT; + /* frame type */ + c = spi_transmit(0x00); + /* nb fields */ + c = spi_transmit(0x00); + /* fields + data */ + for (i=0; i < DATA_LEN + DATA_FIELD_NB + 1; i++) { + c = spi_transmit(0x00); + } + + spi_stop(); + uart_send(0xFF); +} + +#define ID_LEN 8 +uint8_t id_str[ID_LEN]; +void v2xe_read_id (void) { + uint8_t c, i=0; + + spi_start(); + c = spi_transmit(SYNC_FLAG); + c = spi_transmit(GET_MODE_INFO); + c = spi_transmit(TERMINATOR); + + // delay_long(10); + + // c = spi_transmit(0x00); + + + do { c = spi_transmit(0x00); i++;} + while (c!=SYNC_FLAG && i < 20); + + /* if (c != SYNC_FLAG) { */ +/* err_cnt++; */ +/* errno = 1; */ +/* spi_stop(); */ +/* return; */ +/* } */ +// c = spi_transmit(0x00); +/* if (c != MOD_INFO_RESP) { */ +/* err_cnt++; */ +/* errno = 2; */ +/* spi_stop(); */ +/* return; */ +/* } */ + for (c = 0; c < ID_LEN; c++) + id_str[c] = spi_transmit(0x00); + c = spi_transmit(0x00); +/* if (c != TERMINATOR) { */ +/* err_cnt++; */ +/* errno = 3; */ +/* spi_stop(); */ +/* return; */ +/* } */ + + spi_stop(); + + uart_send(0xFF); +} + + +#define UBRRH UBRR0H +#define UBRRL UBRR0L +#define UCSRA UCSR0A +#define UCSRB UCSR0B +#define UCSRC UCSR0C +#define UDR UDR0 + +void uart_init(void) { + /* Baudrate is 38.4k */ + UBRRH = 0; + UBRRL = 25; + /* single speed */ + UCSRA = 0; + /* Enable receiver and transmitter */ + UCSRB = _BV(RXEN) | _BV(TXEN); + /* Set frame format: 8data, 1stop bit */ + UCSRC = _BV(UCSZ1) | _BV(UCSZ0); + +} + +void uart_send(uint8_t c) { + /* Wait for empty transmit buffer */ + while ( !( UCSRA & _BV(UDRE)) ) ; + /* Put data into buffer, sends the data */ + UDR = c; +} + + +inline void periodic_task(void) { + static uint8_t foo; + if (foo == 0) + v2xe_read_id(); + if (foo == 10) + v2xe_setup_data_components(); + if (foo > 10 && !(foo%10)) + v2xe_read_data(); + // uart_send(err_cnt); + // uart_send (errno); + // { + // uint8_t i; + // for (i=0; i<8; i++) + // uart_send(id_str[i]); + // } + // uart_send('\n'); + foo++; + if (foo > 60) foo=0; +} + +int main( void ) { + + timer_init(); + my_spi_init(); + uart_init(); + // sei(); + + delay_long(255); + delay_long(255); + + while (1) { + if (timer_periodic()) + periodic_task(); + } + return 0; +} diff --git a/sw/airborne/autopilot/test/tx_adcs.c b/sw/airborne/autopilot/test/tx_adcs.c new file mode 100644 index 0000000000..1a537d16f2 --- /dev/null +++ b/sw/airborne/autopilot/test/tx_adcs.c @@ -0,0 +1,66 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include +#include "timer.h" +#include "uart.h" +#include "adc.h" + +static struct adc_buf buffers[NB_ADC]; + +void transmit_adc(void) { + uint8_t i; + uart0_transmit((uint8_t)0); uart0_transmit((uint8_t)0); + for(i = 0; i < NB_ADC; i++) { + uint16_t value = buffers[i].sum / AV_NB_SAMPLE; + uart0_transmit((uint8_t)(value >> 8)); + uart0_transmit((uint8_t)(value & 0xff)); + } + uart0_transmit((uint8_t)'\n'); +} + +int main( void ) { + uint8_t i; + uart0_init(); + timer_init(); + adc_init(); + for(i = 0; i < NB_ADC; i++) + adc_buf_channel(i, &buffers[i]); + sei(); + + while( 1 ) { + static uint8_t _1Hz = 0; + + if(timer_periodic()) { + _1Hz++; + if (_1Hz == 60) { + _1Hz = 0; + transmit_adc(); + } + } + } + return 0; +} diff --git a/sw/airborne/autopilot/test/uart_tunnel.c b/sw/airborne/autopilot/test/uart_tunnel.c new file mode 100644 index 0000000000..2bb79257a8 --- /dev/null +++ b/sw/airborne/autopilot/test/uart_tunnel.c @@ -0,0 +1,29 @@ +#include +#include +#include +#include +#include + + +#include "../uart.h" +#include "../timer.h" + +void on_uart0_rx(uint8_t c) { + uart1_transmit(c); +} + +void on_uart1_rx(uint8_t c) { + uart0_transmit(c); +} + +ReceiveUart0(on_uart0_rx) +ReceiveUart1(on_uart1_rx) + +int main( void ) { + uart0_init(); + uart1_init(); + + sei(); + return 0; + +} diff --git a/sw/airborne/autopilot/timer.h b/sw/airborne/autopilot/timer.h new file mode 100644 index 0000000000..7ec726d3b3 --- /dev/null +++ b/sw/airborne/autopilot/timer.h @@ -0,0 +1,91 @@ +/* + * Paparazzi mcu0 timer functions + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * Copyright (C) 2002 Trammell Hudson + * 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. + * + */ + +#ifndef TIMER_H +#define TIMER_H + +#include "std.h" +#include +#include + + +/* + * Enable Timer1 (16-bit) running at Clk/1 for the global system + * clock. This will be used for computing the servo pulse widths, + * PPM decoding, etc. + * + * Low frequency periodic tasks will be signaled by timer 0 + * running at Clk/1024. For 16 Mhz clock, this will be every + * 262144 microseconds, or 61 Hz. + */ +static inline void timer_init( void ) { + + /* Timer0: Modem clock is started in modem.h in ctc mode*/ + + /* Timer1 @ Clk/1: System clock, ppm and servos */ + TCCR1A = 0x00; + TCCR1B = 0x01; + + /* Timer2 @ Clk/1024: Periodic clock */ + TCCR2 = 0x05; +} + + +/* + * Retrieve the current time from the global clock in Timer1, + * disabling interrupts to avoid stomping on the TEMP register. + * If interrupts are already off, the non_atomic form can be used. + */ +static inline uint16_t +timer_now( void ) +{ + return TCNT1; +} + +static inline uint16_t +timer_now_non_atomic( void ) +{ + return TCNT1L; +} + + +/* + * Periodic tasks occur when Timer2 overflows. Check and unset + * the overflow bit. We cycle through four possible periodic states, + * so each state occurs every 30 Hz. + */ +static inline bool_t +timer_periodic( void ) +{ + if( !bit_is_set( TIFR, TOV2 ) ) + return FALSE; + + TIFR = 1 << TOV2; + return TRUE; +} + +#endif diff --git a/sw/airborne/autopilot/uart.c b/sw/airborne/autopilot/uart.c new file mode 100644 index 0000000000..c42ecf00f0 --- /dev/null +++ b/sw/airborne/autopilot/uart.c @@ -0,0 +1,138 @@ +/* + * Paparazzi $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. + * + */ +#include +#include +#include +#include "uart.h" + +#define TX_BUF_SIZE 256 +static uint8_t tx_head0; /* next free in buf */ +static volatile uint8_t tx_tail0; /* next char to send */ +static uint8_t tx_buf0[ TX_BUF_SIZE ]; + +static uint8_t tx_head1; /* next free in buf */ +static volatile uint8_t tx_tail1; /* next char to send */ +static uint8_t tx_buf1[ TX_BUF_SIZE ]; + +void uart0_transmit( unsigned char data ) { + if (UCSR0B & _BV(TXCIE)) { + /* we are waiting for the last char to be sent : buffering */ + if (tx_tail0 == tx_head0 + 1) { /* BUF_SIZE = 256 */ + /* Buffer is full (almost, but tx_head = tx_tail means "empty" */ + return; + } + tx_buf0[tx_head0] = data; + tx_head0++; /* BUF_SIZE = 256 */ + } else { /* Channel is free: just send */ + UDR0 = data; + sbi(UCSR0B, TXCIE); + } +} + +void uart1_transmit( unsigned char data ) { + if (UCSR1B & _BV(TXCIE)) { + /* we are waiting for the last char to be sent : buffering */ + if (tx_tail1 == tx_head1 + 1) { /* BUF_SIZE = 256 */ + /* Buffer is full (almost, but tx_head = tx_tail means "empty" */ + return; + } + tx_buf1[tx_head1] = data; + tx_head1++; /* BUF_SIZE = 256 */ + } else { /* Channel is free: just send */ + UDR1 = data; + sbi(UCSR1B, TXCIE); + } +} + + +void uart0_print_string(const uint8_t* s) { + uint8_t i = 0; + while (s[i]) { + uart0_transmit(s[i]); + i++; + } +} + +void uart0_print_hex(const uint8_t c) { + const uint8_t hex[16] = { '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; + uint8_t high = (c & 0xF0)>>4; + uint8_t low = c & 0x0F; + uart0_transmit(hex[high]); + uart0_transmit(hex[low]); +} + + +SIGNAL(SIG_UART0_TRANS) { + if (tx_head0 == tx_tail0) { + /* Nothing more to send */ + cbi(UCSR0B, TXCIE); /* disable interrupt */ + } else { + UDR0 = tx_buf0[tx_tail0]; + tx_tail0++; /* warning tx_buf_len is 256 */ + } +} + +SIGNAL(SIG_UART1_TRANS) { + if (tx_head1 == tx_tail1) { + /* Nothing more to send */ + cbi(UCSR1B, TXCIE); /* disable interrupt */ + } else { + UDR1 = tx_buf1[tx_tail1]; + tx_tail1++; /* warning tx_buf_len is 256 */ + } +} + +void uart0_init( void ) { + /* Baudrate is 38.4k */ + UBRR0H = 0; + UBRR0L = 25; // 38.4 + // UBRR0L = 103; //9600 + /* single speed */ + UCSR0A = 0; + /* Enable receiver and transmitter */ + UCSR0B = _BV(RXEN) | _BV(TXEN); + /* Set frame format: 8data, 1stop bit */ + UCSR0C = _BV(UCSZ1) | _BV(UCSZ0); + /* Enable uart receive interrupt */ + sbi(UCSR0B, RXCIE ); +} + +void uart1_init( void ) { + /* Baudrate is 38.4k */ + UBRR1H = 0; + UBRR1L = 25; // 38.4 + // UBRR1L = 103; //9600 + + + /* single speed */ + UCSR1A = 0; + /* Enable receiver and transmitter */ + UCSR1B = _BV(RXEN) | _BV(TXEN); + /* Set frame format: 8data, 1stop bit */ + UCSR1C = _BV(UCSZ1) | _BV(UCSZ0); + /* Enable uart receive interrupt */ + sbi(UCSR1B, RXCIE ); +} + diff --git a/sw/airborne/autopilot/uart.h b/sw/airborne/autopilot/uart.h new file mode 100644 index 0000000000..29e892915f --- /dev/null +++ b/sw/airborne/autopilot/uart.h @@ -0,0 +1,48 @@ +/* + * Paparazzi $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. + * + */ +#ifndef _UART_H_ +#define _UART_H_ + +#include + +extern void uart0_init(void); +extern void uart1_init(void); + +extern void uart0_print_string(const uint8_t*); +extern void uart0_print_hex(const uint8_t); +extern void uart0_transmit(const uint8_t); +extern void uart1_transmit(const uint8_t); + +#define ReceiveUart0(cb) \ + SIGNAL( SIG_UART0_RECV ) { \ + uint8_t c = inp(UDR0); \ + cb(c); \ +} +#define ReceiveUart1(cb) \ + SIGNAL( SIG_UART1_RECV ) { \ + uint8_t c = inp(UDR1); \ + cb(c); \ +} + +#endif diff --git a/sw/airborne/autopilot/ubx.h b/sw/airborne/autopilot/ubx.h new file mode 100644 index 0000000000..4b618ae4e3 --- /dev/null +++ b/sw/airborne/autopilot/ubx.h @@ -0,0 +1,38 @@ +/* + * Paparazzi autopilot $Id$ + * + * Copyright (C) 2004 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. + * + */ + +/* + * UBX protocol specific code + * +*/ + + +#ifndef UBX_H +#define UBX_H + +#define GPS_FIX_VALID(gps_mode) (gps_mode == 3) +extern const int32_t utm_east0; +extern const int32_t utm_north0; + +#endif /* UBX_H */ diff --git a/sw/airborne/fly_by_wire/Makefile b/sw/airborne/fly_by_wire/Makefile new file mode 100644 index 0000000000..e1072929f3 --- /dev/null +++ b/sw/airborne/fly_by_wire/Makefile @@ -0,0 +1,77 @@ +# +# $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. +# + +LOCAL_CFLAGS= $(CTL_BRD_FLAGS) + +ARCH = atmega8 +TARGET = fbw +#LOW_FUSE = 3f # crystal # +#LOW_FUSE = 31 # internal 1MHz # +#LOW_FUSE = 1e # ceramic resonator slow rising power p26 # +LOW_FUSE = 2e # ceramic resonator slow rising power p26 # +HIGH_FUSE = cb +EXT_FUSE = ff +LOCK_FUSE = ff +VARINCLUDE = $(PAPARAZZI_HOME)/var/include +ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) +INCLUDES = -I ../../include -I $(VARINCLUDE) -I $(ACINCLUDE) + +ifeq ($(CTL_BRD_VERSION),V1_2_1) +CTL_BRD_FLAGS=-DCTL_BRD_V1_2_1 +endif + +ifeq ($(CTL_BRD_VERSION),V1_2) +CTL_BRD_FLAGS=-DCTL_BRD_V1_2 +endif + +ifeq ($(CTL_BRD_VERSION),V1_1) +CTL_BRD_FLAGS=-DCTL_BRD_V1_1 +endif + + +$(TARGET).srcs = \ + main.c \ + ppm.c \ + servo.c \ + spi.c \ + uart.c \ + adc_fbw.c \ + + +include ../../../conf/Makefile.local +include ../../../conf/Makefile.avr + +fbw.install : warn_conf + +warn_conf : + @echo + @echo '###########################################################' + @grep AIRFRAME_NAME $(ACINCLUDE)/airframe.h + @grep RADIO_NAME $(ACINCLUDE)/radio.h + @echo '###########################################################' + @echo + + +main.o .depend : $(ACINCLUDE)/radio.h $(ACINCLUDE)/airframe.h + +clean : avr_clean + diff --git a/sw/airborne/fly_by_wire/README b/sw/airborne/fly_by_wire/README new file mode 100644 index 0000000000..d56db63899 --- /dev/null +++ b/sw/airborne/fly_by_wire/README @@ -0,0 +1,24 @@ +# $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. +# + + + + diff --git a/sw/airborne/fly_by_wire/adc_fbw.c b/sw/airborne/fly_by_wire/adc_fbw.c new file mode 100644 index 0000000000..911ac705d4 --- /dev/null +++ b/sw/airborne/fly_by_wire/adc_fbw.c @@ -0,0 +1,120 @@ +/* + * Paparazzi fly by wire adc functions + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * Copyright (C) 2002 Trammell Hudson + * 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. + * + */ + +//// ADC3 MVSUP +//// ADC6 MVSERVO + + +#include +#include +#include +#include "adc_fbw.h" + + + +#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1 + +#define VOLTAGE_TIME 0x07 +#define ANALOG_PORT PORTC +#define ANALOG_PORT_DIR DDRC + + +// +#define ANALOG_VREF _BV(REFS0) | _BV(REFS1) + +uint16_t adc_samples[ NB_ADC ]; + +static struct adc_buf* buffers[NB_ADC]; + +void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s) { + buffers[adc_channel] = s; +} + +void +adc_init( void ) +{ + uint8_t i; + /* Ensure that our port is for input with no pull-ups */ + ANALOG_PORT = 0x00; + ANALOG_PORT_DIR = 0x00; + + /* Select our external voltage ref */ + ADMUX = ANALOG_VREF; + + /* Select out clock, turn on the ADC interrupt and start conversion */ + ADCSRA = 0 + | VOLTAGE_TIME + | _BV(ADEN ) + | _BV(ADIE ) + | _BV(ADSC ); + + /* Init to 0 (usefull ?) */ + for(i = 0; i < NB_ADC; i++) + buffers[i] = (struct adc_buf*)0; +} + +/** + * Called when the voltage conversion is finished + * + * 8.913kHz on mega128@16MHz 1kHz/channel ?? +*/ + + +SIGNAL( SIG_ADC ) +{ + uint8_t adc_input = ADMUX & 0x7; + struct adc_buf* buf = buffers[adc_input]; + uint16_t adc_value = ADCW; + /* Store result */ + adc_samples[ adc_input ] = adc_value; + + if (buf) { + uint8_t new_head = buf->head + 1; + if (new_head >= AV_NB_SAMPLE) new_head = 0; + buf->sum -= buf->values[new_head]; + buf->values[new_head] = adc_value; + buf->sum += adc_value; + buf->head = new_head; + } + + /* Find the next input */ + adc_input++; + if (adc_input == 4) + adc_input = 6; // ADC 4 and 5 for i2c + if( adc_input >= 8 ) { + adc_input = 0; +#ifdef CTL_BRD_V1_2 + adc_input = 1; // WARNING ADC0 is for rservo driver reset on v1.2.0 +#endif /* CTL_BRD_V1_2 */ + } + /* Select it */ + ADMUX = adc_input | ANALOG_VREF; + /* Restart the conversion */ + sbi( ADCSR, ADSC ); +} + +#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2_1 */ diff --git a/sw/airborne/fly_by_wire/adc_fbw.h b/sw/airborne/fly_by_wire/adc_fbw.h new file mode 100644 index 0000000000..6dd77eb335 --- /dev/null +++ b/sw/airborne/fly_by_wire/adc_fbw.h @@ -0,0 +1,61 @@ +/* + * Paparazzi fly by wire adc functions + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * Copyright (C) 2002 Trammell Hudson + * 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. + * + */ + +#ifndef _ADC_H_ +#define _ADC_H_ + +#include "airframe.h" + +#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1 + +#include + + +#define NB_ADC 8 + +/* Array containing the last measured value */ +extern uint16_t adc_samples[ NB_ADC ]; + +void adc_init( void ); + +#define AV_NB_SAMPLE 0x20 + +struct adc_buf { + uint16_t sum; + uint16_t values[AV_NB_SAMPLE]; + uint8_t head; +}; + +/* Facility to store last values in a circular buffer for a specific + channel: allocate a (struct adc_buf) and register it with the following + function */ +void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s); + + +#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2 */ + +#endif /* _ADC_H_ */ diff --git a/sw/airborne/fly_by_wire/link_autopilot.h b/sw/airborne/fly_by_wire/link_autopilot.h new file mode 100644 index 0000000000..09edd010f4 --- /dev/null +++ b/sw/airborne/fly_by_wire/link_autopilot.h @@ -0,0 +1,65 @@ +/* $Id$ + * + * (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. + * + */ + +#ifndef LINK_AUTOPILOT_H +#define LINK_AUTOPILOT_H + +#include + +#include "std.h" +#include "radio.h" +#include "airframe.h" + +/* + * System clock in MHz. + */ +#define CLOCK 16 + +typedef int16_t pprz_t; // type of commands + +/* !!!!!!!!!!!!!!!!!!! Value used in gen_airframe.ml !!!!!!!!!!!!!!!!! */ +#define MAX_PPRZ (600 * CLOCK) +#define MIN_PPRZ -MAX_PPRZ + +struct inter_mcu_msg { + int16_t channels[RADIO_CTL_NB]; + uint8_t ppm_cpt; + uint8_t status; + uint8_t nb_err; + uint8_t vsupply; /* 1e-1 V */ +}; + +// Status bits from FBW to AUTOPILOT +#define STATUS_RADIO_OK 0 +#define RADIO_REALLY_LOST 1 +#define AVERAGED_CHANNELS_SENT 2 +#define MASK_FBW_CHANGED 0x3 + +// Statut bits from AUTOPILOT to FBW +#define STATUS_AUTO_OK 0 + +#define FRAME_LENGTH (sizeof(struct inter_mcu_msg)+1) + +#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2) + +#endif // LINK_AUTOPILOT_H diff --git a/sw/airborne/fly_by_wire/main.c b/sw/airborne/fly_by_wire/main.c new file mode 100644 index 0000000000..5bfe6d6ad7 --- /dev/null +++ b/sw/airborne/fly_by_wire/main.c @@ -0,0 +1,182 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include +#include + +#include "timer.h" +#include "servo.h" +#include "ppm.h" +#include "spi.h" +#include "link_autopilot.h" +#include "radio.h" + + +#include "uart.h" + + +#ifndef CTL_BRD_V1_1 +#include "adc_fbw.h" +struct adc_buf vsupply_adc_buf; +struct adc_buf vservos_adc_buf; +#endif + +uint8_t mode; +static uint8_t time_since_last_mega128; +static uint16_t time_since_last_ppm; +bool_t radio_ok, mega128_ok, radio_really_lost; + +static const pprz_t failsafe[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; + +static uint8_t ppm_cpt, last_ppm_cpt; + +#define STALLED_TIME 30 // 500ms with a 60Hz timer +#define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer + + +/* static inline void status_transmit( void ) { */ +/* uint8_t i; */ +/* uart_transmit(7); */ +/* uart_transmit(7); */ +/* for (i=0; i>8); */ +/* uart_transmit(ppm_pulses[i] & 0xff); */ +/* } */ +/* uart_transmit('\n'); */ +/* } */ + + +/* Prepare data to be sent to mcu0 */ +static inline void to_autopilot_from_last_radio (void) { + uint8_t i; + for(i = 0; i < RADIO_CTL_NB; i++) + to_mega128.channels[i] = last_radio[i]; + to_mega128.status = (radio_ok ? _BV(STATUS_RADIO_OK) : 0); + to_mega128.status |= (radio_really_lost ? _BV(RADIO_REALLY_LOST) : 0); + if (last_radio_contains_avg_channels) { + to_mega128.status |= _BV(AVERAGED_CHANNELS_SENT); + last_radio_contains_avg_channels = FALSE; + } + to_mega128.ppm_cpt = last_ppm_cpt; +#ifndef CTL_BRD_V1_1 + to_mega128.vsupply = VoltageOfAdc(vsupply_adc_buf.sum/AV_NB_SAMPLE) * 10; +#else + to_mega128.vsupply = 0; +#endif +} + + +int main( void ) +{ + uart_init_tx(); + uart_print_string("FBW Booting $Id$\n"); + +#ifndef CTL_BRD_V1_1 + adc_init(); + adc_buf_channel(3, &vsupply_adc_buf); + adc_buf_channel(6, &vservos_adc_buf); +#endif + timer_init(); + servo_init(); + ppm_init(); + spi_init(); + sei(); + while( 1 ) { + if( ppm_valid ) { + ppm_valid = FALSE; + ppm_cpt++; + radio_ok = TRUE; + radio_really_lost = FALSE; + time_since_last_ppm = 0; + last_radio_from_ppm(); + if (last_radio_contains_avg_channels) { + mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]); + } + if (mode == MODE_MANUAL) { + servo_set(last_radio); + } + } else if (mode == MODE_MANUAL && radio_really_lost) { + mode = MODE_AUTO; + } + + if ( !SpiIsSelected() && spi_was_interrupted ) { + spi_was_interrupted = FALSE; + if (mega128_receive_valid) { + time_since_last_mega128 = 0; + mega128_ok = TRUE; + if (mode == MODE_AUTO) + servo_set(from_mega128.channels); + } + to_autopilot_from_last_radio(); + spi_reset(); + } + + if (time_since_last_ppm >= STALLED_TIME) { + radio_ok = FALSE; + } + if (time_since_last_ppm >= REALLY_STALLED_TIME) { + radio_really_lost = TRUE; + } + if (time_since_last_mega128 == STALLED_TIME) { + mega128_ok = FALSE; + } + + if ((mode == MODE_MANUAL && !radio_ok) || + (mode == MODE_AUTO && !mega128_ok)) { + servo_set(failsafe); + } + + if(timer_periodic()) { + static uint8_t _1Hz; + static uint8_t _20Hz; + _1Hz++; + _20Hz++; + if (_1Hz >= 60) { + _1Hz = 0; + last_ppm_cpt = ppm_cpt; + ppm_cpt = 0; + } + if (_20Hz >= 3) { + _20Hz = 0; + servo_transmit(); + // status_transmit(); + } + if (time_since_last_mega128 < STALLED_TIME) + time_since_last_mega128++; + if (time_since_last_ppm < REALLY_STALLED_TIME) + time_since_last_ppm++; + } + } + return 0; +} diff --git a/sw/airborne/fly_by_wire/ppm.c b/sw/airborne/fly_by_wire/ppm.c new file mode 100644 index 0000000000..adeae0fbd7 --- /dev/null +++ b/sw/airborne/fly_by_wire/ppm.c @@ -0,0 +1,135 @@ +/* $Id$ + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * (c) 2003 Trammell Hudson + * (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. + * + */ + +#include "radio.h" +#include "ppm.h" + +#define AVERAGING_PERIOD (PPM_FREQ/4) + +/* + * Pulse width is computed as the difference between now and the + * previous pulse. If no pulse has been received between then and + * now, the time of the last pulse will be equal to the last pulse + * we measured. Unfortunately, the Input Capture Flag (ICF1) will + * not be set since the interrupt routine disables it. + * + * Sync pulses are timed with Timer2, which runs at Clk/1024. This + * is slow enough at both 4 and 8 Mhz to measure the lengthy (10ms + * or longer) pulse. + * + * Otherwise, compute the pulse width with the 16-bit timer1, + * push the pulse width onto the stack and increment the + * pulse counter until we have received eight pulses. + */ + +uint16_t ppm_pulses[ PPM_NB_PULSES ]; +pprz_t last_radio[ PPM_NB_PULSES ]; +pprz_t avg_last_radio[ PPM_NB_PULSES ]; +bool_t last_radio_contains_avg_channels = FALSE; +volatile bool_t ppm_valid; + +/* MC3030, Trame PPM7: 25ms, 10.4 au neutre, + sync pulse = 16.2ms with low value on every channels */ + + + + +#define RestartPpmCycle() { state = 0; sync_start = TCNT2; return; } + +SIGNAL( SIG_INPUT_CAPTURE1 ) +{ + static uint16_t last; + uint16_t this; + uint16_t width; + static uint8_t state; + static uint8_t sync_start; + + this = ICR1; + width = this - last; + last = this; + + if( state == 0 ) { + uint8_t end = inp( TCNT2 ); + uint8_t diff = (end - sync_start); + sync_start = end; + + /* The frame period of the mc3030 seems to be 25ms. + * One pulse lasts from 1.05ms to 2.150ms. + * Sync pulse is at least 7ms : (7000*CLOCK)/1024 = 109 + */ + if( diff > (uint8_t)(((uint32_t)(7000ul*CLOCK))/1024ul) ) { + state = 1; + } + } + else { + /* Read a data pulses */ + if( width < 700ul*CLOCK || width > 2300ul*CLOCK) + RestartPpmCycle(); + ppm_pulses[state - 1] = width; + + if (state >= PPM_NB_PULSES) { + ppm_valid = 1; + RestartPpmCycle(); + } else + state++; + } + return; +} + +#define Int16FromPulse(i) (int16_t)((ppm_pulses[(i)] - PpmOfUs(((int[])RADIO_NEUTRALS_US)[i]))*(2*MAX_PPRZ)/(PpmOfUs(((int[])RADIO_MAXS_US[i])-((int[])RADIO_MINS_US[i])))) + + +/* Copy from the ppm receiving buffer to the buffer sent to mcu0 */ +void last_radio_from_ppm() { + static uint8_t avg_cpt = 0; /* Counter for averaging */ + uint8_t i; + + for(i = 0; i < RADIO_CTL_NB; i++) { + int16_t pprz = Int16FromPulse(i); + if (pprz > MAX_PPRZ) + pprz = MAX_PPRZ; + else if (pprz < MIN_PPRZ) + pprz = MIN_PPRZ; + + if (i == RADIO_THROTTLE) { + int16_t gaz = pprz/2; + last_radio[i] = (gaz < 0 ? 0 : gaz); + } else if (AveragedChannel(i)) { + avg_last_radio[i] += pprz / AVERAGING_PERIOD; + } else + last_radio[i] = pprz; + } + + avg_cpt++; + if (avg_cpt == AVERAGING_PERIOD) { + avg_cpt = 0; + for(i = 0; i < RADIO_CTL_NB; i++) + if (AveragedChannel(i)) { + last_radio[i] = avg_last_radio[i]; + avg_last_radio[i] = 0; + } + last_radio_contains_avg_channels = TRUE; + } +} diff --git a/sw/airborne/fly_by_wire/ppm.h b/sw/airborne/fly_by_wire/ppm.h new file mode 100644 index 0000000000..fab02455e1 --- /dev/null +++ b/sw/airborne/fly_by_wire/ppm.h @@ -0,0 +1,102 @@ +/* $Id$ + * + * Decoder for the trainer ports or hacked receivers for both + * Futaba and JR formats. The ppm_valid flag is set whenever + * a valid frame is received. + * + * Pulse widths are stored as unscaled 16-bit values in ppm_pulses[]. + * If you require actual microsecond values, divide by CLOCK. + * For an 8 Mhz clock and typical servo values, these will range + * from 0x1F00 to 0x4000. + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * (c) 2002 Trammell Hudson + * (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. + * + */ + +#ifndef PPM_H +#define PPM_H + + +/** + * Receiver types + */ +#define RXFUTABA 0 +#define RXJR 1 + +#define PPM_RX_TYPE RXFUTABA +#define PPM_FREQ 40 // 25ms + +#include +#include + +#include "timer.h" +#include "link_autopilot.h" + +#define PpmOfUs(x) ((x)*CLOCK) + +#define PPM_DDR DDRB +#define PPM_PORT PORTB +#define PPM_PIN PB0 + +/* + * PPM pulses are falling edge clocked on the ICP, which records + * the state of the global clock. We do not use any noise + * canceling features. + * + * JR might be rising edge clocked; set that as an option + */ +static inline void +ppm_init( void ) +{ +#if PPM_RX_TYPE == RXFUTABA + cbi( TCCR1B, ICES1 ); +#elif PPM_RX_TYPE == RXJR + sbi( TCCR1B, ICES1 ); +#else +# error "ppm.h: Unknown receiver type in PPM_RX_TYPE" +#endif + + /* No noise cancelation */ + sbi( TCCR1B, ICNC1 ); + + /* Set ICP to input, no internal pull up */ + cbi( PPM_DDR, PPM_PIN); + + /* Enable interrupt on input capture */ + sbi( TIMSK, TICIE1 ); +} + +#define PPM_NB_PULSES RADIO_CTL_NB + +extern volatile bool_t ppm_valid; +extern pprz_t last_radio[PPM_NB_PULSES]; +extern bool_t last_radio_contains_avg_channels; + + +#define MODE_MANUAL 0 +#define MODE_AUTO 1 + +#define MODE_OF_PPRZ(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? MODE_MANUAL : MODE_AUTO) + +extern void last_radio_from_ppm(void); +#endif diff --git a/sw/airborne/fly_by_wire/servo.c b/sw/airborne/fly_by_wire/servo.c new file mode 100644 index 0000000000..ae7e525375 --- /dev/null +++ b/sw/airborne/fly_by_wire/servo.c @@ -0,0 +1,193 @@ +/* $Id$ + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * (c) 2002 Trammell Hudson + * (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. + * + */ + + +#include +#include +#include "servo.h" +#include "link_autopilot.h" + +#include "airframe.h" + +#include "uart.h" + + +/* + * Paparazzi boards have one 4017 servo driver. + * It is driven by OCR1A (PB1) with reset on PORTD5. + */ +#define _4017_NB_CHANNELS 10 + +#ifdef CTL_BRD_V1_1 +#define _4017_RESET_PORT PORTD +#define _4017_RESET_DDR DDRD +#define _4017_RESET_PIN 5 +#endif /* CTL_BRD_V1_1 */ + +#ifdef CTL_BRD_V1_2 +#define _4017_RESET_PORT PORTC +#define _4017_RESET_DDR DDRC +#define _4017_RESET_PIN 0 +#endif /* CTL_BRD_V1_2 */ + +#ifdef CTL_BRD_V1_2_1 +#define _4017_RESET_PORT PORTD +#define _4017_RESET_DDR DDRD +#define _4017_RESET_PIN 7 +#endif /* CTL_BRD_V1_2 */ + +#define _4017_CLOCK_PORT PORTB +#define _4017_CLOCK_DDR DDRB +#define _4017_CLOCK_PIN PB1 + +#define SERVO_OCR OCR1A +#define SERVO_ENABLE OCIE1A +#define SERVO_FLAG OCF1A +#define SERVO_FORCE FOC1A +#define SERVO_COM0 COM1A0 +#define SERVO_COM1 COM1A1 + +/* Following macro is required since the compiler does not solve at + compile-time indexation in a known array with a known index */ +#define SERVO_NEUTRAL_(i) SERVOS_NEUTRALS_ ## i +#define SERVO_NEUTRAL(i) (SERVO_NEUTRAL_(i)*CLOCK) + +#define SERVO_NEUTRAL_I(i) (((int[])SERVOS_NEUTRALS[i])*CLOCK) +#define SERVO_MIN_I(i) (((int[])SERVOS_MINS[i])*CLOCK) + +#define SERVO_MIN (SERVO_MIN_US*CLOCK) +#define SERVO_MAX (SERVO_MAX_US*CLOCK) +#define ChopServo(x) ((x) < SERVO_MIN ? SERVO_MIN : ((x) > SERVO_MAX ? SERVO_MAX : (x))) + +/* holds the servo pulses width in clock ticks */ +static uint16_t servo_widths[_4017_NB_CHANNELS]; + +/* + * We use the output compare registers to generate our servo pulses. + * These should be connected to a decade counter that routes the + * pulses to the appropriate servo. + * + * Initialization involves: + * + * - Reseting the decade counters + * - Writing the first pulse width to the counters + * - Setting output compare to set the clock line by calling servo_enable() + * - Bringing down the reset lines + * + * Ideally, you can use two decade counters to drive 20 servos. + */ +void +servo_init( void ) +{ + uint8_t i; + + /* Configure the reset and clock lines */ + _4017_RESET_DDR |= _BV(_4017_RESET_PIN); + _4017_CLOCK_DDR |= _BV(_4017_CLOCK_PIN); + + /* Reset the decade counter */ + sbi( _4017_RESET_PORT, _4017_RESET_PIN ); + + /* Lower the regular servo line */ + cbi( _4017_CLOCK_PORT, _4017_CLOCK_PIN ); + + /* Set all servos at their midpoints */ + for( i=0 ; i < _4017_NB_CHANNELS ; i++ ) + servo_widths[i] = SERVO_MIN; + + /* Set servos to go off some long time from now */ + SERVO_OCR = 32768ul; + + /* + * Configure output compare to toggle the output bits. + */ + TCCR1A |= _BV(SERVO_COM0 ); + + /* Clear the interrupt flags in case they are set */ + TIFR = _BV(SERVO_FLAG); + + /* Unassert the decade counter reset to start it running */ + cbi( _4017_RESET_PORT, _4017_RESET_PIN ); + + /* Enable our output compare interrupts */ + TIMSK |= _BV(SERVO_ENABLE ); +} + + +/* + * Interrupt routine + * + * write the next pulse width to OCR register and + * assert the servo signal. It will be cleared by + * the following compare match. + */ +SIGNAL( SIG_OUTPUT_COMPARE1A ) +{ + static uint8_t servo = 0; + uint16_t width; + + if (servo >= _4017_NB_CHANNELS) { + sbi( _4017_RESET_PORT, _4017_RESET_PIN ); + servo = 0; + // FIXME: 500 ns required by 4017 reset ???? why does it work without! + // asm( "nop; nop; nop; nop;nop; nop; nop; nop;nop; nop; nop; nop;nop; nop; nop; nop;" ); + cbi( _4017_RESET_PORT, _4017_RESET_PIN ); + } + + width = servo_widths[servo]; + + SERVO_OCR += width; + + TCCR1A |= _BV(SERVO_FORCE); + + servo++; +} + +void servo_set_one(uint8_t servo, uint16_t value_us) { + servo_widths[servo] = ChopServo(CLOCK*value_us); +} + +void +servo_transmit(void) { + uint8_t servo; + uart_transmit((uint8_t)0); uart_transmit((uint8_t)0); + + for(servo = 0; servo < _4017_NB_CHANNELS; servo++) { + uart_transmit((uint8_t)(servo_widths[servo] >> 8)); + uart_transmit((uint8_t)(servo_widths[servo] & 0xff)); + } + uart_transmit((uint8_t)'\n'); +} + + +/* + * + * defines how servos react to radio control or autopilot channels + * + */ + +void servo_set(const pprz_t values[]) { + ServoSet(values); /*Generated from airframe.xml */ +} diff --git a/sw/airborne/fly_by_wire/servo.h b/sw/airborne/fly_by_wire/servo.h new file mode 100644 index 0000000000..cd15fa2baa --- /dev/null +++ b/sw/airborne/fly_by_wire/servo.h @@ -0,0 +1,61 @@ +/* $Id$ + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * (c) 2002 Trammell Hudson + * (c) 2003 Pascal Brisset, Antoine Drouin + * + * This is the new decade counter based servo driving code. It uses + * one 16-bit output compare registers to determine when the regular + * servo clock line should be toggled, causing the output to move to the + * next servo. The other 16-bit output compare is used to drive a + * JR or Futaba compatible high-speed digital servo. + * + * User visibile routines: + * + * - servo_init(); + * + * Call once at the start of the program to bring the servos online + * and start the external decade counters. This will also start the + * high speed servo. + * + * - servo_make_pulse_width( length ); + * + * Converts a position value between 0 and 65536 to actual pulsewidth. 0 is + * all the way left (1.0 ms pulse) and 65536 is all the way right (2.0 ms + * pulse). Use it like this: + * + * servo_widths[ i ] = servo_make_pulse_width( val ) + * + * + * 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. + * + */ + +#ifndef SERVO_H +#define SERVO_H + +#include +#include "timer.h" + +extern void servo_init( void ); +extern void servo_set(const pprz_t values[]); +extern void servo_set_one(uint8_t servo, uint16_t value_us); +extern void servo_transmit(void); + + +#endif /* SERVO_H */ diff --git a/sw/airborne/fly_by_wire/spi.c b/sw/airborne/fly_by_wire/spi.c new file mode 100644 index 0000000000..955615d597 --- /dev/null +++ b/sw/airborne/fly_by_wire/spi.c @@ -0,0 +1,112 @@ +/* + * $Id$ + * + * Paparazzi mcu1 spi functions + * + * 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. + * + */ + +#include +#include +#include +#include + +#include "spi.h" + +#define IT_PORT PORTD +#define IT_DDR DDRD +#define IT_PIN 7 + +#define SPI_DDR DDRB +#define SPI_MOSI_PIN 3 +#define SPI_MISO_PIN 4 +#define SPI_SCK_PIN 5 + +struct inter_mcu_msg from_mega128; +struct inter_mcu_msg to_mega128; +volatile bool_t mega128_receive_valid = FALSE; +volatile bool_t spi_was_interrupted = FALSE; + +static volatile uint8_t idx_buf = 0; +static volatile uint8_t xor_in, xor_out; + +void spi_reset(void) { + idx_buf = 0; + xor_in = 0; + xor_out = ((uint8_t*)&to_mega128)[idx_buf]; + SPDR = xor_out; + mega128_receive_valid = FALSE; +} + +void spi_init(void) { + to_mega128.status = 0; + to_mega128.nb_err = 0; + + /* set it pin output */ + // IT_DDR |= _BV(IT_PIN); + + /* set MISO pin output */ + SPI_DDR |= _BV(SPI_MISO_PIN); + /* enable SPI, slave, MSB first, sck idle low */ + SPCR = _BV(SPE); + /* enable interrupt */ + SPCR |= _BV(SPIE); +} + +SIGNAL(SIG_SPI) { + static uint8_t tmp; + + idx_buf++; + + spi_was_interrupted = TRUE; + + if (idx_buf > FRAME_LENGTH) + return; + /* we have sent/received a complete frame */ + if (idx_buf == FRAME_LENGTH) { + /* read checksum from receive register */ + tmp = SPDR; + /* notify valid frame */ + if (tmp == xor_in) + mega128_receive_valid = TRUE; + else + to_mega128.nb_err++; + return; + } + + /* we are sending/receiving payload */ + if (idx_buf < FRAME_LENGTH - 1) { + /* place new payload byte in send register */ + tmp = ((uint8_t*)&to_mega128)[idx_buf]; + SPDR = tmp; + xor_out = xor_out ^ tmp; + } + /* we are done sending the payload */ + else { // idx_buf == FRAME_LENGTH - 1 + /* place checksum in send register */ + SPDR = xor_out; + } + + /* read the byte from receive register */ + tmp = SPDR; + ((uint8_t*)&from_mega128)[idx_buf-1] = tmp; + xor_in = xor_in ^ tmp; +} diff --git a/sw/airborne/fly_by_wire/spi.h b/sw/airborne/fly_by_wire/spi.h new file mode 100644 index 0000000000..84ca3a2bdc --- /dev/null +++ b/sw/airborne/fly_by_wire/spi.h @@ -0,0 +1,48 @@ +/* $Id$ + * + * Paparazzi fbw spi functions + * + * 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. + * + */ + +#ifndef SPI_H +#define SPI_H + +#include "link_autopilot.h" + + +#define SPI_PORT PORTB +#define SPI_PIN PINB +#define SPI_SS_PIN 2 + +#define SpiIsSelected() (bit_is_clear(SPI_PIN, SPI_SS_PIN)) + +extern struct inter_mcu_msg from_mega128; +extern struct inter_mcu_msg to_mega128; +extern volatile bool_t mega128_receive_valid; +extern volatile bool_t spi_was_interrupted; + + +void spi_init(void); +void spi_reset(void); + + +#endif /* SPI_H */ diff --git a/sw/airborne/fly_by_wire/test/Makefile b/sw/airborne/fly_by_wire/test/Makefile new file mode 100644 index 0000000000..3d1ba17283 --- /dev/null +++ b/sw/airborne/fly_by_wire/test/Makefile @@ -0,0 +1,43 @@ + + +all: + @echo "call with 'make TARGET=... compile (or load)'" + +TARGET=check_uart + +LOCAL_CFLAGS= $(CTL_BRD_FLAGS) + +ARCH = atmega8 +INCLUDES = -I ../ -I ../../../include -I ../../../var/include + +CONF_DIR = ../../../../conf +CONF_XML = $(CONF_DIR)/conf.xml +XML_GET=../../../lib/ocaml/xml_get.out + +ifeq ($(CTL_BRD_VERSION),V1_2_1) +CTL_BRD_FLAGS=-DCTL_BRD_V1_2_1 +endif + +ifeq ($(CTL_BRD_VERSION),V1_2) +CTL_BRD_FLAGS=-DCTL_BRD_V1_2 +endif + +ifeq ($(CTL_BRD_VERSION),V1_1) +CTL_BRD_FLAGS=-DCTL_BRD_V1_1 +endif + +rc_transmitter.srcs = rc_transmitter.c ../ppm.c ../uart.c + +setup_servos.srcs = setup_servos.c ../uart.c ../servo.c + +check_uart.srcs = check_uart.c ../uart.c + +tx_adcs.srcs = tx_adcs.c ../uart.c ../adc_fbw.c + +check_spi.srcs = check_spi.c ../uart.c ../spi.c + + +include ../../../../conf/Makefile.local +include ../../../../conf/Makefile.avr + +clean: avr_clean diff --git a/sw/airborne/fly_by_wire/test/check_spi.c b/sw/airborne/fly_by_wire/test/check_spi.c new file mode 100644 index 0000000000..7f0d3f5d09 --- /dev/null +++ b/sw/airborne/fly_by_wire/test/check_spi.c @@ -0,0 +1,43 @@ +#include +#include "timer.h" +#include "spi.h" +#include "uart.h" + +/* Fill the message with dummy values */ +void fill_spi_msg(void) { + uint8_t i; + for(i = 0; i < RADIO_CTL_NB; i++) + to_mega128.channels[i] = i * (MAX_PPRZ / RADIO_CTL_NB); + to_mega128.status = 0xff; + to_mega128.ppm_cpt = 0xff; + to_mega128.vsupply = 0xff; +} + +int main( void ) +{ + uart_init_tx(); + uart_print_string("Booting FBW MCU: $Id$\n"); + spi_init(); + timer_init(); + sei(); + + uint8_t _1Hz = 0; + while( 1 ) { + if(timer_periodic()) { + _1Hz++; + if (_1Hz >= 60) { + _1Hz = 0; + uart_print_string("FBW MCU Alive\n"); + } + } + if ( !SpiIsSelected() && spi_was_interrupted ) { + spi_was_interrupted = FALSE; + if (mega128_receive_valid) { + uart_print_string("SPI OK from mega128\n"); + } else + uart_print_string("SPI error from mega128\n"); + fill_spi_msg(); + spi_reset(); + } + } +} diff --git a/sw/airborne/fly_by_wire/test/check_uart.c b/sw/airborne/fly_by_wire/test/check_uart.c new file mode 100644 index 0000000000..b0f067a7ba --- /dev/null +++ b/sw/airborne/fly_by_wire/test/check_uart.c @@ -0,0 +1,51 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include +#include "timer.h" +#include "uart.h" + +int main( void ) { + uart_init_tx(); + uart_print_string("Booting FBW MCU: $Id$\n"); + timer_init(); + sei(); + uint8_t _1Hz = 0; + uint8_t foo = 0; + while( 1 ) { + if(timer_periodic()) { + _1Hz++; + if (_1Hz >= 60) { + _1Hz = 0; + foo++; + uart_print_string("FBW MCU uart check : alive ["); + uart_print_hex(foo); + uart_print_string("]\n"); + } + } + } + return 0; +} diff --git a/sw/airborne/fly_by_wire/test/rc_transmitter.c b/sw/airborne/fly_by_wire/test/rc_transmitter.c new file mode 100644 index 0000000000..4f487c443a --- /dev/null +++ b/sw/airborne/fly_by_wire/test/rc_transmitter.c @@ -0,0 +1,71 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include + +#include "timer.h" +#include "ppm.h" +#include "radio.h" + +#include "uart.h" + +inline void transmit_radio(void) { + uint8_t ctl; + uart_transmit((uint8_t)0); uart_transmit((uint8_t)0); + for(ctl = 0; ctl < RADIO_CTL_NB; ctl++) { + extern uint16_t ppm_pulses[]; + uint16_t x = ppm_pulses[ctl] / 16; + uart_transmit((uint8_t)(x >> 8)); + uart_transmit((uint8_t)(x & 0xff)); + } + uart_transmit((uint8_t)'\n'); + // uart_transmit('A');uart_transmit('\n'); +} + +int main( void ) { + uart_init_tx(); + uart_print_string("Calib_radio Booting $Id$\n"); + timer_init(); + ppm_init(); + sei(); + int n = 0; + while( 1 ) { + if( ppm_valid ) { + ppm_valid = FALSE; + n++; + if (n == 4) { + n = 0; + transmit_radio(); + } + } + + // A rajouter pour envoyer un message de vie quand la radio n'est pas recue + // if(timer_periodic()) { + // uart_transmit('B');uart_transmit('\n'); + // } + } + return 0; +} diff --git a/sw/airborne/fly_by_wire/test/setup_servos.c b/sw/airborne/fly_by_wire/test/setup_servos.c new file mode 100644 index 0000000000..e8e55bf079 --- /dev/null +++ b/sw/airborne/fly_by_wire/test/setup_servos.c @@ -0,0 +1,119 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include + +#include "timer.h" +#include "servo.h" +#include "uart.h" + +#define MSG_START '\0' +#define MSG_END '\n' + +#define UNINIT 0 +#define GOT_START 1 +#define GOT_CHANNEL 2 +#define GOT_LOW 3 +#define GOT_HI 4 + + +static uint8_t msg_status; +static uint8_t servo; +static uint16_t value; /* micro-seconds */ +static volatile bool_t msg_valid; + +static inline void parse_msg(uint8_t c) { + switch (msg_status) { + case UNINIT: + if (c==MSG_START) + msg_status++; + else + goto restart; + break; + case GOT_START: + servo=c; + msg_status++; + break; + case GOT_CHANNEL: + value=c << 8; + msg_status++; + break; + case GOT_LOW: + value |= c; + msg_status++; + break; + case GOT_HI: + if (c == MSG_END) + msg_valid = TRUE; + goto restart; + } + return; + restart: + msg_status = UNINIT; +} + +/* RxUartCb(parse_msg) */ + +SIGNAL( SIG_UART_RECV ) { + uint8_t c = inp( UDR ); + parse_msg(c); +} + + +int main( void ) { + uart_init_tx(); + uart_init_rx(); + timer_init(); + servo_init(); + sei(); + + uart_print_string("$Id$\n"); + + while (1) { + if (msg_valid) { + msg_valid = FALSE; + servo_set_one(servo, value); + } + + +/* if (timer_periodic()) { */ +/* servo_set_one(SERVO, value); */ +/* value += 10; */ +/* if (value > 2000) value = 1000; */ +/* } */ + +/* if (timer_periodic()) { */ +/* static uint8_t foo; */ +/* if (!foo) { */ +/* uart_transmit('A'); */ +/* uart_transmit('\n'); */ +/* } */ +/* foo++; */ +/* } */ + } + + return 0; +} diff --git a/sw/airborne/fly_by_wire/test/tx_adcs.c b/sw/airborne/fly_by_wire/test/tx_adcs.c new file mode 100644 index 0000000000..2f2d0e9758 --- /dev/null +++ b/sw/airborne/fly_by_wire/test/tx_adcs.c @@ -0,0 +1,66 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include +#include "timer.h" +#include "uart.h" +#include "adc_fbw.h" + +static struct adc_buf buffers[NB_ADC]; + +void transmit_adc(void) { + uint8_t i; + uart_transmit((uint8_t)0); uart_transmit((uint8_t)0); + for(i = 0; i < NB_ADC; i++) { + uint16_t value = buffers[i].sum / AV_NB_SAMPLE; + uart_transmit((uint8_t)(value >> 8)); + uart_transmit((uint8_t)(value & 0xff)); + } + uart_transmit((uint8_t)'\n'); +} + +int main( void ) { + uint8_t i; + uart_init_tx(); + timer_init(); + adc_init(); + for(i = 0; i < NB_ADC; i++) + adc_buf_channel(i, &buffers[i]); + sei(); + + while( 1 ) { + static uint8_t _1Hz = 0; + + if(timer_periodic()) { + _1Hz++; + if (_1Hz == 60) { + _1Hz = 0; + transmit_adc(); + } + } + } + return 0; +} diff --git a/sw/airborne/fly_by_wire/timer.h b/sw/airborne/fly_by_wire/timer.h new file mode 100644 index 0000000000..84079382ad --- /dev/null +++ b/sw/airborne/fly_by_wire/timer.h @@ -0,0 +1,92 @@ +/* $Id$ + * + * Paparazzi fbw timer functions + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * Copyright (C) 2002 Trammell Hudson + * 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. + * + */ + +#ifndef TIMER_H +#define TIMER_H + +#include +#include +#include +#include "link_autopilot.h" + + +/* + * Enable Timer1 (16-bit) running at Clk/1 for the global system + * clock. This will be used for computing the servo pulse widths, + * PPM decoding, etc. + * + * Low frequency periodic tasks will be signaled by timer 2 + * running at Clk/1024. For 16 Mhz clock, this will be every + * 16384 microseconds, or 61 Hz. + */ +static inline void +timer_init( void ) +{ + /* Timer1 @ Clk/1: System clock, ppm and servos pulses */ + TCCR1A = 0x00; + TCCR1B = 0x01; + + /* Timer2 @ Clk/1024: Periodic clock */ + TCCR2 = 0x07; +} + + +/* + * Retrieve the current time from the global clock in Timer1, + * disabling interrupts to avoid stomping on the TEMP register. + * If interrupts are already off, the non_atomic form can be used. + */ +static inline uint16_t +timer_now( void ) +{ + return TCNT1; +} + +static inline uint16_t +timer_now_non_atomic( void ) +{ + return TCNT1L; +} + + +/* + * Periodic tasks occur when Timer2 overflows. Check and unset + * the overflow bit. We cycle through four possible periodic states, + * so each state occurs every 60 Hz. + */ +static inline bool_t +timer_periodic( void ) +{ + if( !bit_is_set( TIFR, TOV2 ) ) + return FALSE; + + TIFR = 1 << TOV2; + return TRUE; +} + +#endif diff --git a/sw/airborne/fly_by_wire/uart.c b/sw/airborne/fly_by_wire/uart.c new file mode 100644 index 0000000000..12450d2f41 --- /dev/null +++ b/sw/airborne/fly_by_wire/uart.c @@ -0,0 +1,110 @@ +/* + * Paparazzi $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. + * + */ + +#include +#include +#include + + +#include "std.h" +#include "uart.h" + +#define TX_BUF_SIZE 256 +static uint8_t tx_head; /* next free in buf */ +static volatile uint8_t tx_tail; /* next char to send */ +static uint8_t tx_buf[ TX_BUF_SIZE ]; + +/* + * UART Baud rate generation settings: + * + * With 16.0 MHz clock,UBRR=25 => 38400 baud + * + */ +void uart_init_tx( void ) { + /* Baudrate is 38.4k */ + UBRRH = 0; + UBRRL = 25; + /* single speed */ + UCSRA = 0; + /* Enable transmitter */ + UCSRB = _BV(TXEN); + /* Set frame format: 8data, 1stop bit */ + UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0); +} + +void uart_init_rx() { + /* Enable receiver */ + UCSRB |= _BV(RXEN); + /* Enable uart receive interrupt */ + sbi( UCSRB, RXCIE ); +} + +void uart_transmit( unsigned char data ) { + if (UCSRB & _BV(TXCIE)) { + /* we are waiting for the last char to be sent : buffering */ + if (tx_tail == tx_head + 1) { /* BUF_SIZE = 256 */ + /* Buffer is full (almost, but tx_head = tx_tail means "empty" */ + return; + } + tx_buf[tx_head] = data; + tx_head++; /* BUF_SIZE = 256 */ + } else { /* Channel is free: just send */ + UDR = data; + sbi(UCSRB, TXCIE); + } +} + +void uart_print_hex ( uint8_t c ) { + const uint8_t hex[16] = { '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; + uint8_t high = (c & 0xF0)>>4; + uint8_t low = c & 0x0F; + uart_transmit(hex[high]); + uart_transmit(hex[low]); +} + +void uart_print_hex16 ( uint16_t c ) { + uint8_t high = (uint8_t)(c>>8); + uint8_t low = (uint8_t)(c); + uart_print_hex(high); + uart_print_hex(low); +} + +void uart_print_string(const uint8_t* s) { + uint8_t i = 0; + while (s[i]) { + uart_transmit(s[i]); + i++; + } +} + +SIGNAL(SIG_UART_TRANS) { + if (tx_head == tx_tail) { + /* Nothing more to send */ + cbi(UCSRB, TXCIE); /* disable interrupt */ + } else { + UDR = tx_buf[tx_tail]; + tx_tail++; /* warning tx_buf_len is 256 */ + } +} diff --git a/sw/airborne/fly_by_wire/uart.h b/sw/airborne/fly_by_wire/uart.h new file mode 100644 index 0000000000..9c95d4e9e6 --- /dev/null +++ b/sw/airborne/fly_by_wire/uart.h @@ -0,0 +1,39 @@ +/* + * Paparazzi $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. + * + */ + +#ifndef _UART_H_ +#define _UART_H_ + +#include + +void uart_init_tx( void ); +void uart_init_rx( void ); +void uart_transmit( unsigned char data ); + +void uart_print_hex ( uint8_t c ); +void uart_print_hex16 ( uint16_t c ); +void uart_print_string(const uint8_t* s); +void uart_print_float( const float * f); + +#endif diff --git a/sw/airborne/quadrirotor_autopilot/Makefile b/sw/airborne/quadrirotor_autopilot/Makefile new file mode 100644 index 0000000000..5b24cf1aa3 --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/Makefile @@ -0,0 +1,95 @@ +# +# $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. +# + + +LOCAL_CFLAGS= $(CTL_BRD_FLAGS) $(GPS_FLAGS) $(SIMUL_FLAGS) + +CONF_DIR = ../../../conf +CONF_XML = $(CONF_DIR)/conf.xml + +ARCH = atmega128 +TARGET = autopilot + +ifeq ($(CTL_BRD_VERSION),V1_2_1) +LOW_FUSE = e0 +HIGH_FUSE = 99 +CTL_BRD_FLAGS=-DCTL_BRD_V1_2_1 +endif + +ifeq ($(CTL_BRD_VERSION),V1_2) +LOW_FUSE = e0 +HIGH_FUSE = 99 +CTL_BRD_FLAGS=-DCTL_BRD_V1_2 +endif + +EXT_FUSE = ff +LOCK_FUSE = ff +INCLUDES = -I ../autopilot -I ../fly_by_wire -I ../../include +OCAMLC = ocamlc -I ../../lib/ocaml + +AP=../autopilot + + +$(TARGET).srcs = \ + main.c \ + $(AP)/modem.c \ + $(AP)/link_fbw.c\ + $(AP)/spi.c \ + $(AP)/adc.c \ + $(AP)/uart.c \ + kalman.c \ + imu.c \ + control.c \ + + +include ../../../conf/Makefile.local +include ../../../conf/Makefile.avr + +autopilot.install : warn_conf + +warn_conf : + @echo + @echo '###########################################################' + @grep AIRFRAME_NAME ../fly_by_wire/airframe.h + @grep RADIO_NAME ../fly_by_wire/radio.h +# @grep FLIGHT_PLAN_NAME flight_plan.h + @echo '###########################################################' + @echo + + +#.depend : messages.h flight_plan.h ubx_protocol.h inflight_calib.h +main.o : messages.h +nav.o : flight_plan.h +gps_ubx.o : ubx_protocol.h +if_calib.o : inflight_calib.h + +GEN_MESSAGES = ./gen_messages.out +MESSAGES_XML = $(CONF_DIR)/messages.xml + +messages.h : $(MESSAGES_XML) $(GEN_MESSAGES) + $(GEN_MESSAGES) $< > $@ + +$(GEN_MESSAGES) : $(AP)/gen_messages.ml + $(OCAMLC) -o $@ xml-light.cma $< + +clean : avr_clean + rm -f *.out *.cm* messages.h flight_plan.h ubx_protocol.h inflight_calib.h diff --git a/sw/airborne/quadrirotor_autopilot/control.c b/sw/airborne/quadrirotor_autopilot/control.c new file mode 100644 index 0000000000..9faa3770d4 --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/control.c @@ -0,0 +1,47 @@ +#include + + +#include "control.h" +#include "kalman.h" +#include "imu.h" +#include "autopilot.h" + +float control_desired_phi; +float control_desired_phi_dot; +float control_desired_theta; +float control_desired_theta_dot; + +int16_t control_command_roll; +int16_t control_command_pitch; + +#define phi_dot_p_gain -2500. +#define theta_dot_p_gain 2500. + +void control_run_rotational_speed_loop() { + float err = control_desired_phi_dot - kalman_public.phi_dot; + control_command_roll = TRIM_PPRZ(phi_dot_p_gain * err); + err = control_desired_theta_dot - kalman_public.theta_dot; + control_command_pitch = TRIM_PPRZ(theta_dot_p_gain * err); +} + +void control_run_raw_rotational_speed_loop() { + float err = control_desired_phi_dot - imu_sample.gyro_x; + control_command_roll = TRIM_PPRZ(phi_dot_p_gain * err); + err = control_desired_theta_dot - imu_sample.gyro_y; + control_command_pitch = TRIM_PPRZ(theta_dot_p_gain * err); +} + + +#define TRIM(val, limit) ( val < -limit ? -limit : val > limit ? limit : val ) + +#define MAX_PHI_DOT 2. +#define phi_p_gain 3. +#define MAX_THETA_DOT 2. +#define theta_p_gain 3. + +void control_run_attitude_loop() { + float err = control_desired_phi - kalman_public.phi; + control_desired_phi_dot = TRIM(phi_p_gain * err, MAX_PHI_DOT); + err = control_desired_theta - kalman_public.theta; + control_desired_theta_dot = TRIM(theta_p_gain * err, MAX_THETA_DOT); +} diff --git a/sw/airborne/quadrirotor_autopilot/control.h b/sw/airborne/quadrirotor_autopilot/control.h new file mode 100644 index 0000000000..0ea2c3df1f --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/control.h @@ -0,0 +1,18 @@ +#ifndef CONTROL_H +#define CONTROL_H + +extern float control_desired_phi; +extern float control_desired_phi_dot; +extern float control_desired_theta; +extern float control_desired_theta_dot; + +extern int16_t control_command_roll; +extern int16_t control_command_pitch; + +void control_run_rotational_speed_loop( void ); +void control_run_raw_rotational_speed_loop( void ); +void control_run_attitude_loop( void ); + + + +#endif /* CONTROL_H */ diff --git a/sw/airborne/quadrirotor_autopilot/imu.c b/sw/airborne/quadrirotor_autopilot/imu.c new file mode 100644 index 0000000000..af42621085 --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/imu.c @@ -0,0 +1,76 @@ +#include + +#include "imu.h" +#include "airframe.h" +#include "adc.h" + +struct ImuSample imu_sample; + +//static struct adc_buf buf_gyro_roll; +//static struct adc_buf buf_accel_y; +//static struct adc_buf buf_accel_z; + + +/* carte sur le coté Y<->Z */ + +#define GYRO_X_NEUTRAL 425 +#define GYRO_X_GAIN 0.0170 + +#define GYRO_Y_NEUTRAL 463 +#define GYRO_Y_GAIN -0.0179 + +#define GYRO_Z_NEUTRAL 460 +#define GYRO_Z_GAIN 0.0180 + +#define ACCEL_X_MIN 272 +#define ACCEL_X_MAX 689 + +#define ACCEL_X_NEUTRAL ((ACCEL_X_MIN + ACCEL_X_MAX)/2) +#define ACCEL_X_GAIN (2 * 9.81 / (ACCEL_X_MAX - ACCEL_X_MIN)) + +#define ACCEL_Y_MIN 725 +#define ACCEL_Y_MAX 272 + +#define ACCEL_Y_NEUTRAL ((ACCEL_Y_MIN + ACCEL_Y_MAX)/2) +#define ACCEL_Y_GAIN (2 * 9.81 / (ACCEL_Y_MAX - ACCEL_Y_MIN)) + +#define ACCEL_Z_MIN 635 +#define ACCEL_Z_MAX 226 + +#define ACCEL_Z_NEUTRAL ((ACCEL_Z_MIN + ACCEL_Z_MAX)/2) +#define ACCEL_Z_GAIN (2 * 9.81 / (ACCEL_Z_MAX - ACCEL_Z_MIN)) + +/* carte debout +Gy : + - 0: 460 + - 16 T/mn: 460 + - 33 T/mn : 370 + - broken +Gz: + - 0: 463 + - 16: 363 + - 33: 270 + - gain 1.79e-2 + + +Gx: + - 0: 425 + - 16: 325 + - 33: 220 + - gain 1.69e-2 + */ + +void imu_init( void ) { + // adc_buf_channel(ADC_CHANNEL_GYRO_X, &buf_gyro_roll); + // adc_buf_channel(ADC_CHANNEL_ACCEL_Y, &buf_accel_y); + // adc_buf_channel(ADC_CHANNEL_ACCEL_Z, &buf_accel_z); +} + +void imu_update( void ) { + imu_sample.gyro_x = (float)((int16_t)adc_samples[ADC_CHANNEL_GYRO_X] - GYRO_X_NEUTRAL) * GYRO_X_GAIN; + imu_sample.gyro_y = (float)((int16_t)adc_samples[ADC_CHANNEL_GYRO_Y] - GYRO_Y_NEUTRAL) * GYRO_Y_GAIN; + imu_sample.gyro_z = (float)((int16_t)adc_samples[ADC_CHANNEL_GYRO_Z] - GYRO_Z_NEUTRAL) * GYRO_Z_GAIN; + imu_sample.accel_x = (float)((int16_t)adc_samples[ADC_CHANNEL_ACCEL_X] - ACCEL_X_NEUTRAL) * ACCEL_X_GAIN; + imu_sample.accel_y = (float)((int16_t)adc_samples[ADC_CHANNEL_ACCEL_Y] - ACCEL_Y_NEUTRAL) * ACCEL_Y_GAIN; + imu_sample.accel_z = (float)((int16_t)adc_samples[ADC_CHANNEL_ACCEL_Z] - ACCEL_Z_NEUTRAL) * ACCEL_Z_GAIN; +} diff --git a/sw/airborne/quadrirotor_autopilot/imu.h b/sw/airborne/quadrirotor_autopilot/imu.h new file mode 100644 index 0000000000..a5b9094ed6 --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/imu.h @@ -0,0 +1,19 @@ +#ifndef IMU_H +#define IMU_H + +struct ImuSample { + float gyro_x; + float gyro_y; + float gyro_z; + float accel_x; + float accel_y; + float accel_z; +}; + +extern struct ImuSample imu_sample; + +//void imu_init( void ); + +void imu_update( void ); + +#endif /* IMU_H */ diff --git a/sw/airborne/quadrirotor_autopilot/kalman.c b/sw/airborne/quadrirotor_autopilot/kalman.c new file mode 100644 index 0000000000..139a420e86 --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/kalman.c @@ -0,0 +1,111 @@ +#include + +#include "kalman.h" + + +struct KalmanPublic kalman_public; + +static const float dt = ( 1024.0 * 256.0 ) / 16000000.0; + +static float P_phi[2][2] = { + { 1, 0 }, + { 0, 1 }, +}; + +static const float R_angle_phi = 1.3 * 1.3; +static const float Q_angle_phi = 0.001; +static const float Q_gyro_phi = 0.0075; + +static float P_theta[2][2] = { + { 1, 0 }, + { 0, 1 }, +}; + +static const float R_angle_theta = 1.3 * 1.3; +static const float Q_angle_theta = 0.001; +static const float Q_gyro_theta = 0.0075; + +void kalman_state_update( float gyro_phi_measure, float gyro_theta_measure ) { + const float unbiased_gyro_phi = gyro_phi_measure - kalman_public.gyro_phi_bias; + const float P_phi_dot[2 * 2] = { + Q_angle_phi - P_phi[0][1] - P_phi[1][0],/* 0,0 */ + - P_phi[1][1], /* 0,1 */ + - P_phi[1][1], /* 1,0 */ + Q_gyro_phi /* 1,1 */ + }; + kalman_public.phi_dot = unbiased_gyro_phi; + kalman_public.phi += unbiased_gyro_phi * dt; + P_phi[0][0] += P_phi_dot[0] * dt; + P_phi[0][1] += P_phi_dot[1] * dt; + P_phi[1][0] += P_phi_dot[2] * dt; + P_phi[1][1] += P_phi_dot[3] * dt; + + const float unbiased_gyro_theta = gyro_theta_measure - kalman_public.gyro_theta_bias; + const float P_theta_dot[2 * 2] = { + Q_angle_theta - P_theta[0][1] - P_theta[1][0],/* 0,0 */ + - P_theta[1][1], /* 0,1 */ + - P_theta[1][1], /* 1,0 */ + Q_gyro_theta /* 1,1 */ + }; + kalman_public.theta_dot = unbiased_gyro_theta; + kalman_public.theta += unbiased_gyro_theta * dt; + P_theta[0][0] += P_theta_dot[0] * dt; + P_theta[0][1] += P_theta_dot[1] * dt; + P_theta[1][0] += P_theta_dot[2] * dt; + P_theta[1][1] += P_theta_dot[3] * dt; + +} + + +void kalman_kalman_update( float ax_measure, float ay_measure, float az_measure ) { + const float angle_phi_measure = atan2( ay_measure, az_measure ); + const float angle_phi_err = angle_phi_measure - kalman_public.phi; + const float C_0 = 1; + const float PCt_0 = C_0 * P_phi[0][0]; /* + C_1 * P[0][1] = 0 */ + const float PCt_1 = C_0 * P_phi[1][0]; /* + C_1 * P[1][1] = 0 */ + const float E = + R_angle_phi + + C_0 * PCt_0 + /* + C_1 * PCt_1 = 0 */ + ; + const float K_0 = PCt_0 / E; + const float K_1 = PCt_1 / E; + const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */ + const float t_1 = C_0 * P_phi[0][1]; /* + C_1 * P[1][1] = 0 */ + + P_phi[0][0] -= K_0 * t_0; + P_phi[0][1] -= K_0 * t_1; + P_phi[1][0] -= K_1 * t_0; + P_phi[1][1] -= K_1 * t_1; + + kalman_public.phi += K_0 * angle_phi_err; + kalman_public.gyro_phi_bias += K_1 * angle_phi_err; + + + const float angle_theta_measure = atan2( -ax_measure, az_measure ); + const float angle_theta_err = angle_theta_measure - kalman_public.theta; + + const float t_C_0 = 1; + const float t_PCt_0 = t_C_0 * P_theta[0][0]; + const float t_PCt_1 = t_C_0 * P_theta[1][0]; + const float t_E = + R_angle_theta + + t_C_0 * t_PCt_0 + /* + C_1 * PCt_1 = 0 */ + ; + ; + const float t_K_0 = t_PCt_0 / t_E; + const float t_K_1 = t_PCt_1 / t_E; + const float t_t_0 = t_PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */ + const float t_t_1 = t_C_0 * P_theta[0][1]; /* + C_1 * P[1][1] = 0 */ + + P_theta[0][0] -= t_K_0 * t_t_0; + P_theta[0][1] -= t_K_0 * t_t_1; + P_theta[1][0] -= t_K_1 * t_t_0; + P_theta[1][1] -= t_K_1 * t_t_1; + + kalman_public.theta += t_K_0 * angle_theta_err; + kalman_public.gyro_theta_bias += t_K_1 * angle_theta_err; + + +} diff --git a/sw/airborne/quadrirotor_autopilot/kalman.h b/sw/airborne/quadrirotor_autopilot/kalman.h new file mode 100644 index 0000000000..7f5c126739 --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/kalman.h @@ -0,0 +1,19 @@ +#ifndef KALMAN_H +#define KALMAN_H + +void kalman_init( void ); +void kalman_state_update( float gyro_phi_measure, float gyro_theta_measure ); +void kalman_kalman_update( float ax_measure, float ay_measure, float az_measure ); + +struct KalmanPublic { + float phi; + float phi_dot; + float gyro_phi_bias; + float theta; + float theta_dot; + float gyro_theta_bias; +}; + +extern struct KalmanPublic kalman_public; + +#endif /* KALMAN_H */ diff --git a/sw/airborne/quadrirotor_autopilot/kalman_phi.c b/sw/airborne/quadrirotor_autopilot/kalman_phi.c new file mode 100644 index 0000000000..5ab88ed919 --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/kalman_phi.c @@ -0,0 +1,62 @@ +#include + +#include "kalman.h" + + +struct KalmanPublic kalman_public; + +static const float dt = ( 1024.0 * 256.0 ) / 16000000.0; + +static float P[2][2] = { + { 1, 0 }, + { 0, 1 }, +}; + +static const float R_angle_phi = 0.3; + +static const float Q_angle_phi = 0.001; +static const float Q_gyro_phi = 0.003; + + +void kalman_state_update( float gyro_phi_measure ) { + const float unbiased_gyro_phi = gyro_phi_measure - kalman_public.gyro_phi_bias; + const float Pdot[2 * 2] = { + Q_angle_phi - P[0][1] - P[1][0],/* 0,0 */ + - P[1][1], /* 0,1 */ + - P[1][1], /* 1,0 */ + Q_gyro_phi /* 1,1 */ + }; + kalman_public.phi_dot = unbiased_gyro_phi; + kalman_public.phi += unbiased_gyro_phi * dt; + P[0][0] += Pdot[0] * dt; + P[0][1] += Pdot[1] * dt; + P[1][0] += Pdot[2] * dt; + P[1][1] += Pdot[3] * dt; + +} + + +void kalman_kalman_update( float ax_measure, float ay_measure ) { + const float angle_phi_measure = atan2( ax_measure, ay_measure ); + const float angle_phi_err = angle_phi_measure - kalman_public.phi; + const float C_0 = 1; + const float PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */ + const float PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */ + const float E = + R_angle_phi + + C_0 * PCt_0 + /* + C_1 * PCt_1 = 0 */ + ; + const float K_0 = PCt_0 / E; + const float K_1 = PCt_1 / E; + const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */ + const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 */ + + P[0][0] -= K_0 * t_0; + P[0][1] -= K_0 * t_1; + P[1][0] -= K_1 * t_0; + P[1][1] -= K_1 * t_1; + + kalman_public.phi += K_0 * angle_phi_err; + kalman_public.gyro_phi_bias += K_1 * angle_phi_err; +} diff --git a/sw/airborne/quadrirotor_autopilot/main.c b/sw/airborne/quadrirotor_autopilot/main.c new file mode 100644 index 0000000000..e558b103db --- /dev/null +++ b/sw/airborne/quadrirotor_autopilot/main.c @@ -0,0 +1,172 @@ +#include +#include +#include + +#include "messages.h" +#include "downlink.h" +#include "airframe.h" +#include "timer.h" +#include "adc.h" +#include "uart.h" +#include "link_fbw.h" +#include "spi.h" +#include "autopilot.h" + +#include "imu.h" +#include "kalman.h" +#include "control.h" + + + +#define TRANS_RAW_IMU() { \ + static uint8_t foo; foo++; \ + if (!(foo%10)) { \ + DOWNLINK_SEND_RAW_IMU( &adc_samples[ADC_CHANNEL_GYRO_X], &adc_samples[ADC_CHANNEL_GYRO_Y], \ + &adc_samples[ADC_CHANNEL_GYRO_Z], &adc_samples[ADC_CHANNEL_ACCEL_X],\ + &adc_samples[ADC_CHANNEL_ACCEL_Y], &adc_samples[ADC_CHANNEL_ACCEL_Z]); \ + } \ + }\ + + +#define TRANS_IMU() { \ + static uint8_t foo; foo++; \ + if (!(foo%10)) { \ + DOWNLINK_SEND_IMU( &imu_sample.gyro_x, &imu_sample.gyro_y, &imu_sample.gyro_z, \ + &imu_sample.accel_x, &imu_sample.accel_y, &imu_sample.accel_z); \ + } \ + } \ + +#define TRANS_KALMAN() { \ + static uint8_t foo; foo++; \ + if (!(foo%10)) { \ + DOWNLINK_SEND_KALMAN( &kalman_public.phi, &kalman_public.phi_dot, \ + &kalman_public.gyro_phi_bias, &kalman_public.theta, \ + &kalman_public.theta_dot, &kalman_public.gyro_theta_bias); \ + } \ + } \ + + +#define PERIODIC_SEND_BAT() DOWNLINK_SEND_BAT(&vsupply, &estimator_flight_time, &low_battery) +#define PERIODIC_SEND_ATTITUDE() DOWNLINK_SEND_ATTITUDE( &(kalman_public.phi), &psi, &(kalman_public.theta)) +#define PERIODIC_SEND_ADC() {} +#define PERIODIC_SEND_SETTINGS() {} +#define PERIODIC_SEND_DESIRED() {} +#define PERIODIC_SEND_CLIMB_PID() {} +#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_ATTITUDE( &(kalman_public.phi), &psi, &(kalman_public.theta)) +#define PERIODIC_SEND_DEBUG() {} +#define PERIODIC_SEND_NAVIGATION_REF() {} + + +uint8_t fatal_error_nb = 0; +uint16_t cputime = 0; +float psi = 0.; + +uint8_t mcu1_status; +uint8_t pprz_mode; +uint8_t vertical_mode; +uint8_t inflight_calib_mode; +uint8_t ir_estim_mode; + +uint8_t vsupply; + +uint8_t low_battery = FALSE; +uint16_t estimator_flight_time = 0; + +inline void copy_from_to_fbw ( void ) { + to_fbw.channels[RADIO_THROTTLE] = from_fbw.channels[RADIO_THROTTLE]; + // to_fbw.channels[RADIO_ROLL] = from_fbw.channels[RADIO_ROLL]; + // to_fbw.channels[RADIO_PITCH] = from_fbw.channels[RADIO_PITCH]; + to_fbw.channels[RADIO_YAW] = from_fbw.channels[RADIO_YAW]; + to_fbw.status = 0; +} + +inline uint8_t pprz_mode_update( void ) { + ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(from_fbw.channels[RADIO_MODE], from_fbw.status)); +} + +inline uint8_t mcu1_status_update( void ) { + uint8_t new_mode = from_fbw.status; + if (mcu1_status != new_mode) { + bool_t changed = ((mcu1_status&MASK_FBW_CHANGED) != (new_mode&MASK_FBW_CHANGED)); + mcu1_status = new_mode; + return changed; + } + return FALSE; +} + +inline void radio_control_task( void ) { + if (link_fbw_receive_valid) { + uint8_t mode_changed = FALSE; + copy_from_to_fbw(); + if (bit_is_set(from_fbw.status, AVERAGED_CHANNELS_SENT)) { + bool_t pprz_mode_changed = pprz_mode_update(); + mode_changed |= pprz_mode_changed; + } + mode_changed |= mcu1_status_update(); + if ( mode_changed ) + DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode); + + if (pprz_mode == PPRZ_MODE_AUTO1) { + control_desired_phi_dot = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_ROLL], 0., -1.25); + control_desired_theta_dot = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_PITCH], 0., 1.25); + } + else if (pprz_mode == PPRZ_MODE_AUTO2) { + control_desired_phi = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_ROLL], 0., -0.6); + control_desired_theta = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_PITCH], 0., 0.6); + } + vsupply = from_fbw.vsupply; + } +} + +inline void periodic_task ( void ) { + static uint8_t _20Hz = 0; + + _20Hz++; + if (_20Hz>=3) _20Hz=0; + + imu_update( ); + TRANS_RAW_IMU(); + TRANS_IMU(); + kalman_state_update(imu_sample.gyro_x, imu_sample.gyro_y); + kalman_kalman_update(imu_sample.accel_x, imu_sample.accel_y, imu_sample.accel_z); + TRANS_KALMAN(); + if (pprz_mode == PPRZ_MODE_AUTO2) { + // control_run_attitude_loop(); + control_run_raw_rotational_speed_loop(); + to_fbw.channels[RADIO_ROLL] = control_command_roll; + to_fbw.channels[RADIO_PITCH] = control_command_pitch; + } + if (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_AUTO2) { + control_run_rotational_speed_loop(); + to_fbw.channels[RADIO_ROLL] = control_command_roll; + to_fbw.channels[RADIO_PITCH] = control_command_pitch; + } + link_fbw_send(); + if (!_20Hz) + PeriodicSend(); +} + +int main(void) { + + timer_init(); + modem_init(); + adc_init(); + spi_init(); + link_fbw_init(); + // uart0_init(); + // imu_init(); + sei(); + + while (1) { + if (timer_periodic()) + periodic_task(); + if (link_fbw_receive_complete) { + radio_control_task(); + link_fbw_receive_complete = FALSE; + } + } + return 0; +} + + + diff --git a/sw/configurator/Makefile b/sw/configurator/Makefile new file mode 100644 index 0000000000..3ae1af8a38 --- /dev/null +++ b/sw/configurator/Makefile @@ -0,0 +1,65 @@ +# +# $Id$ +# Copyright (C) 2004 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. +# + +OCAMLC=ocamlc -g -I +labltk -I +lablgtk2 -I ../lib/ocaml + + +SRC = env.ml tty.ml varXml.ml console.ml tkXml.ml flasher.ml notebook.ml welcome.ml hardware.ml radio.ml servos.ml adc.ml infrared.ml attitude.ml autopilot.ml airframe.ml flightplan.ml upload.ml simulator.ml monitor.ml logalizer.ml main.ml +CMO = $(SRC:.ml=.cmo) + +all : configurator medit.out + +configurator : $(CMO) + $(OCAMLC) -custom -o $@ unix.cma str.cma xml-light.cma labltk.cma jpflib.cma lib.cma $^ + +medit.out : medit.cmo + $(OCAMLC) -o $@ str.cma unix.cma xml-light.cma glibivy-ocaml.cma -I +lablgtk2 lablgtk.cma lablgnomecanvas.cma gtkInit.cmo lib.cma $^ + +%.cmo : %.ml + $(OCAMLC) -c $< + +%.cmi : %.mli + $(OCAMLC) $< + +%.cmi : %.ml + $(OCAMLC) $< + + +%.i : %.ml + $(OCAMLC) -c -i $< + +clean : + rm -f *~ *.cm* *.out configurator medit .depend + + +# +# Dependencies +# + +.depend: + ocamldep *.ml* > .depend + +ifneq ($(MAKECMDGOALS),clean) +-include .depend +endif + +include ../../conf/Makefile.local diff --git a/sw/configurator/adc.ml b/sw/configurator/adc.ml new file mode 100644 index 0000000000..1bef10452b --- /dev/null +++ b/sw/configurator/adc.ml @@ -0,0 +1,36 @@ +(* + * $Id$ + * + * Analog to Digital Converters configuration + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets xml -> + let airframe = Frame.create sheets in + Notebook.create_sheet sheets "Adc" airframe; + + let name_is_adc = fun attributes -> + try Textvariable.get (List.assoc "name" attributes) = "adc" with Not_found -> false in + + let adc_section = VarXml.child xml ~select:name_is_adc "section" in + + TkXml.create_section airframe adc_section diff --git a/sw/configurator/airframe.ml b/sw/configurator/airframe.ml new file mode 100644 index 0000000000..6f816927ad --- /dev/null +++ b/sw/configurator/airframe.ml @@ -0,0 +1,38 @@ +(* + * $Id$ + * + * Airframe parameters configuration + * + * Copyright (C) 2004 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. + * + *) + +let create_sub_sheets = fun sf xml -> + Servos.create_sheet sf xml; + Adc.create_sheet sf xml; + Infrared.create_sheet sf xml; + Attitude.create_sheet sf xml; + Autopilot.create_sheet sf xml + +let create_sheet = fun sheets -> + let airframe = Frame.create sheets in + Notebook.create_sheet sheets "Airframe" airframe; + + TkXml.create airframe create_sub_sheets diff --git a/sw/configurator/attitude.ml b/sw/configurator/attitude.ml new file mode 100644 index 0000000000..642ba45a68 --- /dev/null +++ b/sw/configurator/attitude.ml @@ -0,0 +1,46 @@ +(* + * $Id$ + * + * Attitude control configuration + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets xml -> + let f = Frame.create sheets in + Notebook.create_sheet sheets "Attitude" f; + + + let label = fun l -> Widget.forget_type (Label.create ~text:l f) in + + let empty = Widget.dummy in + + let entry _ = Widget.forget_type (Label.create f) in + + let array = + [| [| entry (); label "Min"; label "Max"; label "Gain"|]; + [| label "Roll"; entry (); entry (); entry ()|]; + [| label "Pitch"; entry (); entry (); entry ()|]; + [| label "Throttle"; entry (); entry (); entry ()|] |] in + + Array.iteri + (fun i -> Array.iteri (fun j w -> Tk.grid ~row:i ~column:j [w])) + array diff --git a/sw/configurator/autopilot.ml b/sw/configurator/autopilot.ml new file mode 100644 index 0000000000..58714ad384 --- /dev/null +++ b/sw/configurator/autopilot.ml @@ -0,0 +1,29 @@ +(* + * $Id$ + * + * Autopilot configuration + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets xml -> + let airframe = Frame.create sheets in + Notebook.create_sheet sheets "Autopilot" airframe; diff --git a/sw/configurator/console.ml b/sw/configurator/console.ml new file mode 100644 index 0000000000..71ce330ee6 --- /dev/null +++ b/sw/configurator/console.ml @@ -0,0 +1,76 @@ +(* + * $Id$ + * + * Console (text widget) handling + * + * Copyright (C) 2004 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. + * + *) + +let console = ref None + +let create = fun parent -> + let c = Text.create parent in + console := Some c; + Text.tag_configure ~tag:"red" ~foreground:`Red c; + Text.tag_configure ~tag:"blue" ~foreground:`Blue c; + c + +let write = fun ?(tags=[]) s -> + match !console with + None -> failwith "Console.write" + | Some c -> + Text.insert ~index:(`End,[]) ~tags ~text:s c; + Text.see c (`End,[]) + + +let buffer_len = 256 + + +let copy_from = + let buffer = String.create buffer_len in + fun c kill color -> + let n = Unix.read c buffer 0 buffer_len in + if n = 0 then begin + Fileevent.remove_fileinput c; + kill () + end else + write ~tags:[color] (String.sub buffer 0 n) + + +let exec = fun command -> + write command; write "\n"; + let (proc_out, _, proc_err) as triple = Unix.open_process_full command [||] in + let proc_out = Unix.descr_of_in_channel proc_out + and proc_err = Unix.descr_of_in_channel proc_err in + let kill = + let twice = ref false in + fun () -> + if !twice then begin + write + (match Unix.close_process_full triple with + Unix.WEXITED c -> Printf.sprintf "--------- terminated(%d)\n\n" c + | Unix.WSIGNALED c -> Printf.sprintf "--------- killed(%d)\n\n" c + | Unix.WSTOPPED c -> Printf.sprintf "--------- stopped(%d)\n\n" c) + end else + twice := true + in + Fileevent.add_fileinput proc_out (fun () -> copy_from proc_out kill "blue"); + Fileevent.add_fileinput proc_err (fun () -> copy_from proc_err kill "red") diff --git a/sw/configurator/console.mli b/sw/configurator/console.mli new file mode 100644 index 0000000000..c3b5f6f1f9 --- /dev/null +++ b/sw/configurator/console.mli @@ -0,0 +1,34 @@ +(* + * $Id$ + * + * Console (text widget) handling + * + * Copyright (C) 2004 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. + * + *) + +val create : 'a Widget.widget -> Widget.text Widget.widget +(** [create widget] creates the (text) console widget *) + +val write : ?tags:Tk.textTag list -> string -> unit +(** Writes to the text console. Usefull tags are "red" and "blue" *) + +val exec : string -> unit +(** Executes a command while logging stdout and stderr in the console *) diff --git a/sw/configurator/env.ml b/sw/configurator/env.ml new file mode 100644 index 0000000000..944e39d9fd --- /dev/null +++ b/sw/configurator/env.ml @@ -0,0 +1,57 @@ +(* + * $Id$ + * + * Global and default settings + * + * Copyright (C) 2004 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. + * + *) + +let pprz_dir = + try + Sys.getenv "PAPARAZZI_DIR" + with + Not_found -> + Filename.concat (Filename.dirname Sys.argv.(0)) "../.." + +let abs = fun x -> pprz_dir ^ "/" ^ x + +let configurator_dir = abs "sw/configurator" + +let fbw_dir = abs "sw/airborne/fly_by_wire" +let ap_dir = abs "sw/airborne/autopilot" +let modem_dir = abs "sw/ground_segment/modem" + +let fbw_tty = "/dev/ttyS1" +let ap_tty = "/dev/ttyS0" +let modem_tty = "/dev/ttyUSB0" + +let tty_rate = Serial.B38400 + + +(* Initialization very early for creation of Textvariables *) +let _ = Tk.openTk () + +let select_one_file = fun ?(filter="*.xml") use -> + let action = function + [] -> () + | [f] -> use f + | _ -> failwith "Env.select_one_file: unepected several files" in + Fileselect.f ~title:"File Selection" ~action ~filter ~file:"" ~multi:false ~sync:false diff --git a/sw/configurator/env.mli b/sw/configurator/env.mli new file mode 100644 index 0000000000..cf56bedcfd --- /dev/null +++ b/sw/configurator/env.mli @@ -0,0 +1,41 @@ +(* + * $Id$ + * + * Global and default settings + * + * Copyright (C) 2004 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. + * + *) + +val pprz_dir : string + +val configurator_dir : string +val fbw_dir : string +val ap_dir : string +val modem_dir : string + +val fbw_tty : string +val ap_tty : string +val modem_tty : string + +val tty_rate : Serial.speed + +val select_one_file : ?filter:string -> (string -> unit) -> unit +(** File selector. Default [filter] is "*.xml" *) diff --git a/sw/configurator/flasher.ml b/sw/configurator/flasher.ml new file mode 100644 index 0000000000..a68b66b98e --- /dev/null +++ b/sw/configurator/flasher.ml @@ -0,0 +1,74 @@ +(* + * $Id$ + * + * Micro-controllers uploading + * + * Copyright (C) 2004 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. + * + *) + +type mcu = Fbw | Ap | Modem + +let string_of = function + Fbw -> "Fly by wire" + | Ap -> "Autopilot" + | Modem -> "Modem" + + +let switch = fun mcu -> + Dialog.create Widget.default_toplevel "Paparazzi upload" (Printf.sprintf "Please connect programmer board to %s\n" (string_of mcu)) ["OK"; "Cancel"] () + +let warn = + let dont_bother_me = Textvariable.create () in + fun continue -> + if Textvariable.get dont_bother_me = "1" then + continue () + else + let t = Toplevel.create Widget.default_toplevel in + Wm.title_set t "Paparazzi warning"; + + Grab.set t; + + let destroy = fun () -> Tk.destroy t; continue () in + + let erase_AP = fun () -> + if switch Ap = 0 then + let command = Printf.sprintf "cd %s; make erase" Env.ap_dir in + Console.exec command; + destroy () in + + let l = Label.create ~text:"Warning: You are about to program the fbw microcontroller. It is possible only if the AP microcontroller is erased." t + and b = Button.create ~text:"Erase AP first" ~command:erase_AP t + and b' = Button.create ~text:"Ok (AP erased)" ~command:destroy t + and b'' = Button.create ~text:"Cancel" ~command:(fun () -> Tk.destroy t) t + and x = Checkbutton.create ~text:"Stop warning me about that" ~variable:dont_bother_me t in + + Tk.pack [l]; + Tk.pack [b;b';b''] ~side:`Left; + Tk.pack [x] + +let make = fun mcu path target -> + let do_it = fun () -> + if switch mcu = 0 then + let command = Printf.sprintf "cd %s; make %s" path target in + Console.exec command in + + if mcu = Fbw && target <> "erase" then warn do_it else do_it () + diff --git a/sw/configurator/flasher.mli b/sw/configurator/flasher.mli new file mode 100644 index 0000000000..fc7cfcccbd --- /dev/null +++ b/sw/configurator/flasher.mli @@ -0,0 +1,30 @@ +(* + * $Id$ + * + * Micro-controllers uploading + * + * Copyright (C) 2004 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. + * + *) + +type mcu = Fbw | Ap | Modem +val make : mcu -> string -> string -> unit +(** [make mcu path target] Calls the "make" command in the given [path] with +the given [target]. [mcu] helps to control the hardware connectivity. *) diff --git a/sw/configurator/flightplan.ml b/sw/configurator/flightplan.ml new file mode 100644 index 0000000000..bb08e8db48 --- /dev/null +++ b/sw/configurator/flightplan.ml @@ -0,0 +1,78 @@ +(* + * $Id$ + * + * Flight plans edition + * + * Copyright (C) 2004 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 Latlong + +let calibrate_xy = fun x0 y0 -> + let scale = 2.5 in + + let print x y = + let xl = x0 + truncate (float x *. scale) + and yl = y0 + truncate (float y *. scale) in + let wgs84 = Latlong.wgs84_of_lambertIIe xl yl in + Console.write (Printf.sprintf "%d %d %f %f\n" x y ((Rad>>Deg)wgs84.Latlong.posn_lat) ((Rad>>Deg)wgs84.Latlong.posn_long)) in + + Console.write (Printf.sprintf "Calibration for x0=%d y0=%d:\n--8<----------------------\n" x0 y0); + + print 0 0; + print 1000 0; + print 0 1000; + Console.write (Printf.sprintf "--8<-----------------------\n") + +let calibrate_ign_tile = fun filename -> + Scanf.sscanf (Filename.basename filename) "F%3d_%3d" (fun x y -> + let x0 = x * 10000 + and y0 = (267 - y) * 10000 in + + calibrate_xy x0 y0) + +let int_of_tv = fun tv -> int_of_string (Textvariable.get tv) + + +let create_sheet = fun sheets -> + let f = Frame.create sheets in + Notebook.create_sheet sheets "Flight Plan" f; + + let b = Button.create ~text:"Calibrate IGN tile" ~command:(fun () -> Env.select_one_file ~filter:"*.png" calibrate_ign_tile) f in + + Tk.pack [b]; + + let tvx = Textvariable.create () + and tvy = Textvariable.create () in + + let lx = Label.create ~text:"LambertIIe: x:" f + and ex = Entry.create ~textvariable:tvx f + and ly = Label.create ~text:"y:" f + and ey = Entry.create ~textvariable:tvy f + and c = Button.create ~text:"Calibrate" ~command:(fun () -> calibrate_xy (int_of_tv tvx) (int_of_tv tvy)) f in + + Tk.pack ~side:`Left [lx]; + Tk.pack ~side:`Left [ex]; + Tk.pack ~side:`Left [ly]; + Tk.pack ~side:`Left [ey]; + Tk.pack ~side:`Left [c] + + diff --git a/sw/configurator/hardware.ml b/sw/configurator/hardware.ml new file mode 100644 index 0000000000..9adf8c7e67 --- /dev/null +++ b/sw/configurator/hardware.ml @@ -0,0 +1,260 @@ +(* + * $Id$ + * + * Microcotrollers connection checking + * + * Copyright (C) 2004 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. + * + *) + +module FbwMcu = struct + let name = "Fly by wire" + let sort = Flasher.Fbw + let path = Env.fbw_dir + let default_tty = Env.fbw_tty +end + +module ApMcu = struct + let name = "Autopilot" + let sort = Flasher.Ap + let path = Env.ap_dir + let default_tty = Env.ap_tty +end + +module ModemMcu = struct + let name = "Ground Modem" + let sort = Flasher.Modem + let path = Env.modem_dir + let default_tty = Env.modem_tty +end + +module type MCU = sig + val name : string + val sort : Flasher.mcu + val path : string + val default_tty : string +end + +let nb_adc = 8 + +let get_2bytes = fun buf i -> + (Char.code buf.[i] lsl 8) lor (Char.code buf.[i+1]) + +let (+.=) r v = r := !r +. v + +let lls = fun values l ()-> + let sum_x = ref 0. + and sum_y = ref 0. + and sum_xy = ref 0. + and sum_x2 = ref 0. + and n = ref 0 in + Hashtbl.iter + (fun x y -> + let x = float_of_string x + and y = float_of_string y in + incr n; + sum_x +.= x; + sum_x2 +.= x*.x; + sum_y +.= y; + sum_xy +.= x*.y) + values; + let n = float !n in + let mx = !sum_x /. n + and my = !sum_y /. n in + let c_xy = mx *. my +. (!sum_xy -. mx *. !sum_y -. my *. !sum_x) /. n + and s2_x = mx *. mx +. (!sum_x2 -. 2.*. mx *. !sum_x) /. n in + let a = c_xy /. s2_x in + let b = my -. a *. mx in + + Label.configure ~text:(Printf.sprintf "a=%f b=%f" a b) l + +let rec check_adcs = fun button w tty make () -> + let text = Tk.cget button `Text in + make "TARGET=tx_adcs load" (); + + let f = Frame.create w in + let destroy = fun () -> + Tk.destroy f; + Button.configure ~text button; + Button.configure ~command:(check_adcs button w tty make) button in + + Button.configure ~text:"Close" button; + Button.configure ~command:destroy button; + + let create_channel = fun i -> + let fc = Frame.create f in + + let value = Textvariable.create () in + + let l = Label.create ~text:(string_of_int i) fc + and v = Label.create ~textvariable:value ~width:4 fc in + + let e = Entry.create ~width:4fc in + let lb = Menubutton.create ~text:"Registered" fc in + let lb_menu = Menu.create lb in + Menubutton.configure lb ~menu:lb_menu; + + let values = Hashtbl.create 97 in (* I would prefer to find the values in the Menu but I do not know how to do it ! *) + let register_values = fun ev -> + let adc_value = Textvariable.get value + and entry_value = Entry.get e in + Hashtbl.add values adc_value entry_value; + Menu.add_command lb_menu + ~label:(adc_value^":"^entry_value) + ~command:(fun () -> Hashtbl.remove values adc_value; Menu.delete ~first:`Active ~last:`Active lb_menu) + in + Tk.bind ~events:[`KeyPressDetail "Return"] ~action:register_values e; + + let lb' = Menubutton.create ~text:"Fit" fc in + let lb_menu' = Menu.create lb' in + Menubutton.configure lb' ~menu:lb_menu'; + let result = Label.create ~width:20 fc in + Menu.add_command lb_menu' ~label:"Linear" ~command:(lls values result); + + + + Tk.pack [l; v] ~side:`Left; + Tk.pack [e] ~side:`Left; + Tk.pack [lb;lb'] ~side:`Left; + Tk.pack [result] ~side:`Left; + (fc, value) + in + + let channels = Array.init nb_adc create_channel in + + (* Listen to tty input *) + Tty.connect tty; + Tty.add_formatted_input tty "\000\000" (2*(nb_adc+1)+1) + (fun input -> + for i = 0 to nb_adc - 1 do + let vi = get_2bytes input (2+2*i) in + Textvariable.set (snd channels.(i)) (string_of_int vi) + done + ); + Tk.bind ~events:[`Destroy] ~action:(fun _ -> Tty.deconnect tty) f; + + (***) + let x = Button.create ~text:"Random" + ~command:(fun () -> + for i = 0 to nb_adc - 1 do + Textvariable.set (snd channels.(i)) (string_of_int (Random.int 1024)) + done + ) f in + (***) + + + Tk.pack [x]; + Tk.pack (List.map fst (Array.to_list channels)) ~side:`Top; + + Tk.pack [f] + + +module Make(Mcu : MCU) = struct + let make = fun target () -> Flasher.make Mcu.sort Mcu.path target + let make_test = fun target () -> Flasher.make Mcu.sort (Mcu.path^"/test") target + + let tty = Textvariable.create () + + let connected = Textvariable.create () + + let get_tty = fun () -> Textvariable.get tty + + let uart = ref Unix.stdin + + let connect_tty = fun () -> + let tty = get_tty ()in + if Textvariable.get connected = "1" then + try + Tty.connect tty + with _ -> + Console.write ~tags:["red"] (Printf.sprintf "Cannot open '%s'\n" tty) + else + try + Tty.deconnect tty + with + Not_found -> + Console.write ~tags:["red"] (Printf.sprintf "Device '%s' not opened\n" tty) + + let log_tty = fun () -> Tty.add_ttyinput (get_tty ()) Console.write + + let check_adcs_b = ref (Button.create Widget.default_toplevel) + + let create = fun parent -> + let f = Frame.create ~borderwidth:4 ~relief:`Sunken parent in + let f' = Frame.create f in + + let l = Label.create ~text:Mcu.name f' + and b0 = Button.create ~text:"Erase" ~command:(make "erase") f' + and b1 = Button.create ~text:"Check link" ~command:(make "check_arch") f' + and b2 = Button.create ~text:"Write Fuses" ~command:(make "wr_fuses") f' + and b3 = Button.create ~text:"Check UART" ~command:(fun () -> make_test (Printf.sprintf "TARGET=check_uart TTY=\"%s\" load" (Textvariable.get tty)) ()) f' + and e = Entry.create ~textvariable:tty ~width:10 f' + and c = Checkbutton.create ~text:"Connect tty" ~variable:connected ~command:connect_tty f' + and c' = Checkbutton.create ~text:"Log tty" ~command:log_tty f' in + + Button.configure b3; + + Entry.insert e ~index:`End ~text:Mcu.default_tty; + + check_adcs_b := Button.create ~text:"Check ADCS" f; + Button.configure !check_adcs_b ~command:(fun () -> check_adcs !check_adcs_b f (get_tty ()) make_test ()); + + + Tk.pack [l]; + Tk.pack [b0;b1;b2] ~side:`Left; + Tk.pack [e] ~side:`Left; + Tk.pack [c;c'] ~side:`Left; + Tk.pack [b3] ~side:`Left; + Tk.pack [f']; + Tk.pack [!check_adcs_b] ~side:`Left; + f +end + +module Fbw = Make(FbwMcu) +module Ap = Make(ApMcu) +module Modem = struct + include Make(ModemMcu) + + let bat_a = Textvariable.create () + let bat_b = Textvariable.create () +end + +let create_sheet = fun sheets -> + let f = Frame.create sheets in + Notebook.create_sheet sheets "Hardware" f; + + let fbw = Fbw.create f + and ap = Ap.create f + and modem = Modem.create f in + + Button.configure ~state:`Disabled !Modem.check_adcs_b; + + let check_spi = Button.create ~text:"Check SPI" ~state:`Disabled ap in + + + let check_downlink = Button.create ~text:"Check Downlink" ~state:`Disabled modem in + + let label = Label.create ~text:"Check the connectivity of your micro-controllers" f in + + Tk.pack ~anchor:`W [check_spi] ~side:`Left; + Tk.pack ~anchor:`W [check_downlink]; + Tk.pack ~anchor:`W [label]; + Tk.pack ~anchor:`W [fbw; ap; modem] + diff --git a/sw/configurator/infrared.ml b/sw/configurator/infrared.ml new file mode 100644 index 0000000000..79d07ed9dd --- /dev/null +++ b/sw/configurator/infrared.ml @@ -0,0 +1,3 @@ +let create_sheet = fun sheets xml -> + let airframe = Frame.create sheets in + Notebook.create_sheet sheets "Infrared" airframe diff --git a/sw/configurator/logalizer.ml b/sw/configurator/logalizer.ml new file mode 100644 index 0000000000..b9b60f5c27 --- /dev/null +++ b/sw/configurator/logalizer.ml @@ -0,0 +1,29 @@ +(* + * $Id$ + * + * Logs handling + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets -> + let airframe = Frame.create sheets in + Notebook.create_sheet sheets "Logalizer" airframe diff --git a/sw/configurator/main.ml b/sw/configurator/main.ml new file mode 100644 index 0000000000..f4060894ba --- /dev/null +++ b/sw/configurator/main.ml @@ -0,0 +1,56 @@ +(* + * $Id$ + * + * Configuration graphic interface + * + * Copyright (C) 2004 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. + * + *) + +let exit = fun () -> + if Dialog.create Widget.default_toplevel "Configurator quit" "Really quit ?\n" ["OK"; "Cancel"] () = 0 then + exit 0 + +let _ = + let top = Widget.default_toplevel in + Wm.title_set top "The Paparazzi Configurator"; + + let sheets = Frame.create top in + + Welcome.create_sheet sheets; + Hardware.create_sheet sheets; + Radio.create_sheet sheets; + Airframe.create_sheet sheets; + Flightplan.create_sheet sheets; + Upload.create_sheet sheets; + Simulator.create_sheet sheets; + Monitor.create_sheet sheets; + Logalizer.create_sheet sheets; + + let quit_button = Button.create ~relief:`Sunken ~text:"Quit" ~command:exit sheets + and (n, _) = Grid.size sheets in + Tk.grid ~column:n ~row:0 [quit_button]; + + let console = Console.create top in + + Tk.pack [sheets]; + Tk.pack [console]; + + Tk.mainLoop() diff --git a/sw/configurator/medit.ml b/sw/configurator/medit.ml new file mode 100644 index 0000000000..f0305607ea --- /dev/null +++ b/sw/configurator/medit.ml @@ -0,0 +1,537 @@ +(* ocamlc -I +lablgtk2 lablgtk.cma lablgnomecanvas.cma gtkInit.cmo medit.ml *) + +open Printf +open Latlong + +let sof = string_of_float +let fos = float_of_string +let sof1 = fun x -> Printf.sprintf "%.1f" x + +(* World (0., 0.) seems to be at the center of the canvas *) + +type meter = float +type en = { east : meter; north : meter } + +let flight_plan = ref (Xml.PCData "") + +module Ref = struct + let world_unit = 2.5 (* 1 pixel = 2.5m *) + let ref0 = ref {posn_lat = 43.237535; posn_long = 1.327747 } + let utm0 () = utm_of WGS84 !ref0 + + let set = fun lat lon -> + ref0 := { posn_lat = (Deg>>Rad) lat; posn_long = (Deg>>Rad) lon } + + + let world_of_en = fun en -> en.east /. world_unit, -. en.north /. world_unit + let en_of_world = fun wx wy -> { east = wx *. world_unit; north = -. wy *. world_unit } + let world_of_utm = fun utm -> + let utm0 = utm0 () in + let x = utm.utm_x -. utm0.utm_x + and y = -. (utm.utm_y -. utm0.utm_y) in + (x /. world_unit, y /. world_unit) + + + let geo_string = fun en -> + let u = utm_of WGS84 !ref0 in + let u' = {utm_x = u.utm_x +. en.east; + utm_y = u.utm_y +. en.north; + utm_zone = u.utm_zone } in + let w = of_utm WGS84 u' in + sprintf "%.4f %.4f" ((Rad>>Deg)w.posn_lat) ((Rad>>Deg)w.posn_long) + + let direction = fun w1 w2 -> + let u1 = utm_of WGS84 w1 + and u2 = utm_of WGS84 w2 in + assert (u1.utm_zone = u2.utm_zone); + (u2.utm_x -. u1.utm_x, u2.utm_y -. u1.utm_y) + + let world_of_wgs84 = fun wgs84 -> + let (dxm, dym) = direction !ref0 wgs84 in + (dxm/. world_unit, -. dym/.world_unit) +end + +let gensym = let n = ref 0 in fun prefix -> incr n; prefix ^ string_of_int !n + + +class type w = object + method alt : float + method name : string + method en : en + method zoom : float -> unit + method delete : unit +end + + +module Waypoints = struct + let waypoints = Hashtbl.create 13 + let add = fun w -> Hashtbl.add waypoints (w:>w) (w:>w) + let remove = fun x -> Hashtbl.remove waypoints x + let iter = fun f -> Hashtbl.iter f waypoints + let clear = fun () -> Hashtbl.iter (fun w _ -> w#delete) waypoints +end + +module RecentFiles = struct + let conf_file = Filename.concat (Sys.getenv "HOME") ".meditrc" + type f = Map of string | FP of string + let l = ref [] + let add = fun f -> + if not (List.mem f !l) then + l := f :: !l + let f_of_xml = fun x -> + let f = Xml.attrib x "file" in + match Xml.tag x with + "map" -> Map f + | "fp" -> FP f + | _ -> failwith "RecentFile.f_of_xml" + let xml_of_f = function + Map f -> Xml.Element ("map", ["file",f], []) + | FP f -> Xml.Element ("fp", ["file",f], []) + let load = fun () -> + let xml = try Xml.parse_file conf_file with _ -> Xml.Element ("",[],[]) in + l := List.map f_of_xml (Xml.children xml) + let save = fun () -> + let f = open_out conf_file in + let xml = Xml.Element ("files", [], List.map xml_of_f !l) in + output_string f (Xml.to_string_fmt xml); + close_out f + let iter = fun f -> List.iter f !l +end + +let s = 5. +let losange = [|s;0.; 0.;s; -.s;0.; 0.;-.s|] + +let georef = fun () -> + let dialog = GWindow.window ~border_width:10 ~title:"Geo ref" () in + let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in + let lat = GEdit.entry ~text:"43.210" ~packing:dvbx#add () in + let lon = GEdit.entry ~text:"1.234" ~packing:dvbx#add () in + let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in + let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in + ignore(cancel#connect#clicked ~callback:dialog#destroy); + ignore(ok#connect#clicked ~callback: + begin fun _ -> + let lat = float_of_string lat#text in + let lon = float_of_string lon#text in + Ref.set lat lon; + dialog#destroy () + end); + dialog#show () + +let current_zoom = ref 1. (* Would be better not to be global ??? *) + + +class waypoint = fun root (name :string) ?(alt=0.) en -> + let xw, yw = Ref.world_of_en en in + object (self) + val mutable x0 = 0. + val mutable y0 = 0. + val item = + GnoCanvas.polygon root ~points:losange + ~props:[`FILL_COLOR "red" ; `OUTLINE_COLOR "midnightblue" ; `WIDTH_UNITS 1.; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] + + val label = GnoCanvas.text root ~props:[`TEXT name; `X s; `Y 0.; `ANCHOR `SW] + val mutable name = name + val mutable alt = alt + initializer self#move xw yw + method name = name + method set_name n = + if n <> name then + name <- n + method alt = alt + method label = label + method xy = let a = item#i2w_affine in (a.(4), a.(5)) (*** item#i2w 0. 0. causes Seg Fault !***) + method move dx dy = item#move dx dy; label#move dx dy + method edit = + let dialog = GWindow.window ~border_width:10 ~title:"Waypoint Edit" () in + let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in + let en = self#en in + let ename = GEdit.entry ~text:name ~packing:dvbx#add () in + let ex = GEdit.entry ~text:(string_of_float en.east) ~packing:dvbx#add () in + let ey = GEdit.entry ~text:(string_of_float en.north) ~packing:dvbx#add () in + let ea = GEdit.entry ~text:(string_of_float alt) ~packing:dvbx#add () in + let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in + let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in + ignore(cancel#connect#clicked ~callback:dialog#destroy); + ignore(ok#connect#clicked ~callback: + begin fun _ -> + self#set_name ename#text; + alt <- float_of_string ea#text; + label#set [`TEXT name]; + self#set {east = float_of_string ex#text; + north = float_of_string ey#text}; + dialog#destroy () + end); + dialog#show () + + + + method event (ev : GnoCanvas.item_event) = + begin + match ev with + | `BUTTON_PRESS ev -> + begin + match GdkEvent.Button.button ev with + | 1 -> self#edit + | 3 -> self#delete + | 2 -> + let x = GdkEvent.Button.x ev + and y = GdkEvent.Button.y ev in + x0 <- x; y0 <- y; + let curs = Gdk.Cursor.create `FLEUR in + item#grab [`POINTER_MOTION; `BUTTON_RELEASE] curs + (GdkEvent.Button.time ev) + | x -> printf "%d\n" x; flush stdout; + end + | `MOTION_NOTIFY ev -> + let state = GdkEvent.Motion.state ev in + if Gdk.Convert.test_modifier `BUTTON2 state then begin + let x = GdkEvent.Motion.x ev + and y = GdkEvent.Motion.y ev in + let dx = !current_zoom *. (x-. x0) + and dy = !current_zoom *. (y -. y0) in + self#move dx dy ; + x0 <- x; y0 <- y + end + | `BUTTON_RELEASE ev -> + if GdkEvent.Button.button ev = 2 then + item#ungrab (GdkEvent.Button.time ev) + | _ -> () + end; + true + initializer ignore(item#connect#event self#event) + method item = item + method en = + let (dx, dy) = self#xy in + Ref.en_of_world dx dy + method set en = + let (xw, yw) = Ref.world_of_en en + and (xw0, yw0) = self#xy in + self#move (xw-.xw0) (yw-.yw0) + method delete = + item#destroy (); + label#destroy (); + Waypoints.remove (self:>w) + method zoom (z:float) = + let a = item#i2w_affine in + a.(0) <- 1./.z; a.(3) <- 1./.z; + item#affine_absolute a; + label#affine_absolute a + end + +let xml_of_wp = fun utm0 w -> + let wgs84 = of_utm WGS84 { utm_x = utm0.utm_x +. w#en.east; utm_y = utm0.utm_y +. w#en.north ; utm_zone = utm0.utm_zone } in + let alt = if w#alt = 0. then [] else ["alt", string_of_float w#alt] in + Xml.Element ("waypoint", ["name",w#name; + "x",sof1 w#en.east; + "y", sof1 w#en.north; + "lat",string_of_float ((Rad>>Deg) wgs84.posn_lat); + "lon",string_of_float ((Rad>>Deg) wgs84.posn_long)]@alt + ,[]) + +let subst_waypoints = fun xml wpts -> + match xml with + Xml.Element (tag, attrs, children) -> + Xml.Element (tag, attrs, List.map (fun c -> if Xml.tag c = "waypoints" then wpts else c) children) + | _ -> failwith "subst_waypoints" + + + +let file_dialog ~title ~callback () = + let sel = GWindow.file_selection ~title ~filename:"*.xml" ~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 write_mission = fun () -> + let l = ref [] in + Waypoints.iter (fun _ w -> l := w :: !l); + let utm0 = (Ref.utm0 ()) in + let children = List.map (xml_of_wp utm0) !l in + let waypoints = Xml.Element ("waypoints", ["utm_x0", sof1 utm0.utm_x;"utm_y0", sof1 utm0.utm_y], children) in + let fp = subst_waypoints !flight_plan waypoints in + + ignore (file_dialog ~title:"Save Flight Plan" ~callback:(fun name -> let f = open_out name in + fprintf f "%s\n" (Xml.to_string_fmt fp); + fprintf f "\n"; + close_out f ) ()) + +let float_attrib = fun x a -> float_of_string (Xml.attrib x a) +let int_attrib = fun x a -> truncate (float_attrib x a) + +let load_mission = fun root file -> + let xml = Xml.parse_file file in + + let default_alt = float_attrib xml "alt" in + + Ref.set (float_attrib xml "lat0") (float_attrib xml "lon0"); + let utm0 = Ref.utm0 () in + let wps = ExtXml.child xml "waypoints" in + + let wp_of_xml = fun xml -> + let a = try float_attrib xml "alt" with _ -> default_alt in + let en = + try + let lat = float_attrib xml "lat" + and lon = float_attrib xml "lon" in + let utm = utm_of WGS84 {posn_lat=(Deg>>Rad)lat; posn_long=(Deg>>Rad)lon} in + {east = (utm.utm_x -. utm0.utm_x); north = (utm.utm_y -. utm0.utm_y) } + with + Xml.No_attribute _ -> + { east= float_attrib xml "x"; north= float_attrib xml "y" } in + let w = new waypoint root (Xml.attrib xml "name") ~alt:a en in + w#zoom !current_zoom; + w in + + List.iter (fun w -> Waypoints.add (w:>w)) (List.map wp_of_xml (Xml.children wps)); + + flight_plan := xml + + +let open_mission = fun cont root ?file () -> + match file with + None -> + ignore (file_dialog ~title:"Open Flight Plan" ~callback:(fun name -> load_mission root name; cont (RecentFiles.FP name)) ()) + | Some name -> load_mission root name; cont (RecentFiles.FP name) + + +let display_map = fun ?(scale = 1.) x y wgs84 map_name root -> + let image = GdkPixbuf.from_file map_name in + let p = GnoCanvas.pixbuf ~pixbuf:image ~props:[`ANCHOR `NW] root in + p#lower_to_bottom (); + let wx, wy = Ref.world_of_wgs84 wgs84 in + p#move (wx -. x*.scale) (wy -. y*.scale); + let a = p#i2w_affine in + a.(0) <- scale; a.(3) <- scale; + p#affine_absolute a; + p + +let load_map = fun root (maps_menu_fact:GMenu.menu GMenu.factory) ref0 filename -> + let register = fun pixbuf -> + let mi = maps_menu_fact#add_item filename in + ignore (mi#connect#activate (fun () -> pixbuf#destroy (); maps_menu_fact#menu#remove mi)) in + if Filename.check_suffix filename ".xml" then + let xml = Xml.parse_file filename in + let map_name = Filename.concat (Filename.dirname filename) (Xml.attrib xml "file") in + + match Xml.attrib xml "projection" with + "UTM" -> + let utm_zone = try int_of_string (Xml.attrib xml "utm_zone") with _ -> fprintf stderr "Warning: utm_zone attribute not specified in '%s'; default is 31\n" filename; flush stderr; 31 in + begin + match Xml.children xml with + p::_ -> + let utm_x = float_attrib p "utm_x" + and utm_y = float_attrib p "utm_y" + and x = float_attrib p "x" + and y = float_attrib p "y" + and scale = float_attrib xml "scale" /. Ref.world_unit in + let wgs84 = of_utm WGS84 {utm_x = utm_x; utm_y = utm_y; utm_zone = utm_zone} in + register (display_map ~scale x y wgs84 map_name root) + | _ -> failwith "Exactly one ref point please" + end + | _ -> failwith "Unknwown projection" + else + try + Scanf.sscanf (Filename.basename filename) "F%3d_%3d" (fun x y -> + let lbt0 = lambert_of lambertIIe ((NTF< failwith "XML or IGN tile expected" + + + + +let open_map = fun cont root maps_menu ?file () -> + match file with + None -> + file_dialog ~title:"Open Map" ~callback:(fun name -> ignore (load_map root maps_menu !Ref.ref0 name); cont (RecentFiles.Map name)) () + | Some name -> + ignore (load_map root maps_menu !Ref.ref0 name); + cont (RecentFiles.Map name) + + + +let create_wp = fun canvas xw yw () -> + let en = Ref.en_of_world xw yw in + let name = gensym "wp" in + let x = new waypoint canvas#root name en in + x#zoom !current_zoom; + Waypoints.add x + +let dragging = ref None + + +let canvas_button_release = fun (canvas:GnoCanvas.canvas) ev -> + let state = GdkEvent.Button.state ev in + if GdkEvent.Button.button ev = 2 then begin + dragging := None; + true + end else + false + +let canvas_button_press = fun (canvas:GnoCanvas.canvas) ev -> + let state = GdkEvent.Button.state ev in + if GdkEvent.Button.button ev = 1 && Gdk.Convert.test_modifier `CONTROL state then + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + let (xw, yw) = canvas#window_to_world xc yc in + (* Effective creation delayed to avoid a bug in lablgtk *) + ignore (GMain.Timeout.add 10 (fun () -> create_wp canvas xw yw (); false)); + true + else if GdkEvent.Button.button ev = 2 then + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + dragging := Some (xc, yc); + true + else begin + false + end + +let canvas_key_press = fun (canvas:GnoCanvas.canvas) ev -> + let (x, y) = canvas#get_scroll_offsets in + match GdkEvent.Key.keyval ev with + | k when k = GdkKeysyms._Up -> canvas#scroll_to x (y-20) ; true + | k when k = GdkKeysyms._Down -> canvas#scroll_to x (y+20) ; true + | k when k = GdkKeysyms._Left -> canvas#scroll_to (x-10) y ; true + | k when k = GdkKeysyms._Right -> canvas#scroll_to (x+10) y ; true + | _ -> false + +let display_coord = fun (canvas:GnoCanvas.canvas) lbl_xy lbl_geo ev -> + let xc = GdkEvent.Motion.x ev + and yc = GdkEvent.Motion.y ev in + let (xw, yw) = canvas#window_to_world xc yc in + let en = Ref.en_of_world xw yw in + let d = sqrt (en.east*.en.east +. en.north*.en.north) in + lbl_xy#set_text (sprintf "%.0fm %.0fm (d=%.0fm)\t" en.east en.north d); + lbl_geo#set_text (Ref.geo_string en); + begin + match !dragging with + Some (x0, y0 ) -> + let xc = GdkEvent.Motion.x ev in + let yc = GdkEvent.Motion.y ev in + let (x, y) = canvas#get_scroll_offsets in + canvas#scroll_to (x+truncate (x0-.xc)) (y+truncate (y0-.yc)) + | None -> () + end; + false + + +let main () = + let window = GWindow.dialog ~title: "Paparazzi" + ~border_width: 1 ~width:800 () in + let quit = fun () -> + RecentFiles.save (); + GMain.Main.quit (); exit 0 in + ignore (window#connect#destroy ~callback:quit); + + let menubar = GMenu.menu_bar ~packing:window#vbox#pack () in + + let adj = GData.adjustment + ~value:1. ~lower:0.05 ~upper:10. + ~step_incr:0.5 ~page_incr:1.0 ~page_size:1.0 () in + + let w = GEdit.spin_button ~adjustment:adj ~rate:0. ~digits:2 ~width:50 ~packing:window#vbox#add () in + + let frame = GBin.frame ~shadow_type:`IN ~height:500 ~width:700 ~packing:window#vbox#add () in + let canvas = GnoCanvas.canvas ~packing:frame#add () in + + canvas#set_center_scroll_region false ; + canvas#set_scroll_region (-2500.) (-2500.) 2500. 2500.; + + let zoom = fun value -> + canvas#set_pixels_per_unit value; + Waypoints.iter (fun _ w -> w#zoom value); + current_zoom := value + in + + ignore (adj#connect#value_changed (fun () -> zoom adj#value)); + + let root = canvas#root in + +(*** ignore (canvas#event#connect#button_press (canvas_button_press canvas)); ***) + + let factory = new GMenu.factory menubar in + let accel_group = factory#accel_group in + let file_menu = factory#add_submenu "File" + and insert_menu = factory#add_submenu "Insert" + and maps_menu = factory#add_submenu "Maps" in + let file_menu_fact = new GMenu.factory file_menu ~accel_group + and maps_menu_fact = new GMenu.factory maps_menu ~accel_group + and insert_menu_fact = new GMenu.factory insert_menu ~accel_group in + + let register_recent_file = fun f -> + RecentFiles.add f; + match f with + RecentFiles.Map f -> + ignore (file_menu_fact#add_item f ~callback:(open_map (fun _ -> ()) root maps_menu_fact ~file:f)) + | RecentFiles.FP f -> + ignore (file_menu_fact#add_item f ~callback:(open_mission (fun _ -> ()) root ~file:f)) in + + ignore (file_menu_fact#add_item "Open Flight Plan" ~key:GdkKeysyms._O ~callback:(open_mission register_recent_file root)); + ignore (file_menu_fact#add_item "Open Map" ~key:GdkKeysyms._M ~callback:(open_map register_recent_file root maps_menu_fact)); + ignore (file_menu_fact#add_item "Write Flight Plan" ~key:GdkKeysyms._S ~callback:write_mission); + ignore (file_menu_fact#add_item "Manual Ref" ~callback:georef); + ignore (file_menu_fact#add_item "Clear Waypoints" ~key:GdkKeysyms._C ~callback:Waypoints.clear); + ignore (file_menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit); + ignore (file_menu_fact#add_separator ()); + + RecentFiles.load (); + + RecentFiles.iter register_recent_file; + + ignore (insert_menu_fact#add_item "Waypoint" ~key:GdkKeysyms._W ~callback:(create_wp canvas 0. 0.)); + + + let bottom = GPack.hbox ~packing:window#vbox#add () in + let lbl_xy = GMisc.label ~packing:bottom#pack () in + let lbl_geo = GMisc.label ~packing:bottom#pack () in + + ignore (canvas#event#connect#motion_notify (display_coord canvas lbl_xy lbl_geo)); + ignore (canvas#event#connect#button_press (canvas_button_press canvas)); + ignore (canvas#event#connect#button_release (canvas_button_release canvas)); + ignore (canvas#event#connect#after#key_press (canvas_key_press canvas)) ; + ignore (canvas#event#connect#enter_notify (fun _ -> canvas#misc#grab_focus () ; false)); + + window#add_accel_group accel_group; + window#show (); + + let port = ref 2010 + and domain = ref "127.255.255.255" in + Arg.parse + [ "-b", Arg.Int (fun x -> port := x), "\tDefault is 2010, unused if IVYBUS is set"; + "-domain", Arg.String (fun x -> domain := x), "\tDefault is 127.255.255.255, unused if IVYBUS is set"] + (fun x -> prerr_endline ("Don't do anything with "^x)) + "Usage: "; + + let bus = + try Sys.getenv "IVYBUS" with + Not_found -> Printf.sprintf "%s:%d" !domain !port in + Ivy.init "medit" "READY" (fun _ _ -> ()); + Ivy.start bus; + + let plot_utm = + let last_plot = ref None in + fun (utm_x:float) (utm_y:float) -> + let utm_zone = (Ref.utm0 ()).utm_zone in + let (x, y) = Ref.world_of_utm {utm_x = utm_x; utm_y = utm_y; utm_zone = utm_zone } in + (match !last_plot with + None -> () + | Some (x', y') -> + ignore (GnoCanvas.line ~points:[|x;y;x';y'|] root)); + last_plot := Some (x,y) + in + + ignore (Ivy.bind (fun _ args -> plot_utm (fos args.(0)/.100.) (fos args.(1)/.100.)) "GPS +[0-9]* +([0-9]*) +([0-9]*)"); + + GMain.Main.main () + +let _ = main () + diff --git a/sw/configurator/monitor.ml b/sw/configurator/monitor.ml new file mode 100644 index 0000000000..e5c3811ccd --- /dev/null +++ b/sw/configurator/monitor.ml @@ -0,0 +1,29 @@ +(* + * $Id$ + * + * Real-time flight monitoring + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets -> + let airframe = Frame.create sheets in + Notebook.create_sheet sheets "Monitor" airframe diff --git a/sw/configurator/notebook.ml b/sw/configurator/notebook.ml new file mode 100644 index 0000000000..d26a8aaf69 --- /dev/null +++ b/sw/configurator/notebook.ml @@ -0,0 +1,62 @@ +(* + * $Id$ + * + * Facility to create tabs (thanx to JB) + * + * Copyright (C) 2004 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. + * + *) + + +let eval l = + let token x = Protocol.TkToken x in + Protocol.tkEval (Array.map token (Array.of_list l)) + +(* Float weights bug for grid *) +let set_weight orient w index weight = + ignore (eval ["grid"; orient ^ "configure"; Widget.name w; + string_of_int index; "-weight"; string_of_int weight]) + +let set_column_weight w = set_weight "column" w +let set_row_weight w = set_weight "row" w + +let conf_relief = fun b value -> + eval [Widget.name b; "config"; "-relief"; value] + +let create_sheet w name child = + let lower b = conf_relief b "sunken" + and upper b = conf_relief b "flat" in + let b = Button.create ~relief:`Sunken ~text:name w in + let cmd () = + let (n, _) = Grid.size w in + let slaves = Grid.slaves ~row:1 w in + if slaves <> [] then Grid.forget slaves; + List.iter (fun w -> ignore (lower w)) (Grid.slaves ~row:0 w); + ignore (upper b); + Tk.grid ~column:0 ~row:1 ~columnspan:n ~padx:20 (*** ~pady:20 ***) ~sticky:"news" [child] in + Button.configure ~borderwidth:1 ~command:cmd b; + let (n, _) = Grid.size w in + Tk.grid ~column:n ~row:0 ~sticky:"news" [b]; + set_column_weight w n 1; + if n = 0 then (cmd (); set_row_weight w 1 1) + else Grid.configure ~columnspan:(succ n) (Grid.slaves ~row:1 w) + + + diff --git a/sw/configurator/notebook.mli b/sw/configurator/notebook.mli new file mode 100644 index 0000000000..59e0d54586 --- /dev/null +++ b/sw/configurator/notebook.mli @@ -0,0 +1,28 @@ +(* + * $Id$ + * + * Facility to create tabs (thanx to JB) + * + * Copyright (C) 2004 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. + * + *) + +val create_sheet : 'a Widget.widget -> string -> 'b Widget.widget -> unit +(** [create_sheet parent name frame] adds the new [frame] tab *) diff --git a/sw/configurator/radio.ml b/sw/configurator/radio.ml new file mode 100644 index 0000000000..4e1ee08303 --- /dev/null +++ b/sw/configurator/radio.ml @@ -0,0 +1,188 @@ +(* + * $Id$ + * + * Radio Control transmitter calibration + * + * Copyright (C) 2004 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 + +let xml_radio = ref VarXml.empty + + +let zero = '\000' +let get_2bytes = fun buf i -> + (Char.code buf.[i] lsl 8) lor (Char.code buf.[i+1]) + +type channel = { + frame : Widget.frame Widget.widget; + update : int -> unit + } + +let soi = string_of_int +let ios = int_of_string + +let nb_neutral = 10 + +let ctl_values = [|"A";"B";"C";"D";"E";"F";"G";"H";"I"|] +let functions = [|"POWER";"ROLL";"PITCH";"DIRECTION";"MODE";"GAIN1";"GAIN2";"LLS";"CALIB"|] +let ctl_values = [|"A";"B";"C";"D";"E";"F";"G"|] +let functions = [|"POWER";"ROLL";"PITCH";"DIRECTION";"MODE";"GAIN1";"GAIN2"|] + +let nb_channels = Array.length ctl_values + + +let popup_entry = fun parent set -> + let t = Toplevel.create parent in + let e = Entry.create ~takefocus:true t in + let action = fun _ -> set (Entry.get e); Tk.destroy t in + Tk.bind e ~events:[`KeyPressDetail "Return"] ~action:action; + let b = Button.create ~text:"ok" ~command:action t in + Tk.pack [e]; + Tk.pack [b] + +let list_button = fun parent values tv -> + let lb = Menubutton.create parent (* ~indicatoron:true *) ~textvariable:tv in + let lb_menu = Menu.create lb in + Menubutton.configure lb ~menu:lb_menu; + Array.iter + (fun v -> + Menu.add_command lb_menu ~label:v ~command:(fun _ -> Textvariable.set tv v)) + values; + Menu.add_command lb_menu ~label:"..." ~command:(fun _ -> popup_entry lb (fun x -> Textvariable.set tv x)); + lb + + + +let one_channel = fun parent i xml_channel -> + let frame = Frame.create ~relief:`Ridge ~borderwidth:1 ~width:5 parent in + let n = Label.create frame ~text:(string_of_int i) ~background:`Green in + + let xml_get = VarXml.attrib xml_channel in + let maximum = xml_get "max" + and neutral = xml_get "neutral" + and minimum = xml_get "min" in + + let rev = Textvariable.create () in + + let ctl = list_button frame ctl_values (xml_get "ctl") + and function_name = list_button frame functions (xml_get "function") in + let current = Label.create frame ~text:"1234" ~relief:`Sunken ~borderwidth:2; + and maxi = Entry.create ~width:4 frame ~textvariable:maximum; + and neutr = Entry.create ~width:4 frame ~textvariable:neutral; + and mini = Entry.create ~width:4 frame ~textvariable:minimum + and reverse = Checkbutton.create ~variable:rev frame + and average = Entry.create ~width:2 ~textvariable:(xml_get "average") frame in + + let max_or_min = fun a b -> + if Textvariable.get rev = "1" then min a b else max a b + and min_or_max = fun a b -> + if Textvariable.get rev = "1" then max a b else min a b in + + + Tk.pack [n] ~side:`Top; + Tk.pack [ctl; function_name] ~side:`Top; + Tk.pack [current] ~side:`Top; + Tk.pack [maxi; neutr; mini] ~side:`Top; + Tk.pack [reverse] ~side:`Top; + Tk.pack [average] ~side:`Top; + + let get_ma = fun () -> ios (Textvariable.get maximum) + and get_mi = fun () -> ios (Textvariable.get minimum) in + + let update_max = fun v -> + let ma = max_or_min (get_ma ()) v in + Textvariable.set maximum (soi ma) + and update_min = fun v -> + let mi = min_or_max (get_mi ()) v in + Textvariable.set minimum (soi mi) in + + let sum_neutral = ref 0 and cpt_neutral = ref 1 in + + Tk.bind ~events:[`ButtonPress] ~action:(fun _ -> Textvariable.set maximum "00000") maxi; + Tk.bind ~events:[`ButtonPress] ~action:(fun _ -> Textvariable.set minimum "99999") mini; + Tk.bind ~events:[`ButtonPress] ~action:(fun _ -> cpt_neutral := 0; sum_neutral := 0) neutr; + + + let update = fun value -> + Label.configure current ~text:(string_of_int value); + update_max value; + update_min value; + if !cpt_neutral < nb_neutral then begin + incr cpt_neutral; + sum_neutral := !sum_neutral + value; + Textvariable.set neutral (soi (!sum_neutral/ !cpt_neutral)); + end + in + + { frame = frame; update = update};; + + + +let program_board = fun w cs () -> + Flasher.make Flasher.Fbw (Printf.sprintf "%s/test" Env.fbw_dir) "TARGET=rc_transmitter load"; + + let tty = Hardware.Fbw.get_tty () in + Tty.connect tty; + Tty.add_formatted_input tty "\000\000" (2*(nb_channels+1)+1) + (fun input -> + for i = 0 to String.length input -1 do + printf "0x%X " (Char.code input.[i]); + done; + print_newline (); + let channel_values = Array.init nb_channels (fun s -> get_2bytes input (2+2*s)) in + for i = 0 to nb_channels - 1 do + cs.(i).update channel_values.(i) + done); + Tk.bind ~events:[`Destroy] ~action:(fun _ -> Tty.deconnect tty) w + + + +let create_display_frame = fun nf xml_radio -> + let xml_channels = Array.of_list (VarXml.children xml_radio) in + let cs = Array.mapi (fun i xml -> one_channel nf i xml) xml_channels in + + let p = Button.create ~text:"Get Radio-Control Values" ~command:(program_board nf cs) nf in + + Tk.pack [p]; + + let legend = Frame.create nf in + let label = fun s -> Label.create legend ~text:s in + Tk.pack ~anchor:`E [label "Channel: "; + label "Control name: "; + label "Function name: "; + label "Current value: "; + label "Max (click to reset): "; + label "Neutral (click to reset): "; + label "Min (click to reset): "; + label "Reverse"; + label "Averaged"] ~side:`Top; + + + Tk.pack (legend::List.map (fun c -> c.frame) (Array.to_list cs)) ~side:`Left + + +let create_sheet = fun sheets -> + let top = Frame.create sheets in + Notebook.create_sheet sheets "Radio" top; + + TkXml.create top create_display_frame diff --git a/sw/configurator/servos.ml b/sw/configurator/servos.ml new file mode 100644 index 0000000000..17838cf444 --- /dev/null +++ b/sw/configurator/servos.ml @@ -0,0 +1,136 @@ +(* + * $Id$ + * + * Servos calibration + * + * Copyright (C) 2004 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. + * + *) + +let servo_update_period = 500 (* ms *) + +let default_max = 2000 +let default_min = 1000 +let max_max = 2500. +let min_min = 500. +let default_neutral = 1500 + +let soi = string_of_int +let ios = int_of_string +let fos = float_of_string + +let output_2bytes = fun tty x -> + prerr_endline (string_of_int x); + Tty.write_byte tty ((x land 0xff00) asr 8); + Tty.write_byte tty (x land 0xff) + +type selected = { widget : Widget.label Widget.widget; channel : int } + +type channel = { frame : Widget.frame Widget.widget; print : out_channel -> unit } + +let one_channel = fun _i parent slider selected servo_xml -> + let frame = Frame.create parent in + let no = Textvariable.get (VarXml.attrib servo_xml "no") in + let n = Label.create frame ~text:no ~background:`Green in + + let create_label = fun id -> + let v = VarXml.attrib servo_xml id in + let l = Label.create frame ~textvariable:v in + + let button_action = fun _event -> + Label.configure l ~relief:`Sunken; + begin + match !selected with + None -> () + | Some s -> Label.configure s.widget ~relief:`Flat + end; + selected := Some { widget=l; channel=int_of_string no }; + + Scale.set slider (fos (Textvariable.get v)); + Scale.configure ~command:(fun value -> Textvariable.set v (string_of_int (truncate value))) slider in + + Tk.bind ~events:[`ButtonPress] ~action:button_action l; + l in + + let maximum = create_label "max" + and neutral = create_label "neutral" + and minimum = create_label "min" in + + Tk.pack [n; maximum; neutral; minimum] ~side:`Top; + frame + + +let rec send_selected_value = fun selected sending_state tty -> + if Textvariable.get sending_state = "1" then begin + Timer.set servo_update_period (fun () -> send_selected_value selected sending_state tty); + match !selected with + Some s -> + Tty.write_byte tty 0; + Tty.write_byte tty s.channel; + output_2bytes tty (ios (Tk.cget s.widget `Text)); + Tty.write tty "\n"; + Tty.flush tty + | None -> () + end + +let create_sheet = fun sheets airframe_xml -> + let top = Frame.create sheets in + Notebook.create_sheet sheets "Servos" top; + + let tf = Frame.create ~relief:`Ridge ~borderwidth:1 top in + + let channels = Frame.create top in + + let sending_state = Textvariable.create () + and selected = ref None in + + let sending = fun () -> + if Textvariable.get sending_state = "1" then begin + let fbw_tty = Textvariable.get Hardware.Fbw.tty in + Console.write (Printf.sprintf "Sending to %s\n" fbw_tty); + Tty.connect fbw_tty; + send_selected_value selected sending_state fbw_tty + end else + Tty.deconnect (Textvariable.get Hardware.Fbw.tty) + in + + let program = fun () -> + Flasher.make Flasher.Fbw (Printf.sprintf "%s/test" Env.fbw_dir) "TARGET=setup_servos load" in + + let legend = Frame.create channels + and slider = Scale.create tf ~orient:`Horizontal ~min:min_min ~max:max_max ~digits:4 ~label:"micro-seconds" ~resolution:1. ~length:300 + and pb = Button.create ~text:"Program board to send" ~command:program tf + and rb = Checkbutton.create ~text:"Sending" ~variable:sending_state ~command:sending tf in + let label = fun s -> Label.create legend ~text:s in + Tk.pack [label "Channel: "; + label "Max (click to select): "; + label "Neutral (click to select): "; + label "Min (click to select): "] ~side:`Top; + + + let servos_xml = Array.of_list (VarXml.children (VarXml.child airframe_xml "servos")) in + + let cs = Array.mapi (fun i s -> one_channel i channels slider selected s) servos_xml in + + Tk.pack (legend::Array.to_list cs) ~side:`Left; + Tk.pack [tf]; + Tk.pack [pb]; Tk.pack [rb]; + Tk.pack [slider]; + Tk.pack [channels] diff --git a/sw/configurator/simulator.ml b/sw/configurator/simulator.ml new file mode 100644 index 0000000000..6e5ee78377 --- /dev/null +++ b/sw/configurator/simulator.ml @@ -0,0 +1,29 @@ +(* + * $Id$ + * + * Hardware in the loop simulator + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets -> + let airframe = Frame.create sheets in + Notebook.create_sheet sheets "Simulator" airframe diff --git a/sw/configurator/tkXml.ml b/sw/configurator/tkXml.ml new file mode 100644 index 0000000000..da91e93960 --- /dev/null +++ b/sw/configurator/tkXml.ml @@ -0,0 +1,86 @@ +(* + * $Id$ + * + * Binding of a widget to an XML file + * + * Copyright (C) 2004 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. + * + *) + +let create_display_frame = fun f xml appli -> + let tf = Frame.create f + and af = Frame.create f in + + let l = Label.create ~text:"Name:" tf + and name = Entry.create ~textvariable:(VarXml.attrib xml "name") tf in + + let destroy = fun () -> + Tk.destroy tf; + Tk.destroy af; + Grab.release f in + + let save = fun file -> + Console.write (Printf.sprintf "Saving to \"%s\"\n" file); + let s = Xml.to_string_fmt (VarXml.to_xml xml) in + let f = open_out file in + output_string f s; + close_out f; + destroy () in + + let save_button = Button.create ~text:"Save Config" ~command:(fun _ -> Env.select_one_file save) tf + and cancel_button = Button.create ~text:"Cancel" ~command:destroy tf in + + Tk.pack [l] ~side:`Left; + Tk.pack [name] ~side:`Left; + Tk.pack [save_button; cancel_button] ~side:`Left; + Tk.pack ~anchor:`N [tf]; + + Grab.set f; + + Tk.pack [af]; + + appli af xml + + + +let create = fun top appli -> + let f = Frame.create top in + let load = fun file -> + Console.write (Printf.sprintf "Reading from \"%s\"\n" file); + + create_display_frame f (VarXml.of_xml (Xml.parse_file file)) appli in + let load_button = Button.create ~text:"Load Config" ~command:(fun _ -> Env.select_one_file load) top in + + Tk.pack [load_button]; + Tk.pack [f] + + +let create_section = fun f xml -> + Tk.grid ~column:0 ~row:0 [Label.create ~text:"Name" f]; + Tk.grid ~column:1 ~row:0 [Label.create ~text:"Value" f]; + + let r = ref 0 in + List.iter + (fun def -> + incr r; + Tk.grid ~column:0 ~row:!r [Label.create ~textvariable:(VarXml.attrib def "name") f]; + Tk.grid ~column:1 ~row:!r [Entry.create ~width:4 ~textvariable:(VarXml.attrib def "value") f]) + (VarXml.children xml) + diff --git a/sw/configurator/tkXml.mli b/sw/configurator/tkXml.mli new file mode 100644 index 0000000000..fda27bafdb --- /dev/null +++ b/sw/configurator/tkXml.mli @@ -0,0 +1,35 @@ +(* + * $Id$ + * + * Binding of a widget to an XML file + * + * Copyright (C) 2004 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. + * + *) + +val create : + 'a Widget.widget -> + (Widget.frame Widget.widget -> VarXml.xml -> unit) -> unit +(** [create parent action] Wraps the given [action] into an XML file handler. +The arguments of [action] are a graphic place and the opened XML object. +[action] may then modify the contents of the XML object and save the +modifications. Note that the interface is grabbed on the created subframe. *) + +val create_section : 'a Widget.widget -> VarXml.xml -> unit diff --git a/sw/configurator/tty.ml b/sw/configurator/tty.ml new file mode 100644 index 0000000000..8b7048651c --- /dev/null +++ b/sw/configurator/tty.ml @@ -0,0 +1,101 @@ +(* + * $Id$ + * + * Serial device handling + * + * Copyright (C) 2004 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. + * + *) + +let buffer_len = 256 + +let ttys = Hashtbl.create 7 +let registered = Hashtbl.create 7 + +let deconnect = fun tty -> + try + let fd = Hashtbl.find ttys tty in + Fileevent.remove_fileinput fd; + Unix.close fd; + Hashtbl.remove ttys tty + with + Not_found -> () + +let connect = fun tty -> + if Hashtbl.mem ttys tty then + deconnect tty; + let buffer = String.create buffer_len in + let fd = Serial.opendev tty Env.tty_rate in + let log = fun () -> + let n = Unix.read fd buffer 0 buffer_len in + let s = String.sub buffer 0 n in + List.iter (fun f -> f s) (Hashtbl.find_all registered tty) in + Fileevent.add_fileinput fd log; + Hashtbl.add ttys tty fd + + +let add_ttyinput = Hashtbl.add registered + +let add_formatted_input = fun tty prefix size f -> + if String.length prefix > size then raise (Invalid_argument "add_formatted_input"); + let buffer = String.create size + and idx = ref 0 in + let rec f' = fun s -> + let n = String.length s + and expected = size - !idx in + Printf.printf "%d " n; flush stdout; + let blitted = min expected n in + String.blit s 0 buffer !idx blitted; + idx := !idx + blitted; + if !idx = size then begin + if String.sub buffer 0 (String.length prefix) = prefix then begin + f buffer; + idx := 0 + end else begin (* Look for the first character of the prefix *) + try + let discarded = String.index_from buffer 1 prefix.[0] in + let kept = size - discarded in + String.blit buffer discarded buffer 0 kept; + idx := kept + with + Not_found -> (* prefix.[0] not found *) + idx := 0 + end + end; + let rest = n - blitted in + if rest > 0 then begin Printf.printf "r=%d\n" rest; flush stdout; f' (String.sub s blitted rest) end in + add_ttyinput tty f' + +let write = fun tty s -> + let fd = Hashtbl.find ttys tty in + let oc = Unix.out_channel_of_descr fd in + Printf.fprintf oc "%s" s; + flush oc + +let write_byte = fun tty b -> + let fd = Hashtbl.find ttys tty in + let oc = Unix.out_channel_of_descr fd in + output_byte oc b; + flush oc + +let flush = fun tty -> + let fd = Hashtbl.find ttys tty in + flush (Unix.out_channel_of_descr fd) + diff --git a/sw/configurator/tty.mli b/sw/configurator/tty.mli new file mode 100644 index 0000000000..90ae020fb3 --- /dev/null +++ b/sw/configurator/tty.mli @@ -0,0 +1,42 @@ +(* + * $Id$ + * + * Serial device handling + * + * Copyright (C) 2004 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. + * + *) + +val connect : string -> unit +val deconnect : string -> unit +(** Opens and closes the given device *) + +val add_ttyinput : string -> (string -> unit) -> unit +(** [add_ttyinput device cb] Attaches the callback [cb] to input events *) + +val add_formatted_input : string -> string -> int -> (string -> unit) -> unit +(** [add_formatted_input device prefix size cb] Same as [add_ttyinput] +but [cb] is called only with an input of length [size] starting with +[prefix]. Characters are discarded until [prefix] is found. *) + +val write : string -> string -> unit +val write_byte : string -> int -> unit +val flush : string -> unit +(** Output on the given device *) diff --git a/sw/configurator/upload.ml b/sw/configurator/upload.ml new file mode 100644 index 0000000000..4389e6564c --- /dev/null +++ b/sw/configurator/upload.ml @@ -0,0 +1,34 @@ +(* + * $Id$ + * + * Uploading flight control to micro-controllers + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets -> + let f = Frame.create sheets in + Notebook.create_sheet sheets "Upload" f; + + let b = Button.create ~text:"Program Fly by Wire MCU" ~command:(Hardware.Fbw.make "load") f + and b' = Button.create ~text:"Program Autopilot MCU" ~command:(Hardware.Ap.make "load") f in + + Tk.pack [b; b'] diff --git a/sw/configurator/varXml.ml b/sw/configurator/varXml.ml new file mode 100644 index 0000000000..cd41dc18b5 --- /dev/null +++ b/sw/configurator/varXml.ml @@ -0,0 +1,91 @@ +(* + * $Id$ + * + * Mutable XML representation based on TK Textvariable + * + * Copyright (C) 2004 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. + * + *) + +type xml = + | Element of (string * (string * Textvariable.textVariable) list * xml list) + | PCData of string + +let empty = PCData "Empty" + +let variable = fun s -> + let t = Textvariable.create () in + Textvariable.set t s; + t + +let rec of_xml = function + | Xml.Element (tag, attributes, children) -> + let varattrs = List.map (fun (a, v) -> (a, variable v)) attributes in + Element (tag, varattrs, List.map of_xml children) + | Xml.PCData string -> PCData string + +let rec to_xml = function + | Element (tag, attributes, children) -> + let varattrs = List.map (fun (a, v) -> (a, Textvariable.get v)) attributes in + Xml.Element (tag, varattrs, List.map to_xml children) + | PCData string -> Xml.PCData string + +let attrib xml a = + match xml with + | Element (_tag, attributes, _children) -> + List.assoc a attributes + | _ -> failwith "VarXml.attrib" + +let children xml = + match xml with + | Element (_tag, _attributes, children) -> children + | _ -> failwith "VarXml.children" + +let child xml ?select c = + let rec find = function + Element (tag, attributes, _children) as elt :: elts -> + if tag = c then + match select with + None -> elt + | Some p -> + if p attributes then elt else find elts + else + find elts + | _ :: elts -> find elts + | [] -> raise Not_found in + find (children xml) + + + +let slash = Str.regexp "/" + +let get = fun path attr root -> + let path = Str.split slash path in + let rec find = fun path attributes xmls -> + match path, xmls with + [], _ -> List.assoc attr attributes + | tag::tags, Element (tag', attributes', children)::elements -> + if tag = tag' then + find tags attributes' children + else + find path attributes elements + | tag::_, _ -> failwith ("VarXml.get "^tag) in + find path [] [root] + diff --git a/sw/configurator/varXml.mli b/sw/configurator/varXml.mli new file mode 100644 index 0000000000..bccae29ff2 --- /dev/null +++ b/sw/configurator/varXml.mli @@ -0,0 +1,37 @@ +(* + * $Id$ + * + * Mutable XML representation based on TK Textvariable + * + * Copyright (C) 2004 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. + * + *) + + +type xml +val empty : xml +val of_xml : Xml.xml -> xml +val to_xml : xml -> Xml.xml +val attrib : xml -> string -> Textvariable.textVariable +val children : xml -> xml list +val child : + xml -> + ?select:((string * Textvariable.textVariable) list -> bool) -> + string -> xml diff --git a/sw/configurator/welcome.ml b/sw/configurator/welcome.ml new file mode 100644 index 0000000000..7979492665 --- /dev/null +++ b/sw/configurator/welcome.ml @@ -0,0 +1,38 @@ +(* + * $Id$ + * + * Paparazzi welcome logo + * + * Copyright (C) 2004 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. + * + *) + +let create_sheet = fun sheets -> + let welcome = Frame.create sheets in + Notebook.create_sheet sheets "Welcome" welcome; + + let l = Label.create ~text:"WELCOME TO THE PAPARAZZI CONFIGURATOR" welcome + in let img = Imagephoto.create() in + Imagephoto.configure img ~file:(Env.configurator_dir^"/penguin.gif") ~format:"gif"; + let l1 = Label.create ~image:img welcome + in Tk.pack [l; l1]; + + + diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile new file mode 100644 index 0000000000..41eeaa713b --- /dev/null +++ b/sw/ground_segment/cockpit/Makefile @@ -0,0 +1,35 @@ +OCAMLC=ocamlc -g +OCAMLOPT=ocamlopt +INCLUDES=-I +lablgtk2 -I +camlimages -I ../../lib/ocaml +LIBS=glibivy-ocaml.cma lablgtk.cma ci_core.cma ci_png.cma ci_gif.cma ci_jpeg.cma ci_tiff.cma ci_bmp.cma ci_ppm.cma ci_ps.cma lib.cma lablgnomecanvas.cma xlib.cma +CMXA=$(LIBS:.cma=.cmxa) + +SRC = map2d.ml +CMO = $(SRC:.ml=.cmo) +CMX = $(SRC:.ml=.cmx) + +all : map2d.opt + + +map2d.out : $(CMO) + $(OCAMLC) $(INCLUDES) $(LIBS) gtkInit.cmo $(CMO) -o $@ + + +map2d.opt : $(CMX) + $(OCAMLOPT) str.cmxa unix.cmxa xml-light.cmxa $(INCLUDES) $(CMXA) gtkInit.cmx $(CMX) -o $@ + +map2d.run: + lablgtk2 str.cma unix.cma xml-light.cma -I +camlimages -I ../../lib/ocaml glibivy-ocaml.cma ci_core.cma ci_png.cma ci_gif.cma ci_jpeg.cma ci_tiff.cma ci_bmp.cma ci_ppm.cma ci_ps.cma lib.cma xlib.cma map2d.ml + + +.SUFFIXES: .ml .mli .cmo .cmi .cmx + +.ml.cmo: + $(OCAMLC) $(INCLUDES) -labels -w s -c $< +.mli.cmi: + $(OCAMLC) $(INCLUDES) -labels -w s -c $< +.ml.cmx: + $(OCAMLOPT) $(INCLUDES) -labels -w s -c $< + +clean: + rm -f *~* *.cm* *.o *.out *.opt diff --git a/sw/ground_segment/cockpit/Paparazzi/APPage.pm b/sw/ground_segment/cockpit/Paparazzi/APPage.pm new file mode 100644 index 0000000000..7f89b08ecc --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/APPage.pm @@ -0,0 +1,46 @@ +package Paparazzi::APPage; +use Paparazzi::NDPage; +@ISA = ("Paparazzi::NDPage"); +use strict; +use Subject; +use Data::Dumper; + +use constant TITLE => "Autopilot"; + +sub populate { + my ($self, $args) = @_; + $args->{-title} = TITLE; + $self->SUPER::populate($args); + $self->configspec( + -ap_status => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + ); +} + +sub ap_status { + my ($self, $old_val, $new_val) = @_; + return unless defined $new_val; +# print "in APPage ap_status\n".Dumper($new_val); + my $zinc = $self->get('-zinc'); + foreach my $field (keys %{$new_val}) { + $zinc->itemconfigure ($self->{'text_'.$field}, + -text => sprintf("%s : %.1f", $field, $new_val->{$field})) if defined $self->{'text_'.$field}; + } +} + +sub build_gui { + my ($self) = @_; + $self->SUPER::build_gui(); + my $zinc = $self->get('-zinc'); + my $dy = $self->get('-height')/10; + my $y=10; + my $x=10; + foreach my $field ('mode', 'h_mode', 'v_mode', 'target_climb', 'target_alt', 'target_heading') { + $self->{'text_'.$field} = $zinc->add('text', $self->{main_group}, + -position => [$x, $y+$self->{vmargin}], + -color => 'white', + -anchor => 'w', + -text => $field); + $y+=$dy; + } +} + diff --git a/sw/ground_segment/cockpit/Paparazzi/EnginePage.pm b/sw/ground_segment/cockpit/Paparazzi/EnginePage.pm new file mode 100644 index 0000000000..4d73b4aba4 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/EnginePage.pm @@ -0,0 +1,78 @@ +package Paparazzi::EnginePage; +use Paparazzi::NDPage; +@ISA = ("Paparazzi::NDPage"); +use strict; +use Subject; + +use Tk; +use Tk::Zinc; +use Math::Trig; +use Data::Dumper; + +use Paparazzi::Utils; +use Paparazzi::RotaryGauge; + +use constant TITLE => "Engine"; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $args->{-title} = TITLE; + $self->configspec( + -engine_status => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + ); +} + +sub engine_status { + my ($self, $old_val, $new_val) = @_; + my $zinc = $self->get('-zinc'); + foreach my $field (keys %{$new_val}) { + $self->{'gauge_'.$field}->configure( -value => $new_val->{$field}) if defined $self->{'gauge_'.$field}; + } +} + +sub build_gui { + my ($self) = @_; + $self->SUPER::build_gui(); + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + use constant GAUGES_PER_ROW => 3; + my @gauges = ( { name => 'throttle', format => "%.1f %%", extends => [0 , 101, 10, 20]}, + { name => 'rpm', format => "%.0f rpm", extends => [0 , 16, 1, 2]}, + { name => 'temp', format => "%.1f °C", extends => [-20, 80, 10, 20]}, + { name => 'bat', format => "%.1f V", extends => [6., 14., 0.5, 1.]}, + { name => 'amp', format => "%.1f A", extends => [0, 16, 1, 2] }, + { name => 'energy', format => "%.1f Wh", extends => [0, 50, 5, 10] }, + ); + my $margin = 5; + my $vmargin = 50; + my $gauge_spacing = ($width - 2 * $margin) / GAUGES_PER_ROW; + my $gauge_radius = $gauge_spacing / 2. * 0.8; + my $gauge_row = 0; + my $gauge_col = 0; + foreach my $gauge (@gauges) { + my $extends = $gauge->{extends}; + my $pos = [ $margin + ($gauge_col+0.1) * $gauge_spacing, $vmargin + $gauge_row * ($gauge_spacing + 30)]; + $self->{'gauge_'.$gauge->{name}} = Paparazzi::RotaryGauge->new(-zinc => $zinc, + -radius => $gauge_radius, + -origin => $pos, + -parent_grp => $self->{main_group}, + -min_val => $extends->[0], + -max_val => $extends->[1], + -tick_spacing => $extends->[2], + -legend_spacing => $extends->[3], + -min_val_angle => -90., + -dead_sector => 30., + -text => $gauge->{name}, + -format => $gauge->{format} + ); + $gauge_col++; + unless ($gauge_col lt GAUGES_PER_ROW) { + $gauge_row++; + $gauge_col = 0; + } + } +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/Geometry.pm b/sw/ground_segment/cockpit/Paparazzi/Geometry.pm new file mode 100644 index 0000000000..c27ab01858 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/Geometry.pm @@ -0,0 +1,44 @@ +package Paparazzi::Geometry; + +use Math::Trig; + +use constant PI_TWO => (pi / 2); +use constant TWO_PI => (2 * pi); + +sub angle_of_heading_rad { + my ($angle)=@_; + return norm_angle_rad( 5 * PI_TWO - $angle ); +} + +sub heading_of_angle_rad { + +} + + +sub norm_heading_rad { + my ($val) = @_; + while ($val > TWO_PI ) {$val -= TWO_PI} + while ($val < 0) {$val += TWO_PI } + return $val; +} + +sub norm_angle_rad { + my ($val) = @_; + while ($val > pi) {$val -= TWO_PI} + while ($val < - pi) {$val += TWO_PI} + return $val; +} + + +sub cart_of_polar { + my ($r, $theta) = @_; + return ($r * cos $theta, $r * sin $theta); +} + +sub polar_of_cart { + my ($x, $y) = @_; + return (sqrt($x*$x+$y*$y), atan2($y, $x)); +} + + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/HistoryView.pm b/sw/ground_segment/cockpit/Paparazzi/HistoryView.pm new file mode 100644 index 0000000000..de9d4348f3 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/HistoryView.pm @@ -0,0 +1,80 @@ +package Paparazzi::HistoryView; + +use strict; +use Carp; +use vars qw(@ISA); + +use Subject; +@ISA = qw(Subject); + +use POSIX; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec( + -zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -nb_bars => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -initial_range => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 1.], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->{x_scrolling_val} = 0; + $self->{bar_width} = POSIX::floor($self->get('-width') / $self->get('-nb_bars')); + $self->{bars} = []; + $self->{nb_visible_bars} = 0; + $self->{scale} = $self->get('-height') / $self->get('-initial_range'); + $self->build_gui(); +} + +sub put_value { + my ($self, $val) = @_; + return unless defined $val and defined $self->{moving_group}; + if ($self->{nb_visible_bars} >= $self->get('-nb_bars')) { + my $rect = shift @{$self->{bars}}; + $self->get('-zinc')->remove($rect); +# print "removing $rect\n"; + } + else { + $self->{nb_visible_bars}++; + } + my $height = $self->get('-height'); + my $h = $val * $self->{scale}; + my $rect = [$self->{x_scrolling_val}, $height-$h, + $self->{x_scrolling_val}+$self->{bar_width}, $height]; + my $bar = $self->get('-zinc')->add('rectangle', $self->{moving_group}, $rect, + -filled => 1, + -fillcolor => 'gray60', + -visible => 1); + push @{$self->{bars}}, $bar; + $self->{x_scrolling_val}+=$self->{bar_width}; + if ($self->{x_scrolling_val} > $self->get('-width')) { + $self->get('-zinc')->translate($self->{moving_group}, -$self->{bar_width}, 0); + } +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $origin = $self->get('-origin'); + my $parent_grp = $self->get('-parent_grp'); + my $height = $self->get('-height'); + my $width = $self->get('-width'); + $self->{main_group} = $zinc->add('group', $parent_grp, -visible => 1); + $zinc->coords($self->{main_group}, $origin); + + $self->{clipping_group} = $zinc->add('group',$self->{main_group}, -visible => 1); + $self->{itemclip} = $zinc->add('rectangle', $self->{clipping_group}, [0, 0, $width, $height], + -visible => 0); + $zinc->itemconfigure($self->{clipping_group}, -clip => $self->{itemclip}); + $self->{moving_group} = $zinc->add('group',$self->{clipping_group}, -visible => 1); + $zinc->add('rectangle', $self->{main_group}, [0, 0, $width, $height], + -visible => 1, -linecolor => "green"); +} diff --git a/sw/ground_segment/cockpit/Paparazzi/Horizon.pm b/sw/ground_segment/cockpit/Paparazzi/Horizon.pm new file mode 100644 index 0000000000..cffb52a68f --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/Horizon.pm @@ -0,0 +1,199 @@ +#============================================================================= +# Horizon Class +#============================================================================= +package Paparazzi::Horizon; +use Subject; +@ISA = ("Subject"); +use strict; + +use Paparazzi::Utils; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -radius => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -roll => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -pitch => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->build_gui(); +} + +sub roll { + my ($self, $old_angle, $new_angle) = @_; + return unless defined $old_angle; + my $dangle = Utils::rad_of_deg($old_angle - $new_angle); + $self->get('-zinc')->rotate($self->{horizon_rotate_group}, $dangle, 0, 0); +} + +sub pitch { + my ($self, $old_angle, $new_angle) = @_; + return unless defined $old_angle; + my $dy = $self->dy_of_angle($new_angle - $old_angle); + $self->get('-zinc')->translate($self->{horizon_translate_group}, 0, $dy); +} + +sub dy_of_angle { + my ($self, $angle) = @_; + return $angle * $self->{y_per_deg}; +} + +sub build_gui { + my ($self) = @_; + my $parent_grp = $self->get('-parent_grp'); + my $zinc = $self->get('-zinc'); + my $radius = $self->get('-radius'); + + $self->{horizon_group} = $zinc->add('group', $parent_grp, -visible => 1); + my $origin = $self->get('-origin'); + $zinc->coords($self->{horizon_group}, $origin); + + + $self->{horizon_rotate_group} = $zinc->add('group', $self->{horizon_group}, -visible => 1); + $self->{horizon_translate_group} = $zinc->add('group', $self->{horizon_rotate_group}, -visible => 1); + + + $self->{fixed_group} = $zinc->add('group', $self->{horizon_group}, -visible => 1); + + $self->{horizon_itemclip} = $zinc->add('arc', $self->{horizon_rotate_group}, + [-$radius, -$radius, $radius, $radius], + -visible => 0); + $zinc->itemconfigure($self->{horizon_rotate_group}, -clip => $self->{horizon_itemclip}); + + # horizon earth + $zinc->add('rectangle', $self->{horizon_translate_group} , + [-$radius, 0, $radius, 3 * $radius], + -linewidth => 0, -filled => 1,-fillcolor => '#986701', #'orange', + ); + # horizon sky + $zinc->add('rectangle', $self->{horizon_translate_group} , + [-$radius, -3 * $radius, $radius, 0], + -linewidth => 0, -filled => 1, -fillcolor => '#0099cb', # 'blue' + ); + # center line + $zinc->add('curve', $self->{horizon_translate_group}, + [-$radius, 0, $radius, 0], + -linewidth => 2, -linecolor => 'white', -filled => 0); + + # pitch scale + $self->{y_per_deg} = $radius / 30; + my $v_tick_font = '-adobe-helvetica-bold-o-normal--12-240-100-100-p-182-iso8859-1'; + my $i; + for ($i=-16; $i <= 16; $i++) { + my $angle = $i*2.5; + my $y = $self->dy_of_angle($angle); + my $x = $radius / 16; + if (!($i%4)) {$x = $radius / 4;} + elsif (!($i%2)) {$x = $radius / 8}; + $zinc->add('curve', $self->{horizon_translate_group}, + [-$x, $y, $x, $y], + -linewidth => 1, + -linecolor => 'white', + -filled => 0); + + if (!($i%4) & ($i != 0)) { + my $text_lab = sprintf("%d", $angle); + my @text_attr = ( -color => 'white', + -font => $v_tick_font, + -text => $text_lab ); + $zinc->add('text', $self->{horizon_translate_group}, + -position => [-$x-10, -$y], + -anchor => 'e', + @text_attr); + $zinc->add('text', $self->{horizon_translate_group}, + -position => [$x+10, -$y], + -anchor => 'w', + @text_attr); + } + } + + # arrow + my $arrow_len = 10; + $zinc->add('curve', $self->{horizon_rotate_group}, + [0, -$radius+1, + -$arrow_len+1, -$radius+$arrow_len, + $arrow_len-1, -$radius+$arrow_len], + -linewidth => 2, + -linecolor => 'yellow', + -filled => 0, + -closed => 1); + + # roll scale + $zinc->add('arc', $self->{fixed_group}, + [-$radius + 1, -$radius + 1, $radius - 1, $radius - 1], + -startangle => -120, + -extent => 60, + -linewidth => 2, + -linecolor => 'white', + -filled => 0); + + for ($i=-4; $i <= 4; $i++) { + my $angle = Utils::rad_of_deg($i * 15); + my $x1 = 0 * cos($angle) - $radius * sin($angle); + my $y1 = 0 * sin($angle) - $radius * cos($angle); + my $x2 = 0 * cos($angle) - ($radius+10) * sin($angle); + my $y2 = 0 * sin($angle) - ($radius+10) * cos($angle); + $zinc->add('curve', $self->{fixed_group}, + [$x1, $y1, $x2, $y2], + -linewidth => 1, + -linecolor => 'white', + -filled => 0); + } + + +# fixed indicator + my @center_sign = [ -3, -3, + -3, 3, + 3, 3, + 3, -3]; + + my @left_sign = [-50, -3, + -80, -3, + -80, 1, + -50, 1, + -50, 15, + -46, 15, + -46, -3]; + my @right_sign = [50, -3, + 80, -3, + 80, 1, + 50, 1, + 50, 15, + 46, 15, + 46, -3]; + my @fixed_indic_attr = ( -linewidth => 1, + -linecolor => 'yellow', + -filled => 1, + -fillcolor => 'black', + -closed => 1 + ); + foreach my $sign_section (\@center_sign, \@left_sign, \@right_sign) { + $zinc->add('curve', $self->{fixed_group}, + @{$sign_section}, + @fixed_indic_attr); + } + + # side black masks + my $pc_black = 0.18; + my @side_masks_attr = ( -linewidth => 0, + -filled => 1, + -fillcolor => 'black' ); + $zinc->add('rectangle', $self->{fixed_group} , + [(1-$pc_black)*$radius , -$radius, + $radius, $radius], + @side_masks_attr); + $zinc->add('rectangle', $self->{fixed_group} , + [-$radius , -$radius, + -(1-$pc_black)*$radius, $radius], + @side_masks_attr); +} + + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/IRPage.pm b/sw/ground_segment/cockpit/Paparazzi/IRPage.pm new file mode 100644 index 0000000000..290a87a417 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/IRPage.pm @@ -0,0 +1,152 @@ +package Paparazzi::IRPage; +use Paparazzi::NDPage; +@ISA = ("Paparazzi::NDPage"); +use strict; +use Subject; +use DigiKit::Button; + +use Paparazzi::HistoryView; + + +use constant TITLE => "Infrared"; +use constant UPDATE_REPEAT => 2000; + +sub populate { + my ($self, $args) = @_; + $args->{-title} = TITLE; + $self->SUPER::populate($args); + $self->configspec( + -wind => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + -lls => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.0015], + ); +} + +sub completeinit { + my $self = shift; + $self->{wind_running} = 0; + $self->SUPER::completeinit(); +# $self->build_gui(); + $self->configure('-pubevts' => 'WIND_COMMAND'); + $self->{timer_id} = $self->get('-zinc')->repeat(UPDATE_REPEAT, [\&onTimer, $self]); +} + +sub onTimer { + my ( $self) = @_; + $self->{history}->put_value(scalar $self->get('-lls')); +} + +sub wind { + my ($self, $old_val, $new_val) = @_; + foreach my $field (keys %{$new_val}) { + $self->get('-zinc')->itemconfigure ($self->{'text_'.$field}, + -text => sprintf("%s : %.4f", $field, $new_val->{$field})) if defined $self->{'text_'.$field}; + } +} + +sub lls { + my ($self, $old_val, $new_val) = @_; + return unless defined $new_val and defined $self->{history}; +# $self->{history}->put_value($new_val); + $self->get('-zinc')->itemconfigure ($self->{'text_auto gain'}, + -text => sprintf("auto gain : %.5f", $new_val)); +} + +#sub put_lls { +# my ($self, $value) = @_; +## $self->{history}->put_value($value); +#} + +sub build_gui { + my ($self) = @_; + $self->SUPER::build_gui(); + my $zinc = $self->get('-zinc'); + my $parent_grp = $self->get('-parent_grp'); + my $main_group = $self->{main_group}; + + my $fields = ["contrast", "gain","auto gain"]; + my ($y, $dy) = ( 35, 20); + foreach my $field (@{$fields}) { + $self->{'text_'.$field} = $zinc->add('text', $main_group, + -position => [20, $y], + -color => 'white', + -anchor => 'w', + -text => "$field"); + $y+=$dy; + } + + my $nb_bars = 125; + $self->{history} = + Paparazzi::HistoryView->new(-zinc => $zinc, + -width => 250, + -height => 50, + -origin => [20, 90], + -parent_grp => $self->{main_group}, + -nb_bars => $nb_bars, + -initial_range => 0.003, + ); + $zinc->add('text', $main_group, + -position => [20, 145], + -color => 'white', + -anchor => 'w', + -text => sprintf("%d seconds", UPDATE_REPEAT * $nb_bars / 1000), + ); + + $zinc->add('text', $main_group, + -position => [10, 170], + -color => 'white', + -anchor => 'w', + -text => "Wind"); + + $fields = ['dir', 'speed','mean_aspeed', 'stddev']; + ($y, $dy) = ( 190, 20); + foreach my $field (@{$fields}) { + $self->{'text_'.$field} = $zinc->add('text', $main_group, + -position => [20, $y], + -color => 'white', + -anchor => 'w', + -text => "$field"); + $y+=$dy; + } + + $self->{button_clear_wind} = DigiKit::Button->new(-widget => $zinc, + -parentgroup => $main_group, + -style => ['Aqualike', + -width => 60, + -height => 20, + -color => 'green', + -text => 'clear', + -trunc => 'right', + ], + -position => [10, 262], + ); + $self->{button_toggle_wind} = DigiKit::Button->new(-widget => $zinc, + -parentgroup => $main_group, + -style => ['Aqualike', + -width => 60, + -height => 20, + -color => 'green', + -text => 'start', + -trunc => 'left', + ], + -position => [70, 262], + ); + $self->{button_clear_wind}->configure(-releasecommand => sub { + $self->notify('WIND_COMMAND', 'clear'); + } + ); + $self->{button_toggle_wind}->configure(-releasecommand => sub { + if ($self->{wind_running}) { + $self->{button_toggle_wind}->value('start'); + $self->notify('WIND_COMMAND', 'stop'); + $self->{wind_running} = 0; + } + else { + $self->{button_toggle_wind}->value('stop'); + $self->notify('WIND_COMMAND', 'start'); + $self->{wind_running} = 1; + } + } + ); + +} + diff --git a/sw/ground_segment/cockpit/Paparazzi/LensScale.pm b/sw/ground_segment/cockpit/Paparazzi/LensScale.pm new file mode 100644 index 0000000000..e3ad9317b5 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/LensScale.pm @@ -0,0 +1,136 @@ +package Paparazzi::LensScale; +use Paparazzi::Scale; +@ISA = ("Paparazzi::Scale"); +use strict; + +use POSIX; +use Subject; + +use constant DECIMAL_SPACING => 18; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec( + -vz => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + ); +} + +#sub completeinit { +# my $self = shift; +# $self->SUPER::completeinit; +## $self->build_gui(); +#} + +use constant VZ_WIDTH => 8; +use constant MAX_VZ => 2. ; + +sub vz { + my ($self, $old_vz, $new_vz) = @_; + return unless defined $old_vz; + my $zinc = $self->get('-zinc'); + my $h = $self->get('-height'); + my $y = $new_vz * $h / 2 / MAX_VZ; + $zinc->coords($self->{vz_itemclip}, [0, 0, VZ_WIDTH, -$y]); + +} + +sub value() { + my ($self, $previous, $new) = @_; + $self->SUPER::value($previous, $new); + my $zinc = $self->get('-zinc'); + my $int_part = POSIX::floor($new); + + $zinc->itemconfigure ($self->{fixed_text}, + -text => sprintf("%.0f.", $int_part)); + my $decimal = POSIX::floor(($new*10))%10; + my $new_y = - $decimal * DECIMAL_SPACING; + $zinc->treset($self->{decimal_group}); + $zinc->translate($self->{decimal_group}, 0, $new_y); +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + $self->SUPER::build_gui; + my $parent_grp = $self->get('-parent_grp'); + $self->{lens_group} = $zinc->add('group', $parent_grp, -visible => 1); + + my ($xorg, $yorg) = $self->get('-origin'); + my $w = $self->get('-width'); + my $h = $self->get('-height'); + my $rc_pc = $self->get('-fig_clm_pc'); + my $h1 = 30; + my $h2 = 50; + my $x2 = $rc_pc * $w; + my $xt = $x2 + 3; + my $y1 = ($h - $h2)/2; + my $y2 = ($h - $h1)/2; + my $y3 = ($h + $h1)/2; + my $y4 = ($h + $h2)/2; + +# print "foo $xorg $yorg $w $h\n"; + + $zinc->coords($self->{lens_group}, [$xorg, $yorg]); + + $zinc->add('rectangle', $self->{lens_group} , + [0, $y2, $w, $y3], + -linewidth => 0, + -filled => 1, + -fillcolor => 'black', + ); + + $zinc->add('curve', $self->{lens_group}, + [0, $y2, + $x2, $y2, + $x2, $y1, + $w - VZ_WIDTH, $y1, + $w - VZ_WIDTH, $y4, + $x2, $y4, + $x2, $y3, + 0, $y3], + -linewidth => 2, + -linecolor => 'yellow', + -filled => 0); + + my $font = '-adobe-helvetica-bold-o-normal--16-240-100-100-p-182-iso8859-1'; + + $self->{fixed_text} = $zinc->add('text', $self->{lens_group}, + -position => [$xt, $h/2], + -color => 'white', + -font => $font, + -anchor => 'e', + -text => "00."); + + $self->{decimal_clipping_group} = $zinc->add('group', $self->{lens_group}, -visible => 1); + $self->{itemclip} = $zinc->add('rectangle', $self->{decimal_clipping_group}, [0, $y1, $w, $y4], + -visible => 0); + $zinc->itemconfigure($self->{decimal_clipping_group}, -clip => $self->{itemclip}); + $self->{decimal_group} = $zinc->add('group', $self->{decimal_clipping_group}, -visible => 1); + for (my $i=-1; $i< 11; $i++) { + $zinc->add('text', $self->{decimal_group}, + -position => [$xt, $h/2 + $i * DECIMAL_SPACING], + -color => 'white', + -font => $font, + -anchor => 'w', + -text => sprintf("%1d", $i%10) + ); + } + + $self->{vz_clipping_group} = $zinc->add('group', $self->{lens_group}, -visible => 1); + $zinc->coords($self->{vz_clipping_group}, [$w - VZ_WIDTH + 1, $h/2]); + $self->{vz_itemclip} = $zinc->add('rectangle', $self->{vz_clipping_group}, [0, -$h/2, VZ_WIDTH, $h/2], + -visible => 0); + $zinc->itemconfigure($self->{vz_clipping_group}, -clip => $self->{vz_itemclip}); + $self->{vz_group} = $zinc->add('group', $self->{vz_clipping_group}, -visible => 1); + + $zinc->add('rectangle', $self->{vz_group} , + [0, -$h/2, VZ_WIDTH, $h/2], + -linewidth => 0, + -filled => 1, + -fillcolor => "=axial 90 |red;150 50|green;150 50", -filled => 1, +# -fillcolor => 'red', + ); + + # print "in LensScale::build_gui\n"; +} diff --git a/sw/ground_segment/cockpit/Paparazzi/MapView.pm b/sw/ground_segment/cockpit/Paparazzi/MapView.pm new file mode 100644 index 0000000000..4c331f4f8a --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/MapView.pm @@ -0,0 +1,479 @@ +package Paparazzi::MapView; + +use Tk; +#use Tk::widgets qw/PNG/; +#use Tk::JPEG; +use Tk::Zinc; +use XML::DOM; +use Math::Trig; +require File::Basename; + +use base "Tk::Frame"; +use strict; + +Construct Tk::Widget 'MapView'; + +sub ClassInit { + my ($class, $mw) = @_; + $class->SUPER::ClassInit($mw); +} + +sub Populate { + my ($self, $args) = @_; + my $render = $args->{-render}; + delete $args->{-render}; + $self->SUPER::Populate($args); + my $zinc = $self->Zinc(-backcolor => 'black', + -borderwidth => 3, + -relief => 'sunken', + -render => $render, + -trackmanagedhistorysize => 500, + -trackvisiblehistorysize => 500 + ); + $zinc->pack(-fill => 'both', -expand => "1"); + $self->Advertise('zinc' => $zinc); + $self->build_gui(); + $self->{tracks} = {}; + $self->set_bindings(); +} + +use constant SCALE_LEN => 200; +use constant SCALE_HEIGHT => 5; +use constant SCALE_X => 50; +use constant SCALE_Y => 1000; + +sub build_gui { + my ($self) = @_; + my $zinc = $self->Subwidget('zinc'); + $self->{main_group} = $zinc->add('group', 1, -visible => 1); + + $self->{pan_group} = $zinc->add('group', $self->{main_group}, -visible => 1); + $self->{zoom_group} = $zinc->add('group', $self->{pan_group}, -visible => 1); + # map + $self->{map_picture_group} = $zinc->add('group', $self->{zoom_group}, -visible => 1); + # waypoints + $self->{map_wp_group} = $zinc->add('group', $self->{zoom_group}, -visible => 1 ); + # track + $self->{map_trajectory_group} = $zinc->add('group', $self->{zoom_group}, -visible => 1); + $self->{map_track_group} = $zinc->add('group', $self->{zoom_group}, -visible => 1); + + # scale + $self->{scale_group} = $zinc->add('group', $self->{main_group}, -visible => 1); + $self->{scale_item} = $zinc->add('rectangle', $self->{scale_group}, + [SCALE_X, SCALE_Y, SCALE_X+SCALE_LEN, SCALE_Y + SCALE_HEIGHT], + -visible => 1, + -filled => 1, + -fillcolor => 'black'); + $self->{scale_text_item} = $zinc->add('text', $self->{main_group}, + -position => [SCALE_X, SCALE_Y - SCALE_HEIGHT], + -anchor => 'w', + -text => "unknown"); + $zinc->configure(-overlapmanager => $self->{map_track_group} ); + +} + +sub set_bindings { + my ($self) = @_; + my $zinc = $self->Subwidget('zinc'); + my $map_grp = $self->{pan_group}; + $zinc->Tk::bind('', [\&mouse_zoom, $self, 1.15]); + $zinc->Tk::bind('', [\&mouse_zoom, $self, 0.75]); + $zinc->Tk::bind('', [\&drag_start, $map_grp, \&drag_motion]); + $zinc->Tk::bind('', [\&drag_stop]); + $zinc->Tk::bind('', [\&show_pos_cbk, $self]); + $self->parent()->Tk::bind('', [\&dec_zoom, $self]); + $self->parent()->Tk::bind('', [\&inc_zoom, $self]); + $self->parent()->Tk::bind('', [\&clear_track, $self]); + $self->parent()->Tk::bind('', [\&clear_track, $self]); + $self->parent()->Tk::bind('', [\&scroll, $self]); + $self->parent()->Tk::bind('', [\&scroll, $self]); + $self->parent()->Tk::bind('', [\&scroll, $self]); + $self->parent()->Tk::bind('', [\&scroll, $self]); +} + +sub load_flight_plan { + my ($self, $xmldata) = @_; + my $parser = XML::DOM::Parser->new(); + my $doc = $parser->parse($xmldata); + + my $flight_plan = $doc->getElementsByTagName('flight_plan')->[0]; + + my $waypoints = $doc->getElementsByTagName('waypoints')->[0]; + $self->{NAV_UTM_EAST0} = $waypoints->getAttribute('utm_x0'); + $self->{NAV_UTM_NORTH0} = $waypoints->getAttribute('utm_y0'); + + foreach my $wp ($doc->getElementsByTagName('waypoint')) { + my ($wp_name, $wp_x_mission, $wp_y_mission, $wp_alt) = + ( $wp->getAttribute('name'), + $wp->getAttribute('x'), + $wp->getAttribute('y'), + $wp->getAttribute('alt')); + my @wp_map = $self->map_of_mission([$wp_x_mission, $wp_y_mission]); + + my $item = $self->Subwidget('zinc')->add( 'waypoint', $self->{map_wp_group}, 1, + -position => \@wp_map, + -labelformat => 'x20x18+0+0', + ); + $self->Subwidget('zinc')->itemconfigure($item, 0, + -text => "$wp_name", + ); + } +} + +sub load_map { + my ($self, $xml_map) = @_; + my $parser = XML::DOM::Parser->new(); + my $doc = $parser->parsefile($xml_map); + my $map_node = $doc->getElementsByTagName('map')->[0]; + my $projection = $map_node->getAttribute('projection'); + my $points = $doc->getElementsByTagName('point'); + my @refpoints; + foreach my $i (0..2) { + my $p = $points->[$i]; + $refpoints[$i]->{map} = [$p->getAttribute('x'), $p->getAttribute('y')]; + $refpoints[$i]->{geo} = [$p->getAttribute('utm_x'), $p->getAttribute('utm_y')]; + } + foreach my $i ('map', 'geo') { + $self->{cal_0}->{$i} = $refpoints[0]->{$i}; + $self->{cal_0X}->{$i} = [subst_c2d($refpoints[1]->{$i}, $refpoints[0]->{$i})]; + $self->{cal_0Y}->{$i} = [subst_c2d($refpoints[2]->{$i}, $refpoints[0]->{$i})]; + $self->{cal_det_OX_0Y}->{$i} = vect_prod_c2d($self->{cal_0X}->{$i}, $self->{cal_0Y}->{$i}); + } + + my $data_dir = File::Basename::dirname($xml_map); + $data_dir =~ /.*\/[^\/]$/; + my $map_filename = $data_dir."/".$map_node->getAttribute('file'); + my $image = $self->Subwidget('zinc')->Photo("bg_picture", -file => $map_filename); + my $img_item = $self->Subwidget('zinc')->add('icon', $self->{map_picture_group}, + -image => $image); +# $self->Subwidget('zinc')->coords($self->{pan_group}, [0, -$image->height()]); + $self->Subwidget('zinc')->treset($self->{pan_group}); +# $self->Subwidget('zinc')->coords($self->{pan_group}, [0 , $image->height()]); +} + +sub geo_of_map { + my ($self, $p_map) = @_; + my @OP = subst_c2d($p_map, $self->{cal_0}->{map}); +# print "OP [@OP]\n"; + my $cx = vect_prod_c2d(\@OP, $self->{cal_0Y}->{map}) / $self->{cal_det_OX_0Y}->{map}; + my $cy = -vect_prod_c2d(\@OP, $self->{cal_0X}->{map}) / $self->{cal_det_OX_0Y}->{map}; +# print "cx cy $cx $cy\n"; + my @result = add_c2d($self->{cal_0}->{geo}, + [add_c2d([scale_c2d($self->{cal_0X}->{geo}, $cx)], + [scale_c2d($self->{cal_0Y}->{geo}, $cy)])]); +# print "result [@result]\n"; + return @result; +} + +sub map_of_geo { + my ($self, $p_geo) = @_; + my @OP = subst_c2d($p_geo, $self->{cal_0}->{geo}); + my $cx = vect_prod_c2d(\@OP, $self->{cal_0Y}->{geo}) / $self->{cal_det_OX_0Y}->{geo}; + my $cy = -vect_prod_c2d(\@OP, $self->{cal_0X}->{geo}) / $self->{cal_det_OX_0Y}->{geo}; +# print "cx cy $cx $cy\n"; + + my @result = add_c2d( $self->{cal_0}->{map}, + [add_c2d([scale_c2d($self->{cal_0X}->{map}, $cx)], + [scale_c2d($self->{cal_0Y}->{map}, $cy)])]); +# print "result [@{$p_geo}] [@result]\n"; + return @result; +} + +sub map_of_mission { + my ($self, $p_mission) = @_; + my $geo_pos = [$p_mission->[0] + $self->{NAV_UTM_EAST0}, + $p_mission->[1] + $self->{NAV_UTM_NORTH0}]; +# print "geo @$geo_pos\n"; + my @result = $self->map_of_geo($geo_pos); +# print "result [@result}\n"; + return @result; +} + +sub set_pos_geo { + my ($self, $pos_utm) = @_; + my $item = $self->Subwidget('zinc')->add('text', $self->{map_trajectory_group}, + -position => [$self->map_of_geo($pos_utm)], + -color => 'blue', + -anchor => 'c', + -text => "."); +} + +sub set_track_geo { + my ($self, $name, $pos_utm) = @_; + return $self->set_track_map($name, [$self->map_of_geo($pos_utm)]); +} + +sub set_track_mission { + my ($self, $name, $pos_xy) = @_; + return $self->set_track_map($name, [$self->map_of_mission($pos_xy)]); +} + + +sub set_track_map { + my ($self, $name, $pos_xy) = @_; + my $zinc = $self->Subwidget('zinc'); + my $track_item = $self->{tracks}->{$name}; + if (not defined $track_item) { + $track_item = $zinc->add( 'track', $self->{map_track_group}, 2, + -position => $pos_xy, + -labelformat => 'x80x18+0+0', + ); + $zinc->itemconfigure($track_item, 0, + -text => "$name", + ); + $zinc->itemconfigure($track_item, +# -filledhistory => 1, +# -circlehistory => 1, +# -mixedhistory => 1, + -symbolcolor => 'green', + -leadercolor => 'green', + -markersize => '10', + -markercolor => 'green', +# -historyvisible => 100, +# -trackvisiblehistorysize => 100, +# -trackmanagedhistorysize => 100, + -historycolor =>'black', + ); + $zinc->itemconfigure($track_item, 0, + -text => "$name", + ); + $self->{tracks}->{$name} = $track_item; + } + else { + $zinc->coords($track_item, $pos_xy); + } + return $track_item; +} + +sub set_picture_mission { + my ($self, $name, $pos_xy, $heading, $scale) = @_; + return $self->set_picture_map($name, [$self->map_of_mission($pos_xy)], $heading, $scale); +} + + + +sub set_picture_map { + my ($self, $name, $pos_xy, $heading, $scale) = @_; + my $zinc = $self->Subwidget('zinc'); + my $track_item = $self->{tracks}->{$name}; + if (not defined $track_item) { + my $_w = 720; my $_h = 570; + my $w = $scale*$_w; + my $h = $scale*$_h; + + + $track_item = $zinc->add( 'rectangle', $self->{map_track_group}, + [$pos_xy->[0] - $w/2, $pos_xy->[1] - $h/2, $pos_xy->[0]+$w/2, $pos_xy->[1]+$h/2] + ); + $zinc->rotate($track_item, $heading, $pos_xy->[0], $pos_xy->[1] ); + + + $self->{tracks}->{$name} = $track_item; + + my $image = $zinc->Photo("$name", -file => $name.".gif"); + my $img_item = $zinc->add('icon', $self->{map_track_group}, + -image => $image); + $zinc->scale($img_item, $scale, $scale); + $zinc->translate($img_item ,$pos_xy->[0] - $w/2 , $pos_xy->[1] -$h/2); + $zinc->rotate($img_item ,$heading, $pos_xy->[0] , $pos_xy->[1]); + + } + else { + $zinc->coords($track_item, $pos_xy); + } + return $track_item; +} + +sub scroll_to_map { + my ($self, $p1_map, $p2_dev) = @_; + my $zinc = $self->Subwidget('zinc'); + + print "p1_map @$p1_map p2_dev @$p2_dev\n"; + + my ($x1_dev, $y1_dev) = $zinc->transform($self->{zoom_group}, 'device',[$p1_map]); + print "$p1_map $x1_dev $y1_dev \n"; + my ($xt, $yt) = subst_c2d($p2_dev, [$x1_dev, $y1_dev]); + $zinc->translate($self->{pan_group}, $xt, $yt); +} + + +sub map_of_dev { + my ($self, $p_dev) = @_; + my $zinc = $self->Subwidget('zinc'); + my ($x_m, $y_m) = $zinc->transform('device', $self->{zoom_group}, [$p_dev]); + print "in map_of_dev @$p_dev -> $x_m, $y_m \n"; + return ($x_m, $y_m); +} + +##### +# +# Callbacks +# +##### + +sub mouse_zoom { + my ($_zinc, $self, $ratio) = @_; + my $zinc = $self->Subwidget('zinc'); + my $ev = $zinc->XEvent(); + my $pointed_on_map = $self->map_of_dev([$ev->x, $ev->y]); + $zinc->scale($self->{pan_group}, $ratio, $ratio); +# $self->scroll_to_map([$pointed_on_map], [$ev->x, $ev->y]); +} + +sub inc_zoom { + my ($zinc, $self) = @_; + my $zzz = $self->Subwidget('zinc'); +# my $ev = $zzz->XEvent(); + $zzz->scale($self->{pan_group}, 1.25, 1.25); +} + +sub adjust_zoom { + my ($binded, $self, $ratio) = @_; + + print ("$binded, $self, $ratio\n"); + + my $zinc = $self->Subwidget('zinc'); # had to bind on frame for keyboard events ??? + $zinc->scale($self->{pan_group}, $ratio, $ratio); + + my $map_tgroup = $self->{zoom_group}; + my $p0 = $zinc->transform('device', $map_tgroup, [0, 0]); + my $p1 = $zinc->transform('device', $map_tgroup, [100, 0]); + + print ("$p0 $p1\n"); + +# my $v = subst_c2d([$p0], [$p1]); +# my $mod = module_c2d([$v]); +# my $r = $mod / SCALE_LEN; +# print "SCALE_LEN -> $mod : $r\n" +} + +sub clear_track { + my ($zinc, $self) = @_; + my $zzz = $self->Subwidget('zinc'); + $zzz->remove($self->{map_trajectory_group}); + $self->{map_trajectory_group} = $zzz->add('group', $self->{zoom_group}, -visible => 1); +# $self->scroll_to_map([1000,200], [250,250]); + + printf ("clear\n"); +} + +my ($x_orig, $y_orig); +sub drag_start { + print "drag start\n"; + my ($zinc, $item, $action) = @_; + my $ev = $zinc->XEvent(); + $x_orig = $ev->x; + $y_orig = $ev->y; + $zinc->Tk::bind('', [$action, $item]); +} + +sub drag_motion { +# print "motion\n"; + my ($zinc, $item) = @_; + my $ev = $zinc->XEvent(); + my $x = $ev->x; + my $y = $ev->y; + $zinc->translate($item, $x-$x_orig, $y-$y_orig); + $x_orig = $x; + $y_orig = $y; + +} + +sub drag_stop { + print "stop\n"; + my ($zinc) = @_; + $zinc->Tk::bind('', ''); +} + +sub show_pos_cbk { + my ($zinc, $self) = @_; + my $ev = $zinc->XEvent(); + my $x = $ev->x; + my $y = $ev->y; + + my $map_tgroup = $self->{zoom_group}; + + my ($x_m, $y_m) = $zinc->transform('device', $map_tgroup, [$x, $y]); + + print "in show_pos_cbk $x $y -> $x_m, $y_m \n"; +} + + +sub scroll { + + +} + + + +### +# +# Geometry.pm +# +### + +sub subst_c2d { + my ($a, $b) = @_; + my @result = map ( {$a->[$_] - $b->[$_]} 0,1); + return @result; +} + +sub add_c2d { + my ($a, $b) = @_; + my @result = map ( {$a->[$_] + $b->[$_]} 0,1); + return @result; +} + +sub module_c2d { + my ($a) = @_; + return sqrt($a->[0]*$a->[0] + $a->[1]*$a->[1]); +} + +sub scale_c2d { + my ($a, $k) = @_; + my @result = map ( {$a->[$_] * $k} 0,1); +} + +sub vect_prod_c2d { + my ($a, $b) = @_; + return $a->[0] * $b->[1] - $b->[0] * $a->[1]; +} + +### +# +# Projections +# +### + + + +#sub utm_of_geo { +# my ($pos_lon, $pos_lat) = @_; + +# let ellipsoid = ellipsoid_of geo in +# my $k0 = 0.9996; +# my $xs = 500000; +# my $ys = (phi > 0.) ? 0 : 10000000; +# let lambda_deg = truncate (floor ((Rad>>Deg)lambda)) in +# let zone = (lambda_deg + 180) / 6 + 1 in +# let lambda_c = (Deg>>Rad) (float (lambda_deg - lambda_deg mod 6 + 3)) in +# let e = ellipsoid.e +# and n = k0 *. ellipsoid.a in +# let ll = latitude_isometrique phi e +# and dl = lambda -. lambda_c in +# let phi' = asin (sin dl /. cosh ll) in +# let ll' = latitude_isometrique phi' 0. in +# let lambda' = atan (sinh ll /. cos dl) in +# let z = C.make lambda' ll' +# and c = serie5 coeff_proj_mercator e in +# let z' = ref (C.scal c.(0) z) in +# for k = 1 to Array.length c - 1 do +# z' := C.add !z' (C.scal c.(k) (C.sin (C.scal (float (2*k)) z))) +# done; +# z' := C.scal n !z'; +# { utm_zone = zone; utm_x = xs + truncate (C.im !z'); utm_y = ys + truncate (C.re !z') };; + + + + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/MissionD.pm b/sw/ground_segment/cockpit/Paparazzi/MissionD.pm new file mode 100644 index 0000000000..143c409833 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/MissionD.pm @@ -0,0 +1,108 @@ +package Paparazzi::MissionD; + +use Tk::ROText; +use XML::DOM; + +use base qw/Tk::Frame/; +use strict; + +Construct Tk::Widget 'MissionD'; + +sub ClassInit { + my ($class, $mw) = @_; + $class->SUPER::ClassInit($mw); +} + +sub Populate { + my ($self, $args) = @_; + $self->SUPER::Populate($args); + my $text = $self->Scrolled('ROText', + -scrollbars => 'osoe', + ); + $text->pack(-fill => 'both', -expand => "1"); + $self->Advertise('text' => $text); + $self->{cur_block} = -1; + $self->{cur_stage} = -1; +# $self->ConfigSpecs(); +# $self->Delegates(); +} + +use Data::Dumper; + +sub get_block_id { + my ($no_block) = @_; + return "block_".$no_block; +} + +sub get_stage_id { + my ($no_block, $no_stage) = @_; + return "stage_".$no_block."_".$no_stage; +} + +sub set_block_and_stage { + my ($self, $new_block, $new_stage) = @_; + my $text = $self->Subwidget('text'); + if ($self->{cur_block} != $new_block) { + $text->tagConfigure(get_block_id($self->{cur_block}), -background => undef); + $text->tagConfigure(get_block_id($new_block), -background => 'green3'); + $text->tagConfigure(get_stage_id($self->{cur_block}, $self->{cur_stage}), -background => undef); + $text->tagConfigure(get_stage_id($new_block, $new_stage), -background => 'green1'); + $self->{cur_block} = $new_block; + $self->{cur_stage} = $new_stage; + } + else { + if ($self->{cur_stage} != $new_stage) { + $text->tagConfigure(get_stage_id($self->{cur_block}, $self->{cur_stage}), -background => undef); + $text->tagConfigure(get_stage_id($self->{cur_block}, $new_stage), -background => 'green1'); + $self->{cur_stage} = $new_stage; + } + } +} + +sub load_flight_plan { + my ($self, $xmldata) = @_; + + my $text = $self->Subwidget('text'); + if (Tk::Exists($text)) { + $text->delete('0.0', 'end'); + } + + my $parser = XML::DOM::Parser->new(); + my $doc = $parser->parse($xmldata); + + my ($blocks, $blocks_stages); + + foreach my $stage ($doc->getElementsByTagName('stage')) { + my $block_name = $stage->getAttribute('block_name'); + my $block_no = $stage->getAttribute('block'); + my $stage_no = $stage->getAttribute('stage'); + my $stage_text = ""; + my $stage_kids = $stage->getChildNodes(); + foreach my $kid (@{$stage_kids}) { + $stage_text = $stage_text.$kid->toString() if $kid->getNodeType() != TEXT_NODE; + } + $blocks_stages->{$block_name}->{$stage_text} = get_stage_id($block_no, $stage_no); + $blocks->{$block_name} = get_block_id($block_no) unless defined $blocks->{block_name}; + } + + # print Dumper(\$blocks); + # print Dumper(\$blocks_stages); + + foreach my $block ($doc->getElementsByTagName('block')){ + my $block_name = $block->getAttribute('name'); + foreach my $line (split (/(\n)/, $block->toString())) { + my $key = $line; + $key =~ s/^\s*//; # remove any leading whitespace + $key =~ s/\s*$//; # remove any trailing whitespace + if ($key ne "") { + my $block_id = $blocks->{$block_name}; + my $stage_id = $blocks_stages->{$block_name}->{$key}; + my $tags = [$block_id]; + push(@{$tags}, ($stage_id)) if defined $stage_id; + $self->Subwidget('text')->insert('end', $line."\n", $tags); + } + } + } +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/ND.pm b/sw/ground_segment/cockpit/Paparazzi/ND.pm new file mode 100644 index 0000000000..cb1a34512a --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/ND.pm @@ -0,0 +1,132 @@ +package Paparazzi::ND; +use Subject; +@ISA = ("Subject"); + +use strict; + +use Math::Trig; +use Tk; +use Tk::Zinc; +use Paparazzi::SatPage; +use Paparazzi::EnginePage; +use Paparazzi::APPage; +use Paparazzi::IRPage; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -page => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, "gps"], + -engine_status => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, undef], + -ap_status => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, undef], + -wind => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, undef], + -lls => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, undef], + -sats => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, undef], + -fix => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, undef], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit(); + $self->build_gui(); + $self->configure('-pubevts' => 'WIND_COMMAND'); +} + +sub page { + my ($self, $old_val, $new_val) = @_; + print "in ND::page [$new_val]\n"; + return unless defined $new_val and defined $self->{sat_view}; +# $self->{sat_view}->configure('-visible' => $i%2); +# $i++; +} + +sub put_lls { + my ($self, $value) = @_; +# $self->{IR}->put_lls($value); +} + +sub build_gui() { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + my $origin = $self->get('-origin'); + + $self->{main_group} = $zinc->add('group', 1, -visible => 1); + $zinc->coords($self->{main_group}, $origin); + $zinc->add('rectangle', $self->{main_group}, + [1, 1, $width-2, $height-2], + -visible => 1, + -filled => 0, + -linecolor => 'red'); + my ($margin, $page_width) = (5, 300); + my $real_width = $page_width - 2*$margin; + my ($page_per_row, $row, $col) = (2, 0, 0); + + my $pages = ['Sat', 'Engine','AP', 'IR']; + foreach my $page (@{$pages}) { + $self->{$page} = $self->component('Paparazzi::'.$page.'Page', + -zinc => $zinc, + -parent_grp => $self->{main_group}, + -origin => [ $margin+$col*$page_width, $margin+$row*$page_width], + -width => $real_width, + -height => $real_width, + -visible => 1, + ); + # $self->{$page} = ('Paparazzi::'.$page.'Page')->new( +# -zinc => $zinc, +# -parent_grp => $self->{main_group}, +# -origin => [ $margin+$col*$page_width, $margin+$row*$page_width], +# -width => $real_width, +# -height => $real_width, +# -visible => 1, +# ); + + if ($page eq "IR") { + $self->{$page}->attach($self, 'WIND_COMMAND', [sub { my ($self, $component, $signal, $arg) = @_; + $self->notify('WIND_COMMAND', $arg)}]); + } + $col++; + unless ($col lt $page_per_row) { $col=0; $row++ }; + } + + + my $sat_h = + { + -itow => 12345, + -nch => 7, + -sats => [ { -chn => 0 , -svid => 3, -flags => 0x00, -qi => 0, -cno => 0, -elev => 45, -azim => 315, -prres => 0.}, + { -chn => 1 , -svid => 22, -flags => 0x01, -qi => 0, -cno => 45.2, -elev => 35, -azim => 300, -prres => 0.}, + { -chn => 2 , -svid => 1, -flags => 0x00, -qi => 0, -cno => 36.6, -elev => 6, -azim => 315, -prres => 0.}, + { -chn => 3 , -svid => 25, -flags => 0x01, -qi => 0, -cno => 45.2, -elev => 45, -azim => 237, -prres => 0.}, + { -chn => 4 , -svid => 6, -flags => 0x01, -qi => 0, -cno => 45.8, -elev => 58, -azim => 61, -prres => 0.}, + { -chn => 5 , -svid => 17, -flags => 0x01, -qi => 0, -cno => 43.8, -elev => 31, -azim => 123, -prres => 0.}, + { -chn => 6 , -svid => 30, -flags => 0x01, -qi => 0, -cno => 42.2, -elev => 53, -azim => 161, -prres => 0.}, + ] + }; + $self->{Sat}->configure( -sats => $sat_h); + $self->{Sat}->configure( -fix => 30.); + +# my $engine_h = { -nb_engine => 2, +# -engine => [{throttle => 50, -rpm => 3500, -temp => 39}, +# {throttle => 50, -rpm => 3400, -temp => 37}], +# -bat => 11.5, +# -energy => 25.2 +# }; + +# my $ap_h = { +# -mode => 1, +# -h_mode => 2, +# -v_mode => 0, +# -target_climb => 1., +# -target_alt => 200., +# -target_heading => 36., +# }; +# $self->{AP}->configure( -ap_status => $ap_h); +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/NDPage.pm b/sw/ground_segment/cockpit/Paparazzi/NDPage.pm new file mode 100644 index 0000000000..2ca48c5699 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/NDPage.pm @@ -0,0 +1,62 @@ +package Paparazzi::NDPage; + +use Subject; +@ISA = ("Subject"); +use strict; + + +use Tk; +use Tk::Zinc; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -title => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -visible => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, "1"], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->build_gui(); +} + +sub visible { + my ($self, $old_val, $new_val) = @_; + print "in EngineView::visible $new_val\n"; + return unless defined $new_val and defined $self->{main_group}; + my $zinc = $self->get('-zinc'); + $zinc->itemconfigure ($self->{main_group}, + -visible => $new_val, + ); +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + my $origin = $self->get('-origin'); + my $parent_grp = $self->get('-parent_grp'); + $self->{main_group} = $zinc->add('group', $parent_grp, -visible => 1); + $zinc->coords($self->{main_group}, $origin); + $zinc->add('rectangle', $self->{main_group}, [0, 0, $width, $height], + -visible => 1, -linecolor => "green"); + my $vmargin = $self->get('-height')*0.1; + $self->{vmargin} = $vmargin; + my $hmargin = 10; + $zinc->add('text', $self->{main_group}, + -position => [$hmargin, $vmargin/2], + -anchor => 'w', + -text => scalar $self->get('-title'), + -color => 'white', + ); +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/PFD.pm b/sw/ground_segment/cockpit/Paparazzi/PFD.pm new file mode 100644 index 0000000000..dc1125dcc9 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/PFD.pm @@ -0,0 +1,199 @@ + +package Paparazzi::PFD; +use Subject; +@ISA = ("Subject"); + +use strict; + +use Math::Trig; +use Tk; +use Tk::Zinc; + +use Paparazzi::Scale; +use Paparazzi::LensScale; +use Paparazzi::Horizon; +use Paparazzi::PFD_Panel; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec + (-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + + -roll => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -pitch => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + + -speed => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -target_speed => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -alt => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -target_alt => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -heading => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -target_heading => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -vz => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + + -ap_mode => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -gps_mode => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -lls_mode => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -lls_value => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -ctrst_mode => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -ctrst_value => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -rc_mode => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + -if_mode => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], + + -nav_dist_wp => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -wind => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit(); + + $self->build_gui(); +# $self->configure( -speed => 9); +# $self->configure( -target_speed => 10); +# $self->configure( -alt => 140); +# $self->configure( -target_alt => 150); +# $self->configure( -roll => 20); +# $self->configure( -pitch => 10); + $self->configure('-pubevts' => 'SHOW_PAGE'); +} + +sub nav_dist_wp { + my ($self, $previous_d, $new_d) = @_; + my $str2 = sprintf("dtwp %.0f m", sqrt($new_d)); + $self->get('-zinc')->itemconfigure( $self->{nav_tab}, 2, + -text => $str2, + -color => 'white', + ); +} + +sub wind { + my ($self, $previous_w, $new_w) = @_; +# my ($dir, $speed, $mean_as, $stddev) = $new_w; + + if ($new_w != 0) { + my $dir_deg = (rad2deg($new_w->{-dir}) + 180)% 360; + my $wind1_str = sprintf ("%.0fdeg %.1f m/s", $dir_deg, $new_w->{-speed}); + my $wind2_str = sprintf ("mas %.1f m/s (%.2f)", $new_w->{-mean_as}, $new_w->{-stddev}); + $self->get('-zinc')->itemconfigure( $self->{wind_tab}, 1, + -text => $wind1_str, + -color => 'white', + ); + $self->get('-zinc')->itemconfigure( $self->{wind_tab}, 2, + -text => $wind2_str, + -color => 'white', + ); + } +} + +sub onPanelCLicked { + print "in PFD::onPanelClicked\n"; + my ($self, $component, $signal, $page) = @_; + print "$signal, $page\n"; + $self->notify('SHOW_PAGE', $page); +} + +sub min { + my ($a, $b) = @_; + return $a le $b ? $a : $b; +} +sub build_gui() { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + my $origin = $self->get('-origin'); + + $self->{main_group} = $zinc->add('group', 1, -visible => 1); + $zinc->coords($self->{main_group}, $origin); + my ($p_x, $p_y, $p_w, $p_h) = (0, 0.02*$width, $width, 0.12*$height); + my $component = $self->component('Paparazzi::PFD_Panel', + -zinc => $zinc, + -parent_grp => $self->{main_group}, + -origin => [$p_x, $p_y], + -width => $p_w, + -height => $p_h, + ); + $component->attach($self, 'CLICKED', ['onPanelCLicked']); + + + my ($c_x, $c_y, $radius) = ($width/2, $height/2, 0.3*min($width, $height)); + $self->component('Paparazzi::Horizon', + -zinc => $zinc, + -parent_grp => $self->{main_group}, + -origin => [$c_x, $c_y], + -radius => $radius, + ); + + $self->{speed_scale} = Paparazzi::Scale->new( -zinc => $zinc, + -parent_grp => $self->{main_group}, + -origin => [ 0.1*$width, 0.25*$height], + -width => 0.15*$width, + -height => 0.56*$height, + -min_val => 0, + -max_val => 40, + -tick_scale => 1, + -repeat_legend => 2, + ); + $self->connectoptions(-speed, S_TO, [$self->{speed_scale}, -value]); + $self->connectoptions(-target_speed, S_TO, [$self->{speed_scale}, -target_value]); + $self->{heading_scale} = Paparazzi::Scale->new( -zinc => $zinc, + -parent_grp => $self->{main_group}, + -origin => [ 0.25*$width, 0.96*$height], + -width => 0.5*$width, + -height => 0.08*$height, + -direction => -1, + -periodic => 1, + -min_val => 0, + -max_val => 360, + -tick_scale => 5, + -repeat_legend => 3, + ); + $self->connectoptions(-heading, S_TO, [$self->{heading_scale}, -value]); + $self->connectoptions(-target_heading, S_TO, [$self->{heading_scale}, -target_value]); + $self->{alt_scale} = Paparazzi::LensScale->new( -zinc => $zinc, + -parent_grp => $self->{main_group}, + -origin => [0.8*$width, 0.25*$height], + -width => 0.15*$width, + -height => 0.56*$height, + -min_val => 0, + -max_val => 3000, + -tick_scale => 5, + -repeat_legend => 2, + -fig_clm_pc => 0.6, + ); + $self->connectoptions(-alt, S_TO, [$self->{alt_scale}, -value]); + $self->connectoptions(-target_alt, S_TO, [$self->{alt_scale}, -target_value]); + $self->connectoptions(-vz, S_TO, [$self->{alt_scale}, -vz]); + # wind informations + my $labelformat = '150x250 x140x20+20+10 x140x20^0>0 x140x20^0>1 x140x20^0>2 x140x20^0>3'; + my $f = '-adobe-helvetica-bold-o-normal--16-240-100-100-p-182-iso8859-1'; + $self->{wind_tab} = $zinc->add('tabular',$self->{main_group}, 5, + -position => [0, + 600], + -labelformat => $labelformat, + ); + $zinc->itemconfigure ( $self->{wind_tab}, 0, + -font => $f, + -color => 'white', + -text => 'Wind', + ); + # nav informations + $self->{nav_tab} = $zinc->add('tabular',$self->{main_group}, 5, + -position => [000, + 600], + -labelformat => $labelformat, + ); + $zinc->itemconfigure ( $self->{nav_tab}, 0, + -font => $f, + -color => 'white', + -text => 'NAV', + ); +} + + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/PFD_Panel.pm b/sw/ground_segment/cockpit/Paparazzi/PFD_Panel.pm new file mode 100644 index 0000000000..2cefa59d2f --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/PFD_Panel.pm @@ -0,0 +1,237 @@ +# The zone above the artificial horizon which gives informations on ap mode, GPS etc.. +#============================================================================= +package Paparazzi::PFD_Panel; +use Subject; +@ISA = ("Subject"); + +use Tk; +use Tk::Zinc; +use Data::Dumper; + +use strict; +sub populate { + my ($self, $args) = @_; + + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -gps_mode => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -ap_mode => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -rc_mode => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -ctrst_mode => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -ctrst_value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -lls_mode => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -lls_value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -if_mode => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], +# -pubevts => [S_NEEDINIT, S_PASSIVE, S_RDWR, S_APPEND, S_NOPRPG,[]] + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit(); + $self->{modes} = [ { name => 'rc', + str => ["lost","ok", "really lost", "not possible"], + color => ["orange", "green", "red", "red"] + }, + { name => 'cal', + str => ["unkwn", "wait", "ok"], + color =>["red", "orange", "green"] + }, + { name => 'ap', + str => ["manual", "auto1", "auto2", "home"], + color =>["green", "green", "green", "orange"] + }, + { name => 'gps', + str => [ "No fix", + "dead reckoning only", + "2D-fix", + "3D-fix", + "GPS + dead reckoning combined"], + color => ["red", "red", "orange", "green", "orange"] + }, + { name => 'lls', + str => ["OFF" , "ON"], + color =>["orange", "green"] + }, + { name => 'if', + str => ["none", "down", "up"], + color =>["green", "orange", "orange"] + } + ]; + $self->{modes_by_name} = {}; + foreach my $mode (@{$self->{modes}}) { + $self->{modes_by_name}->{$mode->{name}} = $mode; + } + $self->build_gui(); + $self->configure('-pubevts' => 'CLICKED'); +} + + +sub build_gui { + my ($self) = @_; + + my $zinc = $self->get('-zinc'); + my $parent_grp = $self->get('-parent_grp'); + $self->{main_group} = $zinc->add('group', $parent_grp, -visible => 1); + my @origin = $self->get('-origin'); + $zinc->coords($self->{main_group}, \@origin); + + + my $modes = $self->{modes}; + + my $nb_col = $#$modes+1; + +# print "nb_col $nb_col\n"; + + my $w = $self->get('-width'); + my $h = $self->get('-height'); + my $i; + for ($i=1; $i<$nb_col; $i++) { + my $x = $i / $nb_col * $w; + $zinc->add('curve', $self->{main_group}, + [$x, 10, $x, $h +10], + -linewidth => 2, + -linecolor => 'white', + -filled => 0); + } + + my $f = '-adobe-helvetica-bold-o-normal--16-240-100-100-p-182-iso8859-1'; + my $labelformat = "100x200 x80x20+20+10 x80x20^0>0 x80x20^0>1"; + my @tab_args = ('tabular',$self->{main_group}, 3, + -labelformat => $labelformat,); + my @tab_style = ( -font => $f, + -color => 'green'); + my $x=-10; + my $dx = $self->get('-width') / $nb_col; + foreach my $mode (@{$self->{modes}}) { + $mode->{tabular} = $zinc->add(@tab_args, -position => [$x, -5] ); + $zinc->itemconfigure ($mode->{tabular}, 0, @tab_style, -text => uc $mode->{name}); + $zinc->bind($mode->{tabular},''=> [\&onRectClicked, $self, $mode->{name}]); + $x += $dx; + } + + + $self->{rect_group} = $zinc->add('group', $self->{main_group}, -visible => 1); + my $rect = $zinc->add('rectangle', $self->{main_group}, [0, 0, $w, $h], + -visible => 0, + -linecolor => 'white', + -filled => '1', + ); +# $zinc->bind($rect,''=> [\&onRectClicked, $self, "coucou"]); + +} + + + +sub onRectClicked { + my ($zinc, $self, $name) = @_; + print "onRectClicked : $name\n"; + $self->notify('CLICKED', $name); +} + + + +sub set_mode { + my ($self, $name, $previous_val, $new_val) = @_; + my $mode = $self->{modes_by_name}->{$name}; + if (defined $mode) { + if (!defined $previous_val || $previous_val != $new_val) { + my $zinc = $self->get('-zinc'); + $zinc->itemconfigure( $mode->{tabular}, 1, + -text => $mode->{str}[$new_val], + -color =>$mode->{color}[$new_val], + ); + } + } +} + +sub gps_mode() { + my ($self, $previous_mode, $new_mode) = @_; + $self->set_mode("gps", $previous_mode, $new_mode); + +} + +sub ap_mode() { + my ($self, $previous_mode, $new_mode) = @_; + $self->set_mode("ap", $previous_mode, $new_mode); +} + +sub rc_mode { + my ($self, $previous_mode, $new_mode) = @_; + $self->set_mode("rc", $previous_mode, $new_mode); +} + +sub lls_mode { + my ($self, $previous_mode, $new_mode) = @_; + $self->set_mode("lls", $previous_mode, $new_mode); +} + +sub if_mode { + my ($self, $previous_mode, $new_mode) = @_; + $self->set_mode("if", $previous_mode, $new_mode); +} + +sub lls_value { + my ($self, $previous_val, $new_val) = @_; + my $mode = $self->{modes_by_name}->{lls}; + if (defined $mode) { + if (!defined $previous_val || $previous_val != $new_val) { + my $zinc = $self->get('-zinc'); + my $str_val = sprintf ("%.4f", $new_val); + $zinc->itemconfigure( $mode->{tabular}, 2, + -text => $str_val, + -color => "green", + ); + } + } +} + + + +sub ctrst_mode { + my ($self, $previous_mode, $new_mode) = @_; + $self->set_mode("ctrst", $previous_mode, $new_mode); +} + +sub ctrst_value { + my ($self, $previous_val, $new_val) = @_; + my $mode = $self->{modes_by_name}->{ctrst}; + if (defined $mode) { + if (!defined $previous_val || $previous_val != $new_val) { + my $zinc = $self->get('-zinc'); + my $str_val = sprintf ("%.4f", $new_val); + $zinc->itemconfigure( $mode->{tabular}, 2, + -text => $str_val, + -color => "green", + ); + } + } +} + + +sub setLabelContent { + my ($self, $item, $labelformat) = @_; + + my @fieldsSpec = split (/ / , $labelformat); + shift @fieldsSpec; + + my $i=0; + foreach my $fieldSpec (@fieldsSpec) { + my ($posSpec) = $fieldSpec =~ /^.\d+.\d+(.*)/ ; + print "$fieldSpec\t$i\t$posSpec\n"; + $self->{zinc}->itemconfigure ($item,$i, + -text => "$i: $posSpec", + -border => "contour", + -color => 'green', + -backcolor => 'white', + -filled => 1 + ); + $i++; + } +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/RCSlider.pm b/sw/ground_segment/cockpit/Paparazzi/RCSlider.pm new file mode 100644 index 0000000000..978082cd72 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/RCSlider.pm @@ -0,0 +1,138 @@ +package Paparazzi::RCSlider; +use Subject; +@ISA = ("Subject"); + +use Tk; +use Tk::Zinc; +use Math::Trig; + +use strict; + +use constant CURSOR_WIDTH => 2; +use constant VERTICAL_CONTROL => 0; +use constant HORIZONTAL_CONTROL => 1; + +sub populate { + my ($self, $args) = @_; + + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -len => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -direction => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -name => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->build_gui(); +} + +sub value() { + my ($self, $previous_value, $new_value) = @_; + my $zinc = $self->get('-zinc'); + my $len = $self->get('-len'); + my $new_c = $new_value * $len/2; + my $cursor_item = $self->{'cursor_item'}; + $zinc->treset($cursor_item); + if ($self->get('-direction') == VERTICAL_CONTROL) { + $zinc->translate($cursor_item, 0, $new_c); + } + else { + $zinc->translate($cursor_item, $new_c, 0); + } +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $main_group = $zinc->add('group', 1, -visible => 1); + $self->{main_group} = $main_group; + my @origin = $self->get('-origin'); + $zinc->coords($main_group, \@origin); + my $w = $self->get('-width'); + my $l = $self->get('-len'); + my $d = $self->get('-direction'); + my $name = $self->get('-name'); + my $rectangle_coor = ($d == VERTICAL_CONTROL) ? + [-$w/2, 0, $w/2, -$l] : [0, -$w/2, $l, $w/2]; + my $cursor_coor = ($d == VERTICAL_CONTROL) ? + [-$w/2, -$l/2 - CURSOR_WIDTH, $w/2, -$l/2+CURSOR_WIDTH] : + [$l/2 - CURSOR_WIDTH, -$w/2, $l/2+CURSOR_WIDTH, $w/2]; + + $zinc->add('text', $main_group, + -position =>[0, 0], + -color => 'white', + -anchor => ($d == VERTICAL_CONTROL) ? 'n':'e', + -text => $name + ); + $zinc->add('rectangle', $main_group , + $rectangle_coor, + -linewidth => 1, + -linecolor => 'black', + -filled => 1, + -fillcolor => 'white', + ); + my $cursor_item = $zinc->add('rectangle', $main_group , + $cursor_coor, + -linewidth => 1, + -linecolor => 'black', + -filled => 1, + -fillcolor => 'yellow', + ); + $self->{'cursor_item'} = $cursor_item; + $zinc->bind($cursor_item, '' => [\&press, $self, \&motion]); + $zinc->bind($cursor_item, '' => [\&release, $self]); +} + +my ($x_orig, $y_orig); + +sub press { + my ($zinc, $self, $action) = @_; + my $ev = $zinc->XEvent(); + my @origin=$zinc->coords($self->{main_group}); + $x_orig = $ev->x - $origin[0]; + $y_orig = $ev->y - $origin[1]; + $zinc->Tk::bind('', [$action, $self]); + } + +sub motion { + my ($zinc, $self) = @_; + my $ev = $zinc->XEvent(); + my @origin=$zinc->coords($self->{main_group}); + my $x = $ev->x - $origin[0]; + my $y = $ev->y - $origin[1]; + + if ($self->get('-direction') == VERTICAL_CONTROL) { + if ($y > -$self->get('-len') and $y < 0) { + $zinc->translate($self->{cursor_item}, 0, $y-$y_orig); + $y_orig = $y; + } + } + else { + if ($x < $self->get('-len') and $x > 0) { + $zinc->translate($self->{cursor_item}, $x-$x_orig, 0); + $x_orig = $x; + } + } +} + +sub release { + my ($zinc, $self) = @_; + $zinc->Tk::bind('', ''); + my $ev = $zinc->XEvent(); + my @origin=$zinc->coords($self->{main_group}); + my $x = $ev->x - $origin[0]; + my $y = $ev->y - $origin[1]; + my $len = $self->get('-len'); + my $value = Utils::trim($self->get('-direction') == VERTICAL_CONTROL ? + -2 * $y/$len - 1. : 2 * $x/$len - 1, -1., 1.); + print "slider release ( $value )\n"; + +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/RCStick.pm b/sw/ground_segment/cockpit/Paparazzi/RCStick.pm new file mode 100644 index 0000000000..0c084eecfe --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/RCStick.pm @@ -0,0 +1,131 @@ +package Paparazzi::RCStick; +use Subject; +@ISA = ("Subject"); + +use Paparazzi::RCSlider; +use Paparazzi::Utils; + +use Tk; +use Tk::Zinc; +use Math::Trig; + +use strict; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -radius => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -name => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -v_name => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -h_name => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -v_value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -h_value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->build_gui(); + $self->connectoptions('-v_value', S_TO, [$self->{v_slider}, '-value']); + $self->connectoptions('-h_value', S_TO, [$self->{h_slider}, '-value']); +} + +sub h_value() { + my ($self, $previous_value, $new_value) = @_; +# print "in h_value $new_value\n"; +} + +sub v_value() { + my ($self, $previous_value, $new_value) = @_; +} + +use constant WIDTH => 14; + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $radius = $self->get('-radius'); + my @origin = $self->get('-origin'); + my @v_origin = ($origin[0], $origin[1] + $radius); + my @h_origin = ($origin[0] - $radius, $origin[1]); + my $v_name = $self->get('-v_name'); + my $h_name = $self->get('-h_name'); + + $self->{v_slider} = Paparazzi::RCSlider->new( -zinc => $zinc, -origin => \@v_origin, + -width => WIDTH, -len => 2 * $radius, + -direction => Paparazzi::RCSlider::VERTICAL_CONTROL, + -name => $v_name + ); + $self->{h_slider} = Paparazzi::RCSlider->new( -zinc => $zinc, -origin => \@h_origin, + -width => WIDTH, -len => 2 * $radius, + -direction => Paparazzi::RCSlider::HORIZONTAL_CONTROL, + -name => $h_name + ); + + my $main_group = $zinc->add('group', 1, -visible => 1); + $zinc->coords($main_group, \@origin); + $self->{main_group} = $main_group; + + + my $cursor_coor = [ - WIDTH/2, - WIDTH/2, WIDTH/2, WIDTH/2 ]; + + my $cursor_item = $zinc->add('arc', $main_group, + $cursor_coor, + -filled => 1, + -fillcolor => "yellow", + -linewidth => 1, + -linecolor => "black", + -startangle => 0, -extent => 360, + -pieslice => 1, -closed => 1, + ); + $self->{'cursor_item'} = $cursor_item; + $zinc->bind($cursor_item, '' => [\&press, $self, \&motion]); + $zinc->bind($cursor_item, '' =>[\&release, $self]); +} + +my ($x_orig, $y_orig); + + sub press { + my ($zinc, $self, $action) = @_; + my $ev = $zinc->XEvent(); + my @origin=$zinc->coords($self->{main_group}); + $x_orig = $ev->x - $origin[0]; + $y_orig = $ev->y - $origin[1]; + $zinc->Tk::bind('', [$action, $self]); + } + +sub motion { + my ($zinc, $self) = @_; + my $ev = $zinc->XEvent(); + my @origin=$zinc->coords($self->{main_group}); + my $x = $ev->x - $origin[0]; + my $y = $ev->y - $origin[1]; + my $radius = $self->get('-radius'); + + if ($y > -$radius and $y < $radius) { + $zinc->translate($self->{cursor_item}, 0, $y-$y_orig); + $y_orig = $y; + } + if ($x > -$radius and $x < $radius) { + $zinc->translate($self->{cursor_item}, $x-$x_orig, 0); + $x_orig = $x; + } +} + +sub release { + my ($zinc, $self) = @_; + $zinc->Tk::bind('', ''); + my $ev = $zinc->XEvent(); + my @origin=$zinc->coords($self->{main_group}); + my $x = $ev->x - $origin[0]; + my $y = $ev->y - $origin[1]; + my $radius = $self->get('-radius'); + my ($v_value, $h_value) = (Utils::trim($y/$radius, -1., 1.), Utils::trim($x/$radius, -1, 1)); + print "stick release ( $v_value, $h_value ) \n"; + $self->configure('-v_value' => $v_value, '-h_value' => $h_value); +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/RCTransmitter.pm b/sw/ground_segment/cockpit/Paparazzi/RCTransmitter.pm new file mode 100644 index 0000000000..a426957acc --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/RCTransmitter.pm @@ -0,0 +1,84 @@ +package Paparazzi::RCTransmitter; + +use strict; + +use Tk; +use Tk::Zinc; +use XML::DOM; + +use base "Tk::Frame"; +use strict; + +use Paparazzi::RCSlider; +use Paparazzi::RCStick; + +Construct Tk::Widget 'Paparazzi::RCTransmitter'; + +use constant TYPE_SLIDER => 0; +use constant TYPE_STICK => 1; +use constant TYPE_SWITCH => 2; + +use constant VERTICAL_CONTROL => 0; +use constant HORIZONTAL_CONTROL => 1; + +sub ClassInit { + my ($class, $mw) = @_; + $class->SUPER::ClassInit($mw); +} + +sub Populate { + my ($self, $args) = @_; + $self->SUPER::Populate($args); + $self->ConfigSpecs( -filename => ['PASSIVE', undef, undef, undef], + -width => ['PASSIVE', undef, undef, undef], + -height => ['PASSIVE', undef, undef, undef]); + $self->{zinc} = $self->Zinc( +# -width => $args->{-width}, -height => $args->{-height}, + -backcolor => 'black', + -borderwidth => 3, + -relief => 'sunken', + -render => '1'); + $self->{main_group} = $self->{zinc}->add('group', 1, -visible => 1); + + my $filename = $args->{-filename}; + my $parser = XML::DOM::Parser->new(); + my $doc = $parser->parsefile($filename); + foreach my $radio ($doc->getElementsByTagName('radio')) { + my $name = $radio->getAttribute('name'); + print "name $name\n"; + my $img_filename = $doc->getElementsByTagName('photo')->[0]->getAttribute('filename'); + print "name $img_filename\n"; + my $image = $self->{zinc}->Photo("bg_picture", -file => $img_filename); + $args->{-width} = $image->width(); + $args->{-height} = $image->height; + $self->{zinc}->configure(-width => $image->width(), -height => $image->height()); + + my $img_item = $self->{zinc}->add('icon', $self->{main_group}, -image => $image); + foreach my $control ($doc->getElementsByTagName('control')) { + if ($control->getAttribute('type') eq 'stick') { + Paparazzi::RCStick->new( -zinc => $self->{zinc}, + -origin => [ $control->getAttribute('x'), $control->getAttribute('y')], + -radius => $control->getAttribute('size'), + -name => $control->getAttribute('name'), + -v_name => $control->getAttribute('v_axe'), + -h_name => $control->getAttribute('h_axe') + ); + } + elsif ($control->getAttribute('type') eq 'slider') { + Paparazzi::RCSlider->new( -zinc => $self->{zinc}, + -origin => [$control->getAttribute('x'), $control->getAttribute('y')], + -width => 14, -len => $control->getAttribute('size'), + -direction => $control->getAttribute('direction') eq "horizontal"? + HORIZONTAL_CONTROL : VERTICAL_CONTROL, + -name => $control->getAttribute('name') + ); + } + } + } + $self->{zinc}->pack(-fill => 'both', -expand => "1"); +} + + + + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/RotaryGauge.pm b/sw/ground_segment/cockpit/Paparazzi/RotaryGauge.pm new file mode 100644 index 0000000000..b9233bef43 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/RotaryGauge.pm @@ -0,0 +1,124 @@ +package Paparazzi::RotaryGauge; +use Subject; +@ISA = ("Subject"); +use strict; + + +use Tk; +use Tk::Zinc; +use Math::Trig; +use Data::Dumper; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + my ($min_val, $max_val, $tick_spacing, $legend_spacing, $min_val_angle, $dead_sector) = (0, 100, 10, 20, 0, 45); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -radius => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -min_val => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, $min_val], + -max_val => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, $max_val], + -tick_spacing => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, $tick_spacing], + -legend_spacing => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, $legend_spacing], + -min_val_angle => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, $min_val_angle], + -dead_sector => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, $dead_sector], + -text => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, ""], + -format => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, "%.1f"], + -value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->{ang_by_val} = Utils::rad_of_deg((360. - $self->get('-dead_sector')) / + ( $self->get('-max_val') - $self->get('-min_val'))); + $self->build_gui(); +} + +sub angle_of_value { + my ($self, $value) = @_; + my $min_val_angle = $self->get('-min_val_angle'); + my $aov = Utils::rad_of_deg($min_val_angle) + $self->{ang_by_val} * ($value - $self->get('-min_val')); +# print "angle_of_value $value -> $aov\n"; + return $aov; +} + +sub value { + my ($self, $old_val, $new_val) = @_; + return unless defined $new_val and defined $self->{ang_by_val}; + my $zinc = $self->get('-zinc'); + my $angle = $self->angle_of_value($new_val); + $zinc->treset($self->{rotate_group}); + $zinc->rotate($self->{rotate_group}, $angle + 1.57, 0, 0); + return unless defined $self->{value_label} and defined $self->get('-format'); + my $name = $self->get('-text'); + $zinc->itemconfigure($self->{value_label}, -text => sprintf($self->get('-format'), $new_val)); +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $origin = $self->get('-origin'); + my $parent_grp = $self->get('-parent_grp'); + my $radius = $self->get('-radius'); + $self->{main_group} = $zinc->add('group', $parent_grp, -visible => 1); + $zinc->coords($self->{main_group}, [$origin->[0]+$radius, $origin->[1]+$radius] ); + my $rad = 0.7*$radius; + $zinc->add('rectangle', $self->{main_group}, [-1.15*$radius, -1.15*$radius, 1.15*$radius, 1.15*$radius+25], + -visible => 1, + -linecolor => 'white', + ); + $zinc->add('arc', $self->{main_group}, + [-$rad, -$rad, $rad, $rad], + -visible => 1, + -linecolor => 'white', + -filled => 0, + ); + $self->{rotate_group} = $zinc->add('group', $self->{main_group}, -visible => 1); + my $min_val_angle = $self->angle_of_value(scalar $self->get('-min_val')); + my ($p1x, $p1y) = ($rad*cos($min_val_angle), $rad*sin($min_val_angle)); + $zinc->add('curve', $self->{rotate_group}, + [0.1*$p1x, 0.1*$p1y, $p1x, $p1y], + -linewidth => 2, + -linecolor => 'white', + -filled => 0); + + # ticks + for (my $val = $self->get('-min_val'); $val < $self->get('-max_val'); $val+= $self->get('-tick_spacing')) { + my $angle = $self->angle_of_value($val); +# print "tick angle_of_value $val -> $angle\n" if ($self->get('-text') eq "bat"); + my ($px, $py) = ($radius * cos($angle), $radius * sin($angle)); + $zinc->add('curve', $self->{main_group}, + [0.7*$px, 0.7*$py, 0.8*$px, 0.8*$py], + -linewidth => 2, + -linecolor => 'white', + ); + } + # legend + for (my $val = $self->get('-min_val'); $val < $self->get('-max_val'); $val+= $self->get('-legend_spacing')) { + my $angle = $self->angle_of_value($val); + $self->{label} = $zinc->add('text', $self->{main_group}, + -position => [$radius * cos($angle), $radius * sin($angle)], + -text => sprintf("%.0f", $val), + -color => 'white', + -anchor => 'c', + ); + } + # title + my $text = $self->get('-text'); + $self->{text_label} = $zinc->add('text', $self->{main_group}, + -position => [0, $radius+8], + -text => $text, + -color => 'white', + -anchor => 'c', + ); + # value + $self->{value_label} = $zinc->add('text', $self->{main_group}, + -position => [0, $radius + 20], + -text => "", + -color => 'white', + -anchor => 'c', + ); +} diff --git a/sw/ground_segment/cockpit/Paparazzi/SatPage.pm b/sw/ground_segment/cockpit/Paparazzi/SatPage.pm new file mode 100644 index 0000000000..03f8296f8c --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/SatPage.pm @@ -0,0 +1,145 @@ +package Paparazzi::SatPage; +use Paparazzi::NDPage; +@ISA = ("Paparazzi::NDPage"); +use strict; +use Subject; + +use Paparazzi::SatSigView; + +use constant TITLE => "Satellites"; +use constant MAX_CH => 16; + +sub populate { + my ($self, $args) = @_; + $args->{-title} = TITLE; + $self->SUPER::populate($args); + $self->configspec( + -sats => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, undef], + -fix => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, undef], + ); +} + + +sub sats { + my ($self, $old_val, $new_val) = @_; + return unless defined $new_val; + + my $itow = $new_val->{-itow}; + my $nb_ch = $new_val->{-nch}; + my $sats = $new_val->{-sats}; + my $zinc = $self->get('-zinc'); + foreach my $sat (@{$sats}) { + my $sat_obj = $self->{satellites}->[$sat->{-chn}]; + $zinc->coords($sat_obj->{-group}, $self->get_pos($sat->{-elev}, $sat->{-azim})); + $zinc->itemconfigure ($sat_obj->{-group}, + -visible => 1, + ); + $zinc->itemconfigure ($sat_obj->{-arc}, + -fillcolor => $sat->{-flags} & 0x01 ? "green3" : "red", + ); + $zinc->itemconfigure ($sat_obj->{-id_lab}, + -text => sprintf("%d", $sat->{-svid}), + ); + $sat_obj->{-sig_view}->configure(-sat => $sat); + } +} + + +sub fix { + my ($self, $old_val, $new_val) = @_; +# print "in fix\n"; + return unless defined $new_val and defined $self->{rg}; +# print "in fix2\n"; +} + + +sub get_pos { + my ($self, $elev, $azim) = @_; + my $sky_radius = $self->{sky_radius}; + my $azim_rad = Utils::rad_of_deg($azim); + + use constant LIN => 1; + + my $k_elev = LIN ? 1 - $elev/90 : 1 - sin(Utils::rad_of_deg($elev)); + my $x = $sky_radius * 1 * sin($azim_rad) * $k_elev; + my $y = $sky_radius * -1 * cos($azim_rad) * $k_elev; + return [$x, $y]; +} + +sub build_gui { + my ($self) = @_; + $self->SUPER::build_gui(); + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + $self->{sky_group} = $zinc->add('group', $self->{main_group}); + my $margin = 10; + my $sky_radius = Utils::min($width, $height)*0.55/2; + $zinc->coords($self->{sky_group}, [$sky_radius+$margin, $sky_radius+$margin+$self->{vmargin}]); + $self->{sky_radius} = $sky_radius; + # elevation scale + for (my $elev = 0; $elev < 90; $elev += 15) { + my $rad = $self->get_pos($elev, 0)->[1]; + $zinc->add('arc', $self->{sky_group}, + [-$rad, -$rad, $rad, $rad], + -visible => 1, + -linecolor => 'white', + -filled => 0, + ); + $zinc->add('text', $self->{sky_group}, + -position => $self->get_pos(90-$elev, 180-$elev), + -text => sprintf("%.0f", $elev), + -color => 'white', + -anchor => 'c', + ); + } + # azimut scale + my $tick_font = '-adobe-helvetica-bold-o-normal--24-240-100-100-p-182-iso8859-1'; + my $ticks = [["S", [0, $sky_radius]], ["E", [ $sky_radius, 0]], + ["N", [0, -$sky_radius]], ["W", [-$sky_radius, 0]]]; + foreach my $tick (@{$ticks}) { + my ($txt, $pos) = @{$tick}; + $zinc->add('text', $self->{sky_group}, + -position => $pos, + -text => $txt, + -color => 'white', + -font => $tick_font, + -anchor => 'c', + ); + } + my $pos_sig_x = 0.6*$width+$margin;; + my $h_sig = $height/ (MAX_CH +1); + my $satellites = []; + my $sat_r = $sky_radius/8; + for (my $chn=0; $chn < MAX_CH; $chn++) { + my $sat_group = $zinc->add('group', $self->{sky_group}, -visible => 0); + my $sat_arc = $zinc->add('arc', $sat_group, [- $sat_r, - $sat_r, $sat_r, $sat_r], + -visible => 1, + -linecolor => 'white', + -filled => 1, + ); + my $id_lab = $zinc->add('text', $sat_group, + -position => [0, 0], + -text => "$chn", + -color => 'white', + -anchor => 'c', + ); + my $sat_sig_view = Paparazzi::SatSigView->new(-zinc => $zinc, + -width => $width/3, + -height => $h_sig, + -origin => [$pos_sig_x, ($chn+0.5) * $h_sig], + -parent_grp => $self->{main_group}, + ); + + push @{$satellites}, { + -sig_view => $sat_sig_view, + -group => $sat_group, + -arc => $sat_arc, + -id_lab => $id_lab, + -elev => 0, + -azim => 0, + }; + + } + $self->{satellites} = $satellites; +} diff --git a/sw/ground_segment/cockpit/Paparazzi/SatSigView.pm b/sw/ground_segment/cockpit/Paparazzi/SatSigView.pm new file mode 100644 index 0000000000..8e412a9321 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/SatSigView.pm @@ -0,0 +1,67 @@ +package Paparazzi::SatSigView; +use Subject; +@ISA = ("Subject"); +use strict; + +use constant MAX_CH => 16; + +use Tk; +use Tk::Zinc; +use Math::Trig; +use Data::Dumper; + +use Paparazzi::Utils; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -sat => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, undef], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->build_gui(); +} + +sub sat { + my ($self, $old_val, $new_val) = @_; + return unless defined $new_val; + $self->get('-zinc')->itemconfigure($self->{-id_lab}, -text => sprintf("%d", $new_val->{-svid})); + $self->get('-zinc')->itemconfigure($self->{-sig_lab}, -text => sprintf("%.1f db", $new_val->{-cno})); +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + my $origin = $self->get('-origin'); + my $parent_grp = $self->get('-parent_grp'); + $self->{main_group} = $zinc->add('group', $parent_grp, -visible => 1); + $zinc->coords($self->{main_group}, $origin); + $zinc->add('rectangle', $self->{main_group}, [0, 0, $width, $height], + -visible => 1, +# -fillcolor => 'red', -filled => 1 + -linecolor => 'white', + ); + $self->{-id_lab} = $zinc->add('text', $self->{main_group}, + -position => [2, 1], + -color => 'white', + -anchor => 'nw', + ); + $self->{-sig_lab} = $zinc->add('text', $self->{main_group}, + -position => [$width/3, 1], + -color => 'white', + -anchor => 'nw', + ); +} + + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/Scale.pm b/sw/ground_segment/cockpit/Paparazzi/Scale.pm new file mode 100644 index 0000000000..c1e649db9c --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/Scale.pm @@ -0,0 +1,206 @@ +#============================================================================= +# Scale Class +#============================================================================= +package Paparazzi::Scale; +use Subject; +@ISA = ("Subject"); +use strict; + +use Tk; +use Tk::Zinc; +use Math::Trig; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -direction => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 1.], + -periodic => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 0], + -min_val => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 0.], + -max_val => [S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 100.], + -disp_tick =>[S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 10.], + -tick_scale =>[S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 1.], + -repeat_legend =>[S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 2.], + -fig_clm_pc =>[S_NOINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, 0.7], + -value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -target_value => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + ); + $self->{value_y} = 0; + $self->{y_per_tick} = 1.; +} + +sub completeinit { +# print "in Scale::completeinit\n"; + my $self = shift; + $self->SUPER::completeinit; + $self->build_gui(); +} + +sub value() { + my ($self, $previous_value, $new_value) = @_; + my $zinc = $self->get('-zinc'); + + my $nb_ticks = ($new_value - $self->get('-min_val')) / $self->get('-tick_scale'); +# print "nb_ticks $nb_ticks\n"; + my $new_y = $nb_ticks * $self->{y_per_tick}; + $new_y = -$new_y if ( $self->get('-direction') < 0); + +# my $new_y = ($new_value / $self->get('-tick_scale')) * $self->{y_per_tick}; + $self->{value_y} = $new_y; + $zinc->treset($self->{moving_group}); + $zinc->translate($self->{moving_group}, 0, $new_y); +} + +sub target_value() { + my ($self, $previous_value, $new_value) = @_; + my $zinc = $self->get('-zinc'); + my $y = $self->get_y_from_value($new_value); + $zinc->treset($self->{target_marker}); + $zinc->translate($self->{target_marker}, 0, $y); + $zinc->itemconfigure ($self->{target_up_label}, + -text => sprintf("%.1f", $new_value), + ); +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + + my $h; my $w; +if ($self->get('-height') > $self->get('-width')) { + $h = $self->get('-height'); + $w = $self->get('-width'); +} + else { + $w = $self->get('-height'); + $h = $self->get('-width'); +} + $self->{y_per_tick} = $h/$self->get('-disp_tick')+1; +# printf ("y_per_tick %d\n" , $self->{y_per_tick}); + my $rc_pc = $self->get('-fig_clm_pc'); # figures column per cent width + my $rc_x = $rc_pc * $w; # figures column x coordinate + my $tick_pc = 0.1; # tick per cent widht + my $tick_x = ($rc_pc-$tick_pc)*$w;# tick x coordinate + + my $arrow_height = 10; + my $arrow_width = 10; + + my $parent_grp = $self->get('-parent_grp'); + $self->{main_group} = $zinc->add('group', $parent_grp, -visible => 1); + my @origin = $self->get('-origin'); + $zinc->coords($self->{main_group}, \@origin); +# $zinc->translate($self->{main_group}, 0, $h/2); + + $self->{fixed_group} = $zinc->add('group', $self->{main_group}, -visible => 1); + + $self->{clipping_group} = $zinc->add('group',$self->{main_group}, -visible => 1); + $self->{itemclip} = $zinc->add('rectangle', $self->{clipping_group}, [-10, 0, $w, $h], + -visible => 0); + $zinc->itemconfigure($self->{clipping_group}, -clip => $self->{itemclip}); + + $self->{moving_group} = $zinc->add('group',$self->{clipping_group}, -visible => 1); + + $zinc->add('rectangle', $self->{fixed_group} , + [0, 0, $rc_x, $h], + -linewidth => 0, + -filled => 1, + -fillcolor => 'gray60'); + $zinc->add('rectangle', $self->{fixed_group} , + [ $rc_x, 0, $w, $h], + -linewidth => 0, + -filled => 1, + -fillcolor => 'black'); + $zinc->add('curve', $self->{fixed_group}, + [0, 0, $w, 0], + -linewidth => 2, + -linecolor => 'white', + -filled => 0); + $zinc->add('curve', $self->{fixed_group}, + [0, $h, $w, $h], + -linewidth => 2, + -linecolor => 'white', + -filled => 0); + $zinc->add('curve', $self->{fixed_group}, + [$rc_x, 0, $rc_x, $h], + -linewidth => 2, + -linecolor => 'white', + -filled => 0); + $zinc->add('curve', $self->{fixed_group}, + [$rc_x, $h/2 , + $rc_x + $arrow_width, $h/2 + $arrow_height/2, + $rc_x + $arrow_width, $h/2 - $arrow_height/2], + -linewidth => 2, + -linecolor => 'yellow', + -filled => 1, + -fillcolor => 'yellow', + -closed => 1); + + $self->{target_marker} = $zinc->add('curve', $self->{moving_group}, + [$rc_x-1, 0, + $rc_x + $arrow_width+1, $arrow_height/2+1, + $rc_x + $arrow_width+1,-$arrow_height/2-1], + -linewidth => 2, + -linecolor => 'HotPink1', + -filled => 0, + -closed => 1); + + + + $zinc->add('curve', $self->{fixed_group}, + [0, $h/2, $rc_x, $h/2], + -linewidth => 2, + -linecolor => 'yellow', + ); + + my $nb_ticks = ($self->get('-max_val') - $self->get('-min_val')) / $self->get('-tick_scale'); + my $tick_font = '-adobe-helvetica-bold-o-normal--18-240-100-100-p-182-iso8859-1'; + my $first_tick = $self->get('-periodic') ? -$nb_ticks:0; + my $last_tick = $self->get('-periodic') ? 2*$nb_ticks:$nb_ticks; + for (my $tick=$first_tick; $tick<=$last_tick; $tick++) { + my $value = ($self->get('-min_val') + $tick) * $self->get('-tick_scale'); + my $y = $self->get_y_from_value($value); + $zinc->add('curve', $self->{moving_group}, + [$tick_x, $y, $rc_x, $y], + -linewidth => 1, + -linecolor => 'white'); + if (!($tick%$self->get('-repeat_legend'))) { + my $text = sprintf("%d", $value % $self->get('-max_val')); + $zinc->add('text', $self->{moving_group}, + -position => [$tick_x/2, $y], + -color => 'white', + -font => $tick_font, + -anchor => 'c', + -text => $text); + } + } + + $self->{target_up_label} = + $zinc->add('text', $self->{main_group}, + -position => [$tick_x/2, 0], + -color => 'HotPink1', + -font => $tick_font, + -anchor => 's', + -text => ""); + + + if ($self->get('-width') > $self->get('-height')) { + @origin = $self->get('-origin'); + $zinc->rotate($self->{main_group}, - Math::Trig::pip2(), $origin[0], $origin[1]); + } +} + + +sub get_y_from_value { + my ($self, $value) = @_; + my $h = ($self->get('-height') > $self->get('-width')) ? + $self->get('-height') : $self->get('-width'); + my $nb_ticks = ($value - $self->get('-min_val')) / $self->get('-tick_scale'); + return $self->get('-direction') > 0 ? $h/2 - $nb_ticks * $self->{y_per_tick} : + $h/2 + $nb_ticks * $self->{y_per_tick}; +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/Strip.pm b/sw/ground_segment/cockpit/Paparazzi/Strip.pm new file mode 100644 index 0000000000..c9c8333d48 --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/Strip.pm @@ -0,0 +1,147 @@ +package Paparazzi::Strip; +use Subject; +@ISA = ("Subject"); + +use Data::Dumper; + +use strict; + +use Math::Trig; +use Tk; +use Tk::Zinc; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -parent_grp => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -name => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -selected => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 0], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit(); + $self->build_gui(); +} + +my $style = { + -linewidth => 3, + -linecolor => '#aaccff', + -fillcolor => 'back', + -relief => 'roundraised' + }; + +my $gradset = { + 'idnt' => '=axial 90 |#ffffff 0|#ffeedd 30|#e9d1ca 90|#e9a89a', + 'back' => '=axial 0 |#c1daff|#8aaaff', + 'shad' => '=path -40 -40 |#000000;50 0|#000000;50 92|#000000;0 100', + 'btn_outside' => '=axial 0 |#ffeedd|#8a9acc', + 'btn_inside' => '=axial 180 |#ffeedd|#8a9acc', + 'ch1' => '=axial 0 |#8aaaff|#5B76ED', + }; + +my @stripGradiants; + +sub init_gradiants { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + unless (@stripGradiants) { + my %gradiants = %{$gradset}; + my ($name, $gradiant); + while (($name, $gradiant) = each(%gradiants)) { + # création des gradients nommés + $zinc->gname($gradiant, $name) unless $zinc->gname($gradiant); + # the previous test is usefull only + # when this script is executed many time in the same process + # (it is typically the case in zinc-demos) + push(@stripGradiants, $name); + } + } +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + my $origin = $self->get('-origin'); + my $parent_grp = $self->get('-parent_grp'); + + $self->init_gradiants(); + + $self->{s_main_group} = $zinc->add('group', $parent_grp, -visible => 1); + $zinc->coords($self->{s_main_group}, $origin); + + my $ombre = $zinc->add('rectangle', $self->{s_main_group} , + [5, 5, $width+5, $height+5], + -filled => 1, + -linewidth => 0, + -fillcolor => 'shad', + -priority => 10, + ); + + $self->{-paper} = $zinc->add('rectangle', $self->{s_main_group} , + [0, 0, $width, $height], + -filled => 1, + -linewidth => $style->{'-linewidth'}, + -linecolor => $style->{'-linecolor'}, + -fillcolor => $style->{'-fillcolor'}, + -relief => $style->{'-relief'}, + -priority => 100 + ); + +# $zinc->bind ($self->{-paper},'',[\&OnButton1PressPaper,$self]); +# $zinc->bind ($self->{-paper},'',[\&OnButton1ReleasePaper,$self]); + +# my $texture = $zinc->Photo('background_texture.gif', +# -file => Tk->findINC("demos/zinc_data/background_texture.gif")); + # $zinc->itemconfigure($foo, -tile => $texture); + my $text = $self->get('-name'); + + $zinc->add('text', $self->{s_main_group}, + -position => [10, 10], + -color => 'white', +# -font => $v_tick_font, + -anchor => 'w', + -text => $text, + -priority => 110); + +} + +sub selected { + my ($self, $previous, $new) = @_; +# print ("in selected $self->get('-name') $previous, $new\n"); + $self->get('-zinc')->itemconfigure ($self->{-paper}, + -fillcolor => $new != 0 ? 'ch1': $style->{'-fillcolor'}, + ); + +} + + +sub OnButton1PressPaper { + my ($zinc, $self) = @_; + $zinc = $self->get('-zinc'); + + $zinc->itemconfigure ($self->{-paper}, + -fillcolor => 'ch1', + ); + +} + +sub OnButton1ReleasePaper { + my ($zinc, $self) = @_; + $zinc = $self->get('-zinc'); + + $zinc->itemconfigure ($self->{-paper}, + -fillcolor => $style->{'-fillcolor'} + ); + + +} + +1; diff --git a/sw/ground_segment/cockpit/Paparazzi/StripPanel.pm b/sw/ground_segment/cockpit/Paparazzi/StripPanel.pm new file mode 100644 index 0000000000..a22b03375b --- /dev/null +++ b/sw/ground_segment/cockpit/Paparazzi/StripPanel.pm @@ -0,0 +1,80 @@ +package Paparazzi::StripPanel; +use Subject; +@ISA = ("Subject"); + +use strict; + +use Math::Trig; +use Tk; +use Tk::Zinc; + +use Paparazzi::Strip; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-zinc => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -origin => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -width => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -height => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -selected_ac => [S_NOINIT, S_METHOD, S_RDWR, S_OVRWRT, S_NOPRPG, 'NONE'], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit(); + $self->build_gui(); +} + +sub build_gui { + my ($self) = @_; + my $zinc = $self->get('-zinc'); + my $width = $self->get('-width'); + my $height = $self->get('-height'); + my $origin = $self->get('-origin'); + + $self->{sp_main_group} = $zinc->add('group', 1, -visible => 1); + $zinc->coords($self->{sp_main_group}, $origin); + + my $board = $zinc->add('rectangle', $self->{sp_main_group} , + [0, 0, $width-5, $height-7], + -linewidth => 0, + -filled => 1, + -fillcolor => 'blue', + ); + my $texture = $zinc->Photo('background_texture.gif', + -file => Tk->findINC("demos/zinc_data/background_texture.gif")); + $zinc->itemconfigure($board, -tile => $texture); + $self->{strips} = {}; + +} + +sub add_strip { + my ($self, $name) = @_; + # add strip only once + return if (defined $self->{strips}->{$name}); + my $zinc = $self->get('-zinc'); + use constant NB_STRIP => 6; + my $step = $self->get('-height') / NB_STRIP; + my $nb_strips = keys %{$self->{strips}}; + my ($p, $w, $h) = ([15, 10 + $step * $nb_strips], 120, 45); + $self->{strips}->{$name} = Paparazzi::Strip->new( -zinc => $zinc, -parent_grp => $self->{sp_main_group}, + -origin => $p, -width => $w, -height => $h, + -name => $name); + $zinc->bind($self->{strips}->{$name}->{-paper},'',[\&OnStripPressed,$self, $name]); +} + +sub OnStripPressed { +# print ("OnStripPressed @_\n"); + my ($zinc, $self, $name) = @_; + $self->configure( -selected_ac => $name); +} + +sub selected_ac { + my ($self, $previous, $new) = @_; + $self->{strips}->{$previous}->configure( -selected => 0) if defined $previous and defined $self->{strips}->{$previous}; + $self->{strips}->{$new}->configure( -selected => 1) if defined $new and $new ne "NONE" and defined $self->{strips}->{$new}; +} + +1; diff --git a/sw/ground_segment/cockpit/cockpit.pl b/sw/ground_segment/cockpit/cockpit.pl new file mode 100755 index 0000000000..e83a7b2cc7 --- /dev/null +++ b/sw/ground_segment/cockpit/cockpit.pl @@ -0,0 +1,359 @@ +#!/usr/bin/perl -w +package Cockpit; + +my @paparazzi_lib; +BEGIN { + @paparazzi_lib = (defined $ENV{PAPARAZZI_SRC}) ? + ($ENV{PAPARAZZI_SRC}."/sw/lib/perl", $ENV{PAPARAZZI_SRC}."/sw/ground_segment/cockpit"):(); +} +use lib (@paparazzi_lib); + +use vars qw (@ISA) ; +use Subject; +@ISA = ("Subject"); + +use strict; +use Paparazzi::Environment; + +use constant COCKPIT_DEBUG => 0; +use constant APP_ID => "Paparazzi Cockpit"; +use constant MESSAGE_WHEN_READY => APP_ID.': READY'; + +use Paparazzi::IvyProtocol; +use Paparazzi::PFD; +use Paparazzi::ND; +use Paparazzi::MissionD; +use Paparazzi::StripPanel; +use Paparazzi::Geometry; + +use Tk; +#use Tk::PNG; +use Tk::Zinc; +use Ivy; +use Text::CSV; +use Data::Dumper; +use Pod::Usage; + +my $options = + { + ivy_bus => "127.255.255.255:2010", + render => 1, + }; + +my $md; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit(); + $self->{aircrafts} = []; + $self->{wind_dir} = 0.; + $self->{wind_speed} = 0.; + $self->start_ivy(); + $self->build_gui(); + +} + +sub build_gui { + my ($self) = @_; + $self->{mw} = MainWindow->new(); + my $top_frame = $self->{mw}->Frame()->pack(-side => 'top', -fill => 'both'); + my $bot_frame = $self->{mw}->Frame()->pack(-side => 'bottom', -fill => 'both'); + my ($stp_p, $stp_w, $stp_h) = ([0, 0], 150, 300); + my ($pfd_p, $pfd_w, $pfd_h) = ([$stp_w, 0] , 300, $stp_h); + my ($nd_p, $nd_w, $nd_h) = ([$pfd_p->[0]+ $pfd_w, 0], 600, 600); + my $zinc = $top_frame->Zinc(-width => $stp_w + $pfd_w + $nd_w , + -height => $nd_h, + -backcolor => 'black', + -borderwidth => 3, -relief => 'sunken', + -render => $options->{render}, + -lightangle => 130,); + $zinc->pack(-side => 'left', -anchor => "nw"); + $self->{strip_panel} = Paparazzi::StripPanel->new( -zinc => $zinc, + -origin => $stp_p, + -width => $stp_w, + -height => $stp_h + ); + $self->{strip_panel}->attach($self, '-selected_ac', ['onAircratftSelection', ()]); + + $self->{pfd} = Paparazzi::PFD->new( -zinc => $zinc, + -origin => $pfd_p, + -width => $pfd_w, + -height => $pfd_h, + ); + $self->{pfd}->attach($self, 'SHOW_PAGE', ['onShowPage']); + $self->{nd} = Paparazzi::ND->new( -zinc => $zinc, + -origin => $nd_p, + -width => $nd_w, + -height => $nd_h, + ); + $self->{nd}->attach($self, 'WIND_COMMAND', ['onWindCommand']); + $md = $bot_frame->MissionD(-bg => '#c1daff'); + $md->pack(-side => 'bottom', -anchor => "n", -fill => 'both'); + + +} + +sub onTimer { + my ( $self) = @_; +# print("in onTimer $self\n"); + $self->{ivy}->sendMsgs("WIND_REQ toto", {-id => "toto"}); + # Paparazzi::IvyProtocol::request_message("ground", "CONFIG", {id => 'ground'}, $self->{ivy}, [$self, \&ivyOnWind]); +} + +sub ivyOnWind { +# print "in ivyOnWind\n"; # if (COCKPIT_DEBUG); + my ($self, @args) = @_; + my $fields_by_name = Paparazzi::IvyProtocol::get_values_by_name("ground", "RES_WIND", \@args); + $self->{wind_dir} = $args[2]; + $self->{wind_speed} = $args[3]; + + my $h = { dir => $args[2], + speed => $args[3], + mean_aspeed => $args[4], + stddev => $args[5], + }; + +# print Dumper($h);# if (COCKPIT_DEBUG); + $self->{nd}->configure('-wind' => $h); +} + +sub onShowPage { + my ($self, $component, $signal, $page) = @_; + print "cockpit::onShowPage $page\n"; + print "$self->{nd}\n"; + $self->{nd}->configure('-page' => $page); +} + +sub onWindCommand { + my ($self, $component, $signal, $cmd) = @_; + print "cockpit::onWindCommand $cmd\n"; + $self->{ivy}->sendMsgs("WIND_COMMAND $cmd"); + if ($cmd eq "start") { + $self->{timer_id} = $self->{mw}->repeat(5000, [\&onTimer, $self]); + $self->{ivy}->sendMsgs("WIND_COMMAND clear"); + } + elsif ($cmd eq "stop") { + $self->{mw}->afterCancel($self->{timer_id}) + } +} + +sub onAircratftSelection { +# print ("onAircratftSelection @_\n"); + my ($self, $_sp, $what, $new_selected_ac ) = @_; + my $ivy = $self->{ivy}; + Paparazzi::IvyProtocol::sendMsg($ivy, "ground", "SELECTED",{ id => $new_selected_ac}); + return if ($new_selected_ac eq "NONE"); + my @ac_events = ( ['FLIGHT_PARAM', \&ivyOnFlightParam], + ['NAV_STATUS', \&ivyOnNavStatus], + ['AP_STATUS', \&ivyOnApStatus], + ['ENGINE_STATUS', \&ivyOnEngineStatus], + ['SATS', \&ivyOnSats], + ); + foreach my $event (@ac_events) { + # removes existing binding + Paparazzi::IvyProtocol::bind_message("aircraft_info", $event->[0], {id => $self->{selected_ac}}, $ivy, undef) unless !defined $self->{selected_ac}; + # add new one + Paparazzi::IvyProtocol::bind_message("aircraft_info", $event->[0], {id => $new_selected_ac}, $ivy, [$self, $event->[1]]); + } + $self->{selected_ac} = $new_selected_ac; +} + +sub start_ivy { + my ($self) = @_; + + Ivy->init (-ivyBus => $options->{ivy_bus}, + -appName => APP_ID, + -loopMode => 'TK', + -messWhenReady => MESSAGE_WHEN_READY, + ) ; + $self->{ivy} = Ivy->new (-statusFunc => \&ivyStatusCbk); + $self->{ivy}->start(); + my $paparazzi_home = Paparazzi::Environment::paparazzi_home(); + Paparazzi::IvyProtocol::read_protocol($paparazzi_home."/conf/messages.xml", "ground"); + Paparazzi::IvyProtocol::read_protocol($paparazzi_home."/conf/messages.xml", "aircraft_info"); + Paparazzi::IvyProtocol::bind_message("ground", "AIRCRAFTS", {}, $self->{ivy}, [$self, \&ivyOnAircrafts]); +# Paparazzi::IvyProtocol::bind_message("ground", "WIND_RES", {}, $self->{ivy}, [$self, \&ivyOnWind]); + $self->{ivy}->bindRegexp ("^ground WIND_RES (\\S+) (\\S+) (\\S+) (\\S+) (\\S+)", [$self, \&ivyOnWind]); + $self->{ivy}->bindRegexp ("^Thon1 RAW (\\S+) RAD_OF_IR (\\S+) (\\S+) (\\S+) (\\S+) (\\S+)", [$self, \&ivyOnIR]); +} + +sub ivyOnIR { + my ($self, @args) = @_; + my $h = { + ir => $args[2], + rad => $args[3], + rad_of_ir => $args[4], + ir_roll_ntrl => $args[5], + ir_pitch_ntrl => $args[6] + }; + $self->{nd}->configure('-lls' => $h->{rad_of_ir}); + $self->{nd}->put_lls($h->{rad_of_ir}); +} + + + + +sub ivyOnAircrafts { +# print "in ivyOnAircrafts\n" if (COCKPIT_DEBUG); + my ($self, @args) = @_; + my $fields_by_name = Paparazzi::IvyProtocol::get_values_by_name("ground", "AIRCRAFTS", \@args); + print Dumper($fields_by_name) if (COCKPIT_DEBUG); + my $ac_list = $fields_by_name->{ac_list}; + my $csv = Text::CSV->new(); + $csv->parse($ac_list); + my @new_ac = $csv->fields(); + my @added_ac = Utils::diff_array(\@new_ac, $self->{aircrafts}); + my @removed_ac = Utils::diff_array($self->{aircrafts}, \@new_ac); + foreach my $ac1 (@added_ac) { + $self->{strip_panel}->add_strip($ac1); + Paparazzi::IvyProtocol::request_message("aircraft_info", "CONFIG", {id => $ac1}, $self->{ivy}, [$self, \&onConfigRes]); + } + $self->{aircrafts} = \@new_ac; +} + +sub ivyOnNavStatus { + my ($self, @args) = @_; + my $fields_by_name = Paparazzi::IvyProtocol::get_values_by_name("aircraft_info", "NAV_STATUS", \@args); + print Dumper($fields_by_name) if (COCKPIT_DEBUG); + $md->set_block_and_stage($fields_by_name->{cur_block}, $fields_by_name->{cur_stage}); +} + +sub onWindRes { + my ($self, @args) = @_; + my $fields_by_name = Paparazzi::IvyProtocol::get_values_by_name("ground", "WIND_RES", \@args); + print Dumper ($fields_by_name) if (COCKPIT_DEBUG); +} + +sub onConfigRes { + print "in onConfigRes\n" if (COCKPIT_DEBUG); + my ($self, @args) = @_; + my $fields_by_name = Paparazzi::IvyProtocol::get_values_by_name("aircraft_info", "CONFIG", \@args); + my $fp = $fields_by_name->{flight_plan}; + # print Dumper($fields_by_name); + my $paparazzi_src = Paparazzi::Environment::paparazzi_src(); + my $gfp_bin = ((defined $paparazzi_src) ? $paparazzi_src."/sw/tools" : "/usr/share/paparazzi/bin") ."/gen_flight_plan.out"; + my $flight_plan_xml = `$gfp_bin -dump $fp`; + $md->load_flight_plan($flight_plan_xml); + $md->set_block_and_stage(0,0); +} + + +sub ivyOnFlightParam { + print "in ivyOnFlightParam\n" if (COCKPIT_DEBUG); + my ($self, @args) = @_; + my $fbn = Paparazzi::IvyProtocol::get_values_by_name("aircraft_info", "FLIGHT_PARAM", \@args); + print Dumper($fbn) if (COCKPIT_DEBUG); + my $gs_dir_rad = Utils::deg2rad( $fbn->{heading}); + my $gs_angle_rad = Paparazzi::Geometry::angle_of_heading_rad( Utils::deg2rad( $fbn->{heading})); +# print "$gs_dir_rad $gs_angle_rad\n"; + my ($xg, $yg) = Paparazzi::Geometry::cart_of_polar ($fbn->{speed}, $gs_angle_rad); + my $wind_angle_rad = Paparazzi::Geometry::angle_of_heading_rad( Utils::deg2rad( $self->{wind_dir} + Math::Trig::pi)); + my ($xw, $yw) = Paparazzi::Geometry::cart_of_polar ($self->{wind_speed}, $wind_angle_rad); + my ($xa, $ya) = ($xg+$xw, $yg+$yw); + my ($as, $ad) = Paparazzi::Geometry::polar_of_cart ($xa, $ya); + +# print "gs $xg $yg w $xw $yw as $xa $ya $as $ad\n"; + + + $self->{pfd}->configure( + -roll => $fbn->{roll}, + -pitch => $fbn->{pitch}, +# -speed => $fbn->{speed}, + -speed => $as, + -target_speed => $fbn->{speed}, +# -heading => $fbn->{heading}, + -heading => $ad, +# -target_heading => $fbn->{heading}, + -alt => $fbn->{alt}, + -vz => $fbn->{climb}, +# -gps_mode => 3, + -lls_mode => 1, +# -lls_value => 1.1 , + -ctrst_mode => 2 , + -ctrst_value => 200, + -rc_mode => 1, + -if_mode => 1, + ); +# $self->{nd}->configure(); + + +} + +sub ivyOnApStatus { + print "in ivyOnApStatus\n" if (COCKPIT_DEBUG); + my ($self, @args) = @_; + my $fbn = Paparazzi::IvyProtocol::get_values_by_name("aircraft_info", "AP_STATUS", \@args); +# print $self->{selected_ac}." ".Dumper($fbn);# if (COCKPIT_DEBUG); + $self->{pfd}->configure( -ap_mode => $fbn->{mode}, +# -h_mode => $fbn->{h_mode}, +# -v_mode => $fbn->{v_mode}, +# -target_vz => $fbn->{target_climb} + -target_alt => $fbn->{target_alt}, + -target_heading => $fbn->{target_heading}, + ); + $self->{nd}->configure( -ap_status, $fbn); +} + +sub ivyOnEngineStatus { + print "in ivyOnEngineStatus\n" if (COCKPIT_DEBUG); + my ($self, @args) = @_; + my $fbn = Paparazzi::IvyProtocol::get_values_by_name("aircraft_info", "ENGINE_STATUS", \@args); + $self->{nd}->configure( -engine_status, $fbn); +} + +sub ivyOnSats { + print "in ivyOnSats\n" if (COCKPIT_DEBUG); + my ($self, @args) = @_; + my $fbn = Paparazzi::IvyProtocol::get_values_by_name("aircraft_info", "SATS", \@args); + $self->{nd}->configure( -sats, $fbn); +} + + + +sub ivyStatusCbk { + printf("in ivyStatusCbk\n") if (COCKPIT_DEBUG); +} + +Paparazzi::Environment::parse_command_line($options) || pod2usage(-verbose => 0); +print Dumper($options); +my $cockpit = Cockpit->new(); +MainLoop(); + +__END__ + +=head1 NAME + +cockpit + +=head1 SYNOPSIS + +cockpit [options] + +Options: + -ivybus the ivy bus (eg 127.2552.55255:2010) + -render toggle opengl usage + +=head1 OPTIONS + +=over 8 + +=item B<-help> + +Print a brief help message and exits. + +=item B<-man> + +Prints the manual page and exits. + +=back + +=head1 DESCRIPTION + +B will display an aircraft cockpit. + +=cut diff --git a/sw/ground_segment/cockpit/map.pl b/sw/ground_segment/cockpit/map.pl new file mode 100755 index 0000000000..b828ad4aad --- /dev/null +++ b/sw/ground_segment/cockpit/map.pl @@ -0,0 +1,129 @@ +#!/usr/bin/perl -w +package Map; + +my @paparazzi_lib; +BEGIN { + @paparazzi_lib = (defined $ENV{PAPARAZZI_SRC}) ? + ($ENV{PAPARAZZI_SRC}."/sw/lib/perl", $ENV{PAPARAZZI_SRC}."/sw/ground_segment/cockpit"):(); +} +use lib (@paparazzi_lib); + +use vars qw (@ISA) ; +use Subject; +@ISA = ("Subject"); + +use strict; +use Paparazzi::Environment; + +use constant MAP_DEBUG => 0; +use constant APP_ID => "Paparazzi Map"; +use constant MESSAGE_WHEN_READY => APP_ID.': READY'; + +use Paparazzi::IvyProtocol; +use Paparazzi::MapView; +use Paparazzi::Utils; + +use Getopt::Long; +use Tk; +use Ivy; +use Text::CSV; + +my $paparazzi_home = Paparazzi::Environment::paparazzi_home(); + +my $options = { + paparazzi_home => $paparazzi_home, + ivy_bus => "127.255.255.255:2010", + data_dir => $paparazzi_home."/data", + map_file => "maps/defaultUTM.xml", + conf_dir => $paparazzi_home."/conf", + render => "1" + }; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit(); + $self->{aircrafts} = []; + $self->start_ivy(); + $self->build_gui(); +} + +sub start_ivy { + my ($self) = @_; + + Ivy->init (-ivyBus => $options->{ivy_bus}, + -appName => APP_ID, + -loopMode => 'TK', + -messWhenReady => MESSAGE_WHEN_READY, + ); + $self->{ivy} = Ivy->new(); + $self->{ivy}->start(); + Paparazzi::IvyProtocol::read_protocol($paparazzi_home."/conf/messages.xml", "aircraft_info"); + Paparazzi::IvyProtocol::read_protocol($paparazzi_home."/conf/messages.xml", "ground"); + Paparazzi::IvyProtocol::bind_message("ground", "AIRCRAFTS", {}, $self->{ivy}, [$self, \&ivyOnAircrafts]); +} + +sub build_gui { + my ($self) = @_; + my $mw = MainWindow->new(); + $mw->title("Paparazzi map : $options->{map_file}"); + $mw->geometry("600x600"); + $self->{map_view} = $mw->MapView(-render => $options->{render}); + $self->{map_view}->pack(-fill => 'both', -expand => "1"); + + $self->{map_view}->load_map($options->{data_dir}."/".$options->{map_file}); +# my $flight_plan = `$paparazzi_home/sw/tools/gen_flight_plan.out -dump $ flight_plan_name`; +# $mv->load_flight_plan($flight_plan); +} + +sub ivyOnAircrafts { + print "in ivyOnAircrafts\n"; + my ($self, @args) = @_; + my $fields_by_name = Paparazzi::IvyProtocol::get_values_by_name("ground", "AIRCRAFTS", \@args); +# print Dumper($fields_by_name); + my $ac_list = $fields_by_name->{ac_list}; + my $csv = Text::CSV->new(); + $csv->parse($ac_list); + my @new_ac = $csv->fields(); + my @added_ac = Utils::diff_array(\@new_ac, $self->{aircrafts}); + my @removed_ac = Utils::diff_array($self->{aircrafts}, \@new_ac); + foreach my $new_ac (@added_ac) { + print "added_ac $new_ac\n"; + Paparazzi::IvyProtocol::bind_message("aircraft_info", "FLIGHT_PARAM", {id => $new_ac}, $self->{ivy}, [$self, \&ivyOnFlightParam]); + my $track_item = $self->{map_view}->set_track_geo($new_ac, [0, 0]); + + } + foreach my $ac2 (@removed_ac) { + print "removed_ac $ac2\n"; + } + $self->{aircrafts} = \@new_ac; +} + +sub ivyOnFlightParam { +# print "in ivyOnFlightParam\n"; + my ($self, @args) = @_; + my $fbn = Paparazzi::IvyProtocol::get_values_by_name("aircraft_info", "FLIGHT_PARAM", \@args); + my $ac = $fbn->{id}; +# print Dumper($fbn); + $self->{map_view}->set_track_geo($ac, [$fbn->{east}, $fbn->{north}]); +} + +use Data::Dumper; + +GetOptions ("b=s" => \$options->{ivy_bus}, + "t=s" => \$options->{paparazzi_home}, + "d=s" => \$options->{data_dir}, + "m=s" => \$options->{map_file}, + "c=s" => \$options->{conf_dir}, + "r=s" => \$options->{render}, + ); +print Dumper($options); +my $map = Map->new(); +MainLoop(); + +1; + diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml new file mode 100644 index 0000000000..88f88ddd67 --- /dev/null +++ b/sw/ground_segment/cockpit/map2d.ml @@ -0,0 +1,225 @@ +open Printf +open Latlong + +type color = string + +let fos = float_of_string +let list_separator = Str.regexp "," + +module G = MapCanvas + +let home = Env.paparazzi_home +let (//) = Filename.concat +let default_path_SRTM = home // "data" // "SRTM" +let default_path_maps = home // "data" // "maps" // "" +let default_path_missions = home // "conf" + +let gen_flight_plan = + try + Sys.getenv "PAPARAZZI_SRC" // "sw/tools/gen_flight_plan.out" + with + Not_found -> "/usr/bin/paparazzi gen_flight_plan" + + +type aircraft = { + track : MapTrack.track; + color: color; + mutable fp_group : MapWaypoints.group option + } + +let live_aircrafts = Hashtbl.create 3 + +let map_ref = ref None + +let float_attr = fun xml a -> float_of_string (ExtXml.attrib xml a) + +let load_map = fun (geomap:G.widget) xml_map -> + let dir = Filename.dirname xml_map in + let xml_map = Xml.parse_file xml_map in + let image = dir // ExtXml.attrib xml_map "file" + and scale = float_attr xml_map "scale" + and utm_zone = + try int_of_string (Xml.attrib xml_map "utm_zone") with + _ -> 31 in + geomap#set_world_unit scale; + let one_ref = ExtXml.child xml_map "point" in + let x = float_attr one_ref "x" and y = float_attr one_ref "y" + and utm_x = float_attr one_ref "utm_x" and utm_y = float_attr one_ref "utm_y" in + let utm_x0 = utm_x -. x *. scale + and utm_y0 = utm_y +. y *. scale in + + let utm_ref = + match !map_ref with + None -> + let utm0 = {utm_x = utm_x0; utm_y = utm_y0; utm_zone = utm_zone } in + map_ref := Some utm0; + utm0 + | Some utm -> + assert (utm_zone = utm.utm_zone); + utm in + + let wgs84_of_en = fun en -> + of_utm WGS84 {utm_x = utm_ref.utm_x +. en.G.east; utm_y = utm_ref.utm_y +. en.G.north; utm_zone = utm_zone} in + + geomap#set_wgs84_of_en wgs84_of_en; + let en0 = {G.east=utm_x0 -. utm_ref.utm_x; north=utm_y0 -. utm_ref.utm_y} in + ignore (geomap#display_map en0 (GdkPixbuf.from_file image)); + geomap#moveto en0 + + +let file_of_url = fun url -> + if String.sub url 0 7 = "file://" then + String.sub url 7 (String.length url - 7) + else + let tmp_file = Filename.temp_file "fp" ".xml" in + Sys.command (sprintf "wget -O %s %s" tmp_file url); + tmp_file + +let load_mission = fun color geomap url -> + let file = file_of_url url in + let xml = Xml.parse_in (Unix.open_process_in (sprintf "%s -dump %s" gen_flight_plan file)) in + let xml = ExtXml.child xml "flight_plan" in + let lat0 = float_attr xml "lat0" + and lon0 = float_attr xml "lon0" in + let utm0 = utm_of WGS84 {posn_lat = (Deg>>Rad)lat0; posn_long = (Deg>>Rad)lon0 } in + let waypoints = ExtXml.child xml "waypoints" in + + let utm_ref = + match !map_ref with + None -> + map_ref := Some utm0; + utm0 + | Some utm -> + assert (utm0.utm_zone = utm.utm_zone); + utm in + let en_of_xy = fun x y -> + {G.east = x +. utm0.utm_x -. utm_ref.utm_x; + G.north = y +. utm0.utm_y -. utm_ref.utm_y } in + + let fp = new MapWaypoints.group ~color ~editable:false geomap in + List.iter + (fun wp -> + let en = en_of_xy (float_attr wp "x") (float_attr wp "y") in + let alt = try Some (float_attr wp "alt") with _ -> None in + ignore (MapWaypoints.waypoint fp ~name:(ExtXml.attrib wp "name") ?alt en) + ) + (Xml.children waypoints); + fp + + +let aircraft_pos_msg = fun track utm_x utm_y heading -> + match !map_ref with + None -> () + | Some utm0 -> + let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in + track#add_point en; + track#move_icon en heading + +let new_color = + let colors = ref ["red"; "blue"; "green"] in + fun () -> + match !colors with + x::xs -> + colors := xs @ [x]; + x + | [] -> failwith "new_color" + + +let ivy_request = fun s f -> + let b = ref (Obj.magic ()) in + let cb = fun response -> + Ivy.unbind !b; + f response in + let id = sprintf "%s_%d" (Filename.basename Sys.argv.(1)) (Unix.getpid ()) in + b := Ivy.bind (fun _ args -> cb args.(0)) (sprintf "response %s (.*)" id); + Ivy.send (sprintf "request %s %s" id s) + + +let ask_fp = fun geomap ac -> + let b = ref (Obj.magic ()) in + let load_fp = fun file -> + Ivy.unbind !b; + let ac = Hashtbl.find live_aircrafts ac in + ac.fp_group <- Some (load_mission ac.color geomap file) in + b := Ivy.bind (fun _ args -> load_fp args.(0)) (sprintf "ground FLIGHT_PLAN %s (.*)" ac); + Ivy.send (sprintf "ask FLIGHT_PLAN %s" ac) + + +let show_mission = fun geomap ac on_off -> + if on_off then + ask_fp geomap ac + else + let a = Hashtbl.find live_aircrafts ac in + match a.fp_group with + None -> () + | Some g -> + a.fp_group <- None; + g#group#destroy () + +let resize_track = fun ac track -> + match GToolbox.input_string ~text:(string_of_int track#size) ~title:ac "Track size" with + None -> () + | Some s -> track#resize (int_of_string s) + + + +let live_aircrafts_msg = fun (geomap:MapCanvas.widget) acs -> + List.iter + (fun ac -> + if not (Hashtbl.mem live_aircrafts ac) then begin + let ac_menu = geomap#factory#add_submenu ac in + let ac_menu_fact = new GMenu.factory ac_menu in + let fp = ac_menu_fact#add_check_item "Fligh Plan" ~active:false in + ignore (fp#connect#toggled (fun () -> show_mission geomap ac fp#active)); + let color = new_color () in + let track = new MapTrack.track ~name:ac ~color:color geomap in + ignore (ac_menu_fact#add_item "Clear Track" ~callback:(fun () -> track#clear)) ; + ignore (ac_menu_fact#add_item "Resize Track" ~callback:(fun () -> resize_track ac track)) ; + let b = + Ivy.bind + (fun _ args -> aircraft_pos_msg track (fos args.(0)) (fos args.(1))(fos args.(2))) + (sprintf "%s +FLIGHT_PARAM +[^ ]* +[^ ]* +([0-9\\.]*) +([0-9\\.]*) +[0-9\\.]* +([0-9\\.]*)" ac) in + Hashtbl.add live_aircrafts ac { track = track; color = color; fp_group = None } + end + ) + acs + + +let _ = + let ivy_bus = ref "127.255.255.255:2010" + and map_file = ref "" + and mission_file = ref "" in + let options = + [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"; + "-m", Arg.String (fun x -> map_file := x), "Map description file"] in + Arg.parse (options) + (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x) + "Usage: "; + (* *) + Ivy.init "Paparazzi map 2D" "READY" (fun _ _ -> ()); + Ivy.start !ivy_bus; + + Srtm.add_path default_path_SRTM; + + let window = GWindow.window ~title: "Map2d" ~border_width:1 ~width:400 () in + let vbox= GPack.vbox ~packing: window#add () in + let quit = fun () -> GMain.Main.quit (); exit 0 in + ignore (window#connect#destroy ~callback:quit); + + let geomap = new MapCanvas.widget ~height:400 () in + let accel_group = geomap#menu_fact#accel_group in + ignore (geomap#menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit); + + vbox#pack ~expand:true geomap#frame#coerce; + + (* Loading an initial map *) + if !map_file <> "" then begin + let xml_map_file = Filename.concat default_path_maps !map_file in + load_map geomap xml_map_file + end; + + Ivy.bind (fun _ args -> live_aircrafts_msg geomap (Str.split list_separator args.(0))) "ground +AIRCRAFTS +(.*)"; + + window#add_accel_group accel_group; + window#show (); + GMain.Main.main () diff --git a/sw/ground_segment/cockpit/radio_control.pl b/sw/ground_segment/cockpit/radio_control.pl new file mode 100755 index 0000000000..2ceb1d06c5 --- /dev/null +++ b/sw/ground_segment/cockpit/radio_control.pl @@ -0,0 +1,68 @@ +#!/usr/bin/perl -w +package RadioControl; + +my @paparazzi_lib; +BEGIN { + @paparazzi_lib = (defined $ENV{PAPARAZZI_SRC}) ? + ($ENV{PAPARAZZI_SRC}."/sw/lib/perl", $ENV{PAPARAZZI_SRC}."/sw/ground_segment/cockpit"):(); +} +use lib (@paparazzi_lib); + +use strict; +use Paparazzi::Environment; +use Paparazzi::RCTransmitter; +use Paparazzi::IvyProtocol; + + +use Getopt::Long; +use Tk; +use Ivy; + +use constant APP_ID => "Paparazzi RadioControl"; +use constant MESSAGE_WHEN_READY => APP_ID.': READY'; + +my $options = { + radio_file => "fc28.xml", + ivy_bus => "127.255.255.255:2010", + }; + +GetOptions ( + "r=s" => \$options->{radio_file}, + ); + + +my $mw = MainWindow->new(); +Paparazzi::RCTransmitter->new( + $mw, + -filename => Paparazzi::Environment::paparazzi_home()."/conf/radios/".$options->{radio_file} + )->pack(); +start_ivy(); +MainLoop(); + +my $self = {}; + +sub start_ivy { +# my ($self) = @_; + + Ivy->init (-ivyBus => $options->{ivy_bus}, + -appName => APP_ID, + -loopMode => 'TK', + -messWhenReady => MESSAGE_WHEN_READY, + ) ; + $self->{ivy} = Ivy->new (-statusFunc => \&ivyStatusCbk); + $self->{ivy}->start(); + my $paparazzi_home = Paparazzi::Environment::paparazzi_home(); + Paparazzi::IvyProtocol::read_protocol($paparazzi_home."/conf/messages.xml", "telemetry_fbw"); + Paparazzi::IvyProtocol::bind_message("telemetry_fbw", "RC", {}, $self->{ivy}, [$self, \&ivyOnRc]); +} + +sub ivyStatusCbk { + +} + +sub ivyOnRc { +# my ($self) = @_; + + + +} diff --git a/sw/ground_segment/modem/Makefile b/sw/ground_segment/modem/Makefile new file mode 100644 index 0000000000..88dfbb8d45 --- /dev/null +++ b/sw/ground_segment/modem/Makefile @@ -0,0 +1,40 @@ +# +# modem $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. +# + +ARCH = atmega8 +TARGET = modem_gnd +LOW_FUSE = 3f +HIGH_FUSE = cb +EXT_FUSE= ff +LOCK_FUSE= ff +INCLUDES= -I ../../include + +$(TARGET).srcs = \ + main.c \ + uart.c \ + soft_uart.c \ + adc.c \ + +include ../../../conf/Makefile.local +include ../../../conf/Makefile.avr + +clean : avr_clean diff --git a/sw/ground_segment/modem/README b/sw/ground_segment/modem/README new file mode 100644 index 0000000000..f7f32367e2 --- /dev/null +++ b/sw/ground_segment/modem/README @@ -0,0 +1,28 @@ +# +# $Id$ +# Copyright (C) 2004 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. +# + +This directory contains code for the mega8 MCU in the +ground modem. +The mega8 drives the CMX469 modem. This is necessary because +the crystal in the transmitting CMX469 is 4Mhz instead of +specified 4.032 . +This leads to incompatibility with the laptop baud generator. diff --git a/sw/ground_segment/modem/adc.c b/sw/ground_segment/modem/adc.c new file mode 100644 index 0000000000..0584677469 --- /dev/null +++ b/sw/ground_segment/modem/adc.c @@ -0,0 +1,39 @@ + +#include +#include +#include +#include + +#include "avr/std.h" +#include "adc.h" + +#define ANALOG_PORT PORTC +#define ANALOG_PORT_DIR DDRC +#define VALIM 7 + +uint16_t adc_alim; +volatile uint8_t adc_got_val; + +void adc_init( void ) +{ + /* Ensure that our port is for input with no pull-ups */ + ANALOG_PORT &= ~_BV(VALIM); + ANALOG_PORT_DIR &= ~_BV(VALIM); + + /* Select our external voltage ref, which is tied to Vcc and channel VALIM*/ + ADMUX = VALIM; + + /* Turn off the analog comparator */ + sbi( ACSR, ACD ); + + /* turn on the ADC, clock/128, interrupts, free running mode and starts conversion */ + ADCSRA = _BV(ADEN) | _BV(ADPS0) | _BV(ADPS1) | _BV(ADPS2) | _BV(ADIE) | _BV(ADFR) | _BV(ADSC); +} + + +SIGNAL( SIG_ADC ) +{ + /* Store result */ + adc_alim = ADCW; + adc_got_val = TRUE; +} diff --git a/sw/ground_segment/modem/adc.h b/sw/ground_segment/modem/adc.h new file mode 100644 index 0000000000..c5e0d77bf4 --- /dev/null +++ b/sw/ground_segment/modem/adc.h @@ -0,0 +1,8 @@ +#ifndef ADC_H +#define ADC_H + +void adc_init( void ); +extern uint16_t adc_alim; +extern volatile uint8_t adc_got_val; + +#endif diff --git a/sw/ground_segment/modem/link_tmtc.h b/sw/ground_segment/modem/link_tmtc.h new file mode 100644 index 0000000000..e58b75063b --- /dev/null +++ b/sw/ground_segment/modem/link_tmtc.h @@ -0,0 +1,92 @@ +#ifndef LINK_TMTC_H +#define LINK_TMTC_H + +#define STX 0x02 +#define ETX 0x03 + +#define MSG_DATA 0 +#define MSG_ERROR 1 +#define MSG_CD 2 +#define MSG_DEBUG 3 +#define MSG_VALIM 4 + + + +#define LINK_TMTC_SEND_DATA(data, _len) { \ + uint8_t checksum = 0; \ + const uint8_t real_len = 2+_len; \ + uint8_t i; \ + uart_putc(STX); \ + uart_putc(real_len); \ + checksum^=real_len; \ + uart_putc(MSG_DATA); \ + checksum^=MSG_DATA; \ + for (i=0; i<_len; i++) { \ + uart_putc(data[i]); \ + checksum^=data[i]; \ + } \ + uart_putc(checksum); \ + uart_putc(ETX); \ +} + +#define LINK_TMTC_SEND_ERROR(error) { \ + uint8_t checksum = 0; \ + const uint8_t real_len = 2+1; \ + uart_putc(STX); \ + uart_putc(real_len); \ + checksum^=real_len; \ + uart_putc(MSG_ERROR); \ + checksum^=MSG_ERROR; \ + uart_putc(error); \ + checksum^=error; \ + uart_putc(checksum); \ + uart_putc(ETX); \ +} + + +#define LINK_TMTC_SEND_CD(cd) { \ + uint8_t checksum = 0; \ + const uint8_t real_len = 2+1; \ + uart_putc(STX); \ + uart_putc(real_len); \ + checksum^=real_len; \ + uart_putc(MSG_CD); \ + checksum^=MSG_CD; \ + uart_putc(cd); \ + checksum^=cd; \ + uart_putc(checksum); \ + uart_putc(ETX); \ +} + +#define LINK_TMTC_SEND_DEBUG() { \ + uint8_t checksum = 0; \ + const uint8_t real_len = 2+1; \ + uart_putc(STX); \ + uart_putc(real_len); \ + checksum^=real_len; \ + uart_putc(MSG_DEBUG); \ + checksum^=MSG_DEBUG; \ + uart_putc(uart_nb_ovrrun); \ + checksum^=uart_nb_ovrrun; \ + uart_putc(checksum); \ + uart_putc(ETX); \ +} + +#define LINK_TMTC_SEND_VALIM(_valim) { \ + uint8_t checksum = 0; \ + const uint8_t real_len = 2+2; \ + uart_putc(STX); \ + uart_putc(real_len); \ + checksum^=real_len; \ + uart_putc(MSG_VALIM); \ + checksum^=MSG_VALIM; \ + uart_putc(*(uint8_t*)(_valim)); \ + checksum^= *(uint8_t*)(_valim); \ + uart_putc(* ((uint8_t*)(_valim) + 1)); \ + checksum^= *((uint8_t*)(_valim) + 1); \ + uart_putc(checksum); \ + uart_putc(ETX); \ +} + + +#endif diff --git a/sw/ground_segment/modem/main.c b/sw/ground_segment/modem/main.c new file mode 100644 index 0000000000..ed75ef3684 --- /dev/null +++ b/sw/ground_segment/modem/main.c @@ -0,0 +1,70 @@ +#include +#include +#include +#include +#include + + +#include "timer.h" +#include "soft_uart.h" +#include "adc.h" +#include "uart.h" +#include "link_tmtc.h" + +#define FALSE 0 +#define TRUE (!FALSE) + +static uint16_t cputime = 0; // seconds + +#define INPUT_BUF_LEN 10 +static uint8_t input_buf[INPUT_BUF_LEN]; +static uint8_t input_buf_idx = 0; + +static uint16_t saved_valim; + +inline void periodic_task( void ) { // 15 Hz + static uint8_t _1Hz = 0; + _1Hz++; + if (_1Hz>=15) _1Hz=0; + + if (!_1Hz) { + uint8_t cd_status = bit_is_set(SOFT_UART_CD_PIN, SOFT_UART_CD); + cputime++; + LINK_TMTC_SEND_CD(cd_status); + LINK_TMTC_SEND_VALIM(&saved_valim); + LINK_TMTC_SEND_DEBUG(); + } +} + +int main( void ) { + /* init peripherals */ + timer_init(); + uart_init(); + soft_uart_init(); + adc_init(); + sei(); + + /* enter mainloop */ + while( 1 ) { + if(timer_periodic()) + periodic_task(); + if (soft_uart_error) { + LINK_TMTC_SEND_ERROR(soft_uart_error); + soft_uart_error = 0; + } + if (soft_uart_got_byte) { + input_buf[input_buf_idx] = soft_uart_byte; + input_buf_idx++; + if (input_buf_idx >= INPUT_BUF_LEN) { + LINK_TMTC_SEND_DATA(input_buf, input_buf_idx); + input_buf_idx = 0; + } + soft_uart_got_byte = FALSE; + } + if (adc_got_val) { + saved_valim = adc_alim; + adc_got_val = FALSE; + } + } + return 0; +} diff --git a/sw/ground_segment/modem/soft_uart.c b/sw/ground_segment/modem/soft_uart.c new file mode 100644 index 0000000000..5e1bd4e475 --- /dev/null +++ b/sw/ground_segment/modem/soft_uart.c @@ -0,0 +1,80 @@ +#include "soft_uart.h" + +#include +#include +#include + +#define FALSE 0 +#define TRUE (!FALSE) + + +volatile uint8_t soft_uart_got_byte = FALSE; +uint8_t soft_uart_byte; +volatile uint8_t soft_uart_error = 0; + +#define RX_CLOCKED_DATA_PORT PORTB +#define RX_CLOCKED_DATA_DDR DDRB +#define RX_CLOCKED_DATA_PIN PINB +#define RX_CLOCKED_DATA 0 + + +void soft_uart_init(void) { + + /* set CD pin as input, no pullup */ + SOFT_UART_CD_DDR &= ~_BV(SOFT_UART_CD); + SOFT_UART_CD_PORT &= ~_BV(SOFT_UART_CD); + + /* set DATA pin as input no pullup*/ + RX_CLOCKED_DATA_DDR &= ~_BV(RX_CLOCKED_DATA); + RX_CLOCKED_DATA_PORT &= ~_BV(RX_CLOCKED_DATA); + + /* setup rx interrupt on failing edge of clock */ + MCUCR = _BV(ISC11); + /* clear interrupt flag */ + sbi(GIFR, INTF1); + /* enable interrupt */ + sbi(GICR, INT1); +} + + +SIGNAL(SIG_INTERRUPT1) { + static uint8_t rx_buf_idx = 0; + static uint8_t rx_buf; + + if (bit_is_clear(SOFT_UART_CD_PIN, SOFT_UART_CD)) { + rx_buf_idx = 0; + } + else { + if (rx_buf_idx==0) { + // start bit + if (bit_is_clear(RX_CLOCKED_DATA_PIN, RX_CLOCKED_DATA)) { + rx_buf = 0; + rx_buf_idx++; + } + } + else if (rx_buf_idx < 9) { + // data bits + rx_buf >>= 1; + if (bit_is_set(RX_CLOCKED_DATA_PIN, RX_CLOCKED_DATA)) + rx_buf |= 0x80; + rx_buf_idx++; + } + else { + // stop bit + if (bit_is_set(RX_CLOCKED_DATA_PIN, RX_CLOCKED_DATA)) { + if (soft_uart_got_byte) { + soft_uart_error = RX_ERROR_OVERRUN; + } + else { + soft_uart_byte = rx_buf; + soft_uart_got_byte = TRUE; + } + } + else { + // framing error + soft_uart_error = RX_ERROR_FRAMING; + } + rx_buf_idx = 0; + } + } +} diff --git a/sw/ground_segment/modem/soft_uart.h b/sw/ground_segment/modem/soft_uart.h new file mode 100644 index 0000000000..a1a034c58a --- /dev/null +++ b/sw/ground_segment/modem/soft_uart.h @@ -0,0 +1,21 @@ +#ifndef SOFT_UART_H +#define SOFT_UART_H + +#include + +extern volatile uint8_t soft_uart_got_byte; +extern uint8_t soft_uart_byte; + +#define RX_ERROR_FRAMING 1 +#define RX_ERROR_OVERRUN 2 +extern volatile uint8_t soft_uart_error; + +#define SOFT_UART_CD_PORT PORTD +#define SOFT_UART_CD_DDR DDRD +#define SOFT_UART_CD_PIN PIND +#define SOFT_UART_CD 6 + +void soft_uart_init(void); + + +#endif diff --git a/sw/ground_segment/modem/timer.h b/sw/ground_segment/modem/timer.h new file mode 100644 index 0000000000..c71ac78e83 --- /dev/null +++ b/sw/ground_segment/modem/timer.h @@ -0,0 +1,91 @@ +/* + * Paparazzi mcu0 timer functions + * + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * Copyright (C) 2002 Trammell Hudson + * 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. + * + */ + +#ifndef TIMER_H +#define TIMER_H + +#include +#include +#include + + +/* + * Enable Timer1 (16-bit) running at Clk/1 for the global system + * clock. This will be used for computing the servo pulse widths, + * PPM decoding, etc. + * + * Low frequency periodic tasks will be signaled by timer 0 + * running at Clk/1024. For 4 Mhz clock, this will be every + * 65536 microseconds, or 15 Hz. + */ +static inline void timer_init( void ) { + /* Timer0 @ Clk/64: Software UART */ +/* TCCR0 = 0x03; */ + + /* Timer1 @ Clk/1: System clock, ppm and servos */ + // TCCR1A = 0x00; + // TCCR1B = 0x01; + + /* Timer2 @ Clk/1024: Periodic clock*/ + TCCR2 = 0x07; +} + + +/* + * Retrieve the current time from the global clock in Timer1, + * disabling interrupts to avoid stomping on the TEMP register. + * If interrupts are already off, the non_atomic form can be used. + */ +static inline uint16_t +timer_now( void ) +{ + return TCNT1; +} + +static inline uint16_t +timer_now_non_atomic( void ) +{ + return TCNT1L; +} + + +/* + * Periodic tasks occur when Timer2 overflows. Check and unset + * the overflow bit. We cycle through four possible periodic states, + * so each state occurs every 30 Hz. + */ +static inline uint8_t +timer_periodic( void ) +{ + if( !bit_is_set( TIFR, TOV2 ) ) + return 0; + + TIFR = 1 << TOV2; + return 1; +} + +#endif diff --git a/sw/ground_segment/modem/uart.c b/sw/ground_segment/modem/uart.c new file mode 100644 index 0000000000..90701bef83 --- /dev/null +++ b/sw/ground_segment/modem/uart.c @@ -0,0 +1,81 @@ +#include +#include +#include +#include "uart.h" + + +uint8_t uart_nb_ovrrun = 0; + +#define TX_BUF_SIZE 100 + +static volatile uint8_t tx_head = TX_BUF_SIZE - 1; +static volatile uint8_t tx_tail = TX_BUF_SIZE - 1; +static uint8_t tx_buf[ TX_BUF_SIZE ]; + + +/* + * UART Baud rate generation settings: + * + * With 16.0 MHz clock,UBRR=25 => 38400 baud + * With 8.0 Mhz clock, UBRR=12 => 38400 baud + * + * With 4.0 MHz UBRR=12 + ub2X=1 -> 38400 baud + */ + +void uart_init( void ) { + /* Baudrate is 38.4k */ + UBRRH = 0; + UBRRL = 12; + /* double speed */ + UCSRA = _BV(U2X); + /* Enable transmitter */ + UCSRB = _BV(TXEN); + /* Set frame format: 8data, 1stop bit */ + UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0); +} + + +static inline void load_next_byte( void ) { + uint8_t tmp_tail; + /* load a new byte */ + tmp_tail = tx_tail + 1; + if( tmp_tail >= TX_BUF_SIZE ) + tmp_tail = 0; + tx_tail = tmp_tail; + UDR = tx_buf[tx_tail]; +} + +void uart_putc( unsigned char c ) { + uint8_t tmp_head; + + tmp_head = tx_head + 1; + if( tmp_head >= TX_BUF_SIZE ) + tmp_head = 0; + /* if buffer is full do nothing */ + if( tmp_head == tx_tail ) { + uart_nb_ovrrun++; + return; + } + + /* copy data to buffer */ + tx_buf[ tmp_head ] = c; + /* update head */ + tx_head = tmp_head; + + /* if we were not allready transmitting */ + if (bit_is_clear(UCSRB, TXCIE)) { + /* load a byte */ + load_next_byte(); + /* enable interrupt */ + sbi(UCSRB, TXCIE); + } +} + +SIGNAL( SIG_UART_TRANS ) { + /* if we have nothing left to transmit */ + if( tx_head == tx_tail ) + /* disable data register empty interrupt */ + cbi(UCSRB, TXCIE); + else + load_next_byte(); +} diff --git a/sw/ground_segment/modem/uart.h b/sw/ground_segment/modem/uart.h new file mode 100644 index 0000000000..be7f03d11b --- /dev/null +++ b/sw/ground_segment/modem/uart.h @@ -0,0 +1,19 @@ +#ifndef _UART_H_ +#define _UART_H_ + +#include +#include +#include +#include + + + +/************************************************************************* + * + * UART code. + */ + +void uart_init( void ); +void uart_putc( unsigned char c ); +extern uint8_t uart_nb_ovrrun; +#endif diff --git a/sw/ground_segment/speech/README b/sw/ground_segment/speech/README new file mode 100644 index 0000000000..d2a50a2e89 --- /dev/null +++ b/sw/ground_segment/speech/README @@ -0,0 +1,25 @@ +# +# $Id$ +# Copyright (C) 2004 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. +# + +This directory contains code that use the festival speech engine to +pronounce warnings and parameters. +It uses Ivy diff --git a/sw/ground_segment/speech/paparazzi_speak.pl b/sw/ground_segment/speech/paparazzi_speak.pl new file mode 100755 index 0000000000..e26095037c --- /dev/null +++ b/sw/ground_segment/speech/paparazzi_speak.pl @@ -0,0 +1,216 @@ +#!/usr/bin/perl -w + +package PaparazziSpeak; + +my @paparazzi_lib; +BEGIN { + @paparazzi_lib = (defined $ENV{PAPARAZZI_SRC}) ? + ($ENV{PAPARAZZI_SRC}."/sw/lib/perl"):(); +} +use lib (@paparazzi_lib); + +use strict; +use Paparazzi::Environment; + +use constant APP_ID => "Paparazzi Speaker"; +use constant MESSAGE_WHEN_READY => APP_ID." : READY"; + +use strict; + +use IO::Socket; +use POSIX; +use Getopt::Long; +use Ivy; + +use Paparazzi::IvyProtocol; + + +sub new() { + + my ($proto, $festd_host, $festd_port) = @_; + my $self = { + 'ivy' => undef, + 'festival_handle' => undef, + 'vbat' => 0, + 'cur_wp' => -1, + 'cnt_nav' => 0, + }; + $self->{options} = { +# paparazzi_home => $paparazzi_home, + ivy_bus => "127.255.255.255:2010", + }; + + bless $self; + $self->parse_args(); + $self->start_ivy(); + $self->connect_to_festival($festd_host, $festd_port); + $self->say_hello(); + + # Trap signal in order to exit cleanly + $SIG{TERM} = \&catchSigTerm ; + + return $self; +} + +sub parse_args { + my ($self) = @_; + my $options = $self->{options}; + GetOptions ("b=s" => \$options->{ivy_bus}, + "t=s" => \$options->{paparazzi_home}, + ); +} + +sub start_ivy() { + my ($self) = @_; + Ivy->init (-ivyBus => $self->{options}->{ivy_bus}, + -appName => APP_ID, + -loopMode => 'LOCAL', + -messWhenReady => MESSAGE_WHEN_READY, + ) ; + my $paparazzi_home = Paparazzi::Environment::paparazzi_home(); + Paparazzi::IvyProtocol::read_protocol($paparazzi_home."/conf/messages.xml", "ground"); + Paparazzi::IvyProtocol::read_protocol($paparazzi_home."/conf/messages.xml", "aircraft_info"); + + $self->{ivy} = Ivy->new (-statusFunc => \&ivyStatusCbk) ; +# $self->{ivy}->bindRegexp (IvyMsgs::CALIB_START_Regexp(), [$self, \&ivyOnCalibStart]); +# $self->{ivy}->bindRegexp (IvyMsgs::CALIB_CONTRAST_Regexp(), [$self, \&ivyOnCalibContrast]); +# $self->{ivy}->bindRegexp (IvyMsgs::NAVIGATION_Regexp(), [$self, \&ivyOnNavigation]); +# $self->{ivy}->bindRegexp (IvyMsgs::BAT_Regexp(), [$self, \&ivyOnBat]); +# $self->{ivy}->bindRegexp (IvyMsgs::PPRZ_MODE_Regexp(), [$self, \&ivyOnPprzMode]); +# $self->{ivy}->bindRegexp (IvyMsgs::TAKEOFF_Regexp(), [$self, \&ivyOnTakeOff]); + $self->{ivy}->start() ; +} + +sub catchSigTerm() { + print ("in catchSigTerm\n"); + +} + +sub ivyStatusCbk { + print ("in ivyStatusCbk\n"); +} + +sub say_hello() { + my ($self) = @_; + $self->speak('Hello. Welcome to Paparazzi.'); +} + +sub ivyOnNavigation() { + my ($self, $sender, $cur_wp, $pos_x, $pos_y, $desired_course, $dist2_wp, $course_pgain) = @_; + # printf("NAVIGATION wp $cur_wp, x $pos_x, y $pos_y, dc $desired_course, d2wp $dist2_wp, cpg $course_pgain\n"); + + if ($self->{cur_wp} != $cur_wp) { + $self->speak(sprintf("current waypoint : %s.", $cur_wp)); + $self->{cur_wp} = $cur_wp; + $self->{cnt_nav} = 0; + } + else { + my $rdist = floor(sqrt($dist2_wp)/10)*10; + printf "dist2wp $rdist\n"; + if (($rdist ge 100 and $self->{cnt_nav} == 16) or + ($rdist ge 20 and $rdist le 100 and ($self->{cnt_nav})%5 == 0)) { + $self->speak(sprintf("distance to waypoint : %s. meters", $rdist)); + } + } + $self->{cnt_nav}++; +} + +sub ivyOnBat() { + my ($self, $sender, $voltage, $flight_time, $low_battery) = @_; + my $vbat = $voltage/10; + + if ($voltage le $low_battery) { + if ($self->{vbat} != $vbat) { + $self->speak(sprintf("battery : Warning : battery low : %s volts.", $vbat)); + $self->{vbat} = $vbat; + } + } + else { + if (abs($self->{vbat} - $vbat) ge 0.2) { + $self->speak(sprintf("battery : %s volts.", $vbat)); + $self->{vbat} = $vbat; + } + } +} + +sub ivyOnPprzMode() { + my @autopilot_mode_name=("manual", "auto one", "auto two", "home"); + my ($self, $sender, $ap_mode, $ap_altitude, $if_calib_mode, $mcu1_status, $lls_calib) = @_; + if ($self->{ap_mode} != $ap_mode) { + my $ap_str = $autopilot_mode_name[$ap_mode]; + $self->speak(sprintf("autopilot mode : %s.", $ap_str)); + $self->{ap_mode} = $ap_mode; + } +} + +sub ivyOnCalibStart() { + my ($self, $sender) = @_; + $self->speak("contrast calibration triggered"); +} + +sub ivyOnCalibContrast() { + my ($self, $sender, $adc) = @_; + my $pc_contrast = ceil($adc / 1024 * 100); + my $txt = sprintf("contrast %sper cent", $pc_contrast); + print "txt $txt\n"; + $self->speak($txt); +} + +sub ivyOnTakeOff() { + my ($self, $sender) = @_; + $self->speak('Take Off'); +} + +sub speak() { + my ($self, $what) = @_; + my $handle = $self->{festival_handle}; + my $sable_fmt = + ' + + + + %s + + + '; + + my $cmd_fmt = sprintf("(tts_text \"%s\" \'sable)\n", $sable_fmt); + my $fest_cmd = sprintf $cmd_fmt, $what; + print $handle $fest_cmd; +} + +sub connect_to_festival() { + my ($self, $host, $port) = @_; + $self->{festival_handle} = IO::Socket::INET->new(Proto => "tcp", + PeerAddr => $host, + PeerPort => $port); + $self->{festival_handle}->autoflush(1); + Ivy->fileEvent($self->{festival_handle}, [\&FestivalOnReceive, $self]); + print STDERR "[Connected to $host:$port]\n"; +} + +sub FestivalOnReceive { + my ($self) = @_; + my $file_stuff_key = "ft_StUfF_key"; # defined in speech tools + print "FestivalOnReceive\n"; + my $handle = $self->{festival_handle}; + my $line = <$handle>; + + # print "line: [$line]\n"; +# if ($line eq "WV\n") { # we have a waveform coming +# print "Waveform\n"; +# } + +# if ($line eq "LP\n") { # we have a waveform coming +# print "Lisp\n"; +# } +# if ($line =~ s/$file_stuff_key(.*)$//s) { +# print STDOUT $line; +# } +} + + + +PaparazziSpeak->new("localhost", 1314); +Ivy->mainLoop(); + diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile new file mode 100644 index 0000000000..0f32da2dbe --- /dev/null +++ b/sw/ground_segment/tmtc/Makefile @@ -0,0 +1,54 @@ +# +# $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. +# + +include ../../../conf/Makefile.local + +all: messages.cmo modem.cmo receive.cmo receive.opt messages.opt + +clean: + rm -f receive *.bak *~ core *.o .depend *.opt *.out *.cm* + +OCAMLC = ocamlc +OCAMLOPT = ocamlopt +INCLUDES= -I ../../lib/ocaml -I +lablgtk2 + +messages.opt : messages.ml + $(OCAMLOPT) $(INCLUDES) -o $@ unix.cmxa xml-light.cmxa glibivy-ocaml.cmxa -I +lablgtk2 lablgtk.cmxa gtkInit.cmx str.cmxa lib.cmxa $^ + strip $@ + +messages.run: + lablgtk2 str.cma -I ../../lib/ocaml ivy-ocaml.cma lib.cma messages.cmo + +receive.opt : modem.ml receive.ml + $(OCAMLOPT) $(INCLUDES) -o $@ str.cmxa unix.cmxa xml-light.cmxa glibivy-ocaml.cmxa -I +lablgtk2 lablgtk.cmxa lib.cmxa $^ + strip $@ + +receive.out : modem.ml receive.ml + $(OCAMLC) -g $(INCLUDES) -o $@ str.cma unix.cma xml-light.cma glibivy-ocaml.cma -I +lablgtk2 lablgtk.cma lib.cma $^ + + +receive.run: + lablgtk2 str.cma -I ../../lib/ocaml ivy-ocaml.cma lib.cma modem.cmo receive.ml + + +%.cmo : %.ml + $(OCAMLC) $(INCLUDES) -c $< diff --git a/sw/ground_segment/tmtc/bilink.ml b/sw/ground_segment/tmtc/bilink.ml new file mode 100644 index 0000000000..b2d0824150 --- /dev/null +++ b/sw/ground_segment/tmtc/bilink.ml @@ -0,0 +1,44 @@ +(* ocamlc -I ../../lib/ocaml unix.cma -I +lablgtk2 lablgtk.cma lib.cma bilink.ml *) + +(* Adresse carte sol : 01 18 04 c0 00 4f *) +(* Adresse carte embarquee : 01 18 04 c0 00 51 *) +let send = fun fd com -> + Wavecard.send fd com; + flush (Unix.out_channel_of_descr fd) + +(* Wavecard.send fd ("REQ_READ_RADIO_PARAM", "\000"); *) +(* Wavecard.send fd ("REQ_FIRMWARE_VERSION", ""); *) +(* Wavecard.send fd ("REQ_READ_RADIO_PARAM", "\005"); *) +(* Wavecard.send fd ("REQ_SEND_SERVICE", "\255\255\255\255\255\255\032"); *) +(* Wavecard.send fd ("REQ_SEND_SERVICE", "\001\024\004\192\000\079\032"); *) +(* Wavecard.send fd ("REQ_READ_REMOTE_RSSI", "\001\024\004\192\000\079"); *) +(* Wavecard.send fd ("REQ_SEND_MESSAGE", "\001\024\004\192\000\079HELLO WORLD");*) + + +let send_ack = fun delay fd -> + GMain.Timeout.add delay (fun _ -> send ("ACK", "")) + + +let print_cmd = fun (name, data) -> + Printf.fprintf stderr "%s:" name; + for i = 0 to String.length data - 1 do + Printf.fprintf stderr " %02x" (Char.code data.[i]) + done; + Printf.fprintf stderr "\n"; flush stderr + +let _ = + let dev = ref "/dev/ttyS0" in + Arg.parse + [ "-d", Arg.String (fun x -> dev := x), "Device\tDefault is /dev/ttyS0"] + (fun x -> prerr_endline ("Warning: don't know what to do with "^x)) + "Usage: "; + + let fd = if !dev = "" then Unix.stdin else Serial.opendev !dev Serial.B9600 in + + ignore (GMain.Timeout.add 2000 (fun _ -> send fd; true)); + + let cb = Wavecard.receive ~ack:(send_ack 100) print_cmd in + + ignore (GMain.Io.add_watch `IN (fun () -> cb fd; true) (GMain.Io.channel_of_descr fd)); + + GMain.Main.main () diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml new file mode 100644 index 0000000000..cfa238b85e --- /dev/null +++ b/sw/ground_segment/tmtc/messages.ml @@ -0,0 +1,139 @@ +open Printf + +let update_delay = 1. (* Min time in second before two updates *) +let led_delay = 500 (* Time in milliseconds while the green led is displayed *) + + +let space = Str.regexp "[ \t]+" + +let (//) = Filename.concat + +let xml_file = Env.paparazzi_src // "conf" // "messages.xml" + +(* let port = ref 2010 + let domain = ref "127.255.255.255" *) + +let green = "#00e000" +let red = "#ff0000" +let black = "#000000" +let yellow = "#ffff00" + +let led2 color1 color2 = [| +"16 16 5 1"; +" c None"; +". c black"; +"X c white"; +"o c "^color1; +"O c "^color2; +" ...... "; +" ........XX "; +" ...ooooooXXX "; +" ..ooooooooooXX "; +" ..oooXXXooooXX "; +"..oooXXXooooooXX"; +"..ooXXooooooooXX"; +"..ooXXooooooooXX"; +"..ooXoooooooooXX"; +"..ooooooooooooXX"; +"..ooooooooooooXX"; +" ..ooooooooooXX "; +" ..ooooooooooXX "; +" ...ooooooXXX "; +" ..XXXXXXXX "; +" XXXXXX "|] + +let led color = led2 color "None" + +let format = fun field -> + try + match Xml.attrib field "type", Xml.attrib field "format" with + "float", f -> fun x -> Printf.sprintf (Obj.magic f) (float_of_string x) + | _ -> fun x -> x + with _ -> fun x -> x + + + +open GMain +let _ = + let bus = ref "127.255.255.255:2010" in + let classes = ref ["telemetry_ap";"ground"] in + Arg.parse + [ "-b", Arg.String (fun x -> bus := x), "Bus\tDefault is 127.255.255.25:2010"; + "-c", Arg.String (fun x -> classes := x :: !classes), "class name"] + (fun x -> prerr_endline ("WARNING: don't do anything with "^x)) + "Usage: "; + + let xml = Xml.parse_file xml_file in + + let window = GWindow.window ~title:"Paparazzi messages" () in + let quit = fun () -> GMain.Main.quit (); exit 0 in + ignore (window#connect#destroy ~callback:quit); + + let notebook = GPack.notebook ~packing:window#add ~tab_pos:`LEFT () in + + let pm = fun color -> + GDraw.pixmap_from_xpm_d ~data:(led color) ~window:window () in + let black_led = pm black + and green_led = pm green + and yellow_led = pm yellow + and red_led = pm red in + + let xml_classes = + List.filter (fun x -> List.mem (Xml.attrib x "name") !classes) (Xml.children xml) in + + let messages = List.flatten (List.map Xml.children xml_classes) in + + let pages = + List.map + (fun m -> + let id = (Xml.attrib m "name") in + let h = GPack.hbox () in + let v = GPack.vbox ~width:200 () in + let l = GMisc.label ~text:id ~packing:h#add () in + let led = GMisc.pixmap black_led ~packing:h#pack () in + let time = GMisc.label ~text:"___" ~packing:h#pack () in + notebook#append_page ~tab_label:h#coerce v#coerce; + let fields = + List.map + (fun f -> + let h = GPack.hbox ~packing:v#pack () in + let unit = try "("^Xml.attrib f "unit"^")" with _ -> "" in + let name = Printf.sprintf "%s %s %s: " (Xml.attrib f "type") (Xml.attrib f "name") unit in + let _ = GMisc.label ~text:name ~packing:h#pack () in + let l = GMisc.label ~text:"XXXX" ~packing:h#pack () in + fun x -> let fx = format f x in if l#label <> fx then l#set_text fx + ) + (Xml.children m) in + let n = List.length fields in + let last_update = ref (Unix.gettimeofday ()) in + let time_since_last = ref 0 in + ignore (GMain.Timeout.add 1000 (fun () -> incr time_since_last; time#set_text (sprintf "%2d" !time_since_last); true)); + let display = fun line -> + time_since_last := 0; + let t = Unix.gettimeofday () in + if t > !last_update +. update_delay then begin + last_update := t; + let args = Str.split space line in + try + List.iter2 (fun f x -> f x) fields args; + + led#set_pixmap green_led; + ignore (GMain.Timeout.add led_delay (fun () -> led#set_pixmap yellow_led; false)) + with + Invalid_argument "List.iter2" -> + led#set_pixmap red_led; + Printf.fprintf stderr "%s: expected %d, got %d (%s)\n" id n (List.length args) line; flush stderr + end + in + let regexp = Printf.sprintf "[\\.0-9]+ %s (.*)" id in + ignore (Ivy.bind (fun _ args -> display args.(0)) regexp); + (id, (led, fields)) + ) + messages in + + window#show (); + + Ivy.init "Paparazzi messages" "READY" (fun _ _ -> ()); + Ivy.start !bus; + + GMain.Main.main () diff --git a/sw/ground_segment/tmtc/modem.ml b/sw/ground_segment/tmtc/modem.ml new file mode 100644 index 0000000000..cde4bce982 --- /dev/null +++ b/sw/ground_segment/tmtc/modem.ml @@ -0,0 +1,112 @@ +(* + * $Id$ + * + * Ground harware modem handling + * + * Copyright (C) 2004 CENA/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 Protocol = struct +(* Header: STX, length of (payload + checksum) *) +(* Payload: tag, data *) +(* Tailer : checksum, ETX *) + + let stx = Char.chr 0x02 + let etx = 0x03 + let index_start = fun buf -> + String.index buf stx + + let payload_length = fun buf start -> + Char.code buf.[start+1] - 1 + + let length = fun buf start -> + let len = String.length buf - start in + if len >= 2 then + Char.code buf.[start+1] + 3 + else + raise Serial.Not_enough + + let checksum = fun msg -> + let l = String.length msg in + let ck_a = ref 0 in + for i = 1 to l - 3 do + ck_a := Char.code msg.[i] lxor !ck_a + done; + !ck_a = Char.code msg.[l-2] && Char.code msg.[l-1] = etx +end + +let msg_data = 0 +let msg_error = 1 +let msg_cd = 2 +let msg_debug = 3 +let msg_valim = 4 + +type status = { + mutable last_message_date : float; + mutable valim : float; + mutable cd : int; + mutable error : int; + mutable debug : int; + mutable nb_byte : int; + mutable nb_msg : int; + mutable nb_err : int + } + +let max_stalled_time = 2. + +let status = { + last_message_date = Unix.gettimeofday () -. max_stalled_time; (* FIXME *) + valim = 0.; + cd = 0; + error = 0; + debug = 0; + nb_byte = 0; + nb_msg = 0; + nb_err = 0 +} +(* FIXME *) + let valim = fun x -> float x *. 0.0162863 -. 1.17483 +(* FIXME *) + +let parse = fun msg -> + let len = String.length msg in + let id = Char.code msg.[2] in + if id = msg_data then + Some (String.sub msg 3 (len-5)) + else begin + begin + match id with + | x when x = msg_error -> + status.error <- (Char.code msg.[3]) + | x when x = msg_cd -> + status.cd <- (Char.code msg.[3]) + | x when x = msg_debug -> + status.debug <- (Char.code msg.[3]) + | x when x = msg_valim -> + status.valim <- (valim (Char.code msg.[4] * 0x100 + Char.code msg.[3])); + printf "valim=%f\n" status.valim; flush stdout; + | _ -> (* Uncorrect id *) + status.nb_err <- status.nb_err + 1 + end; + None + end diff --git a/sw/ground_segment/tmtc/receive.ml b/sw/ground_segment/tmtc/receive.ml new file mode 100644 index 0000000000..93af00e9ef --- /dev/null +++ b/sw/ground_segment/tmtc/receive.ml @@ -0,0 +1,306 @@ +(* + * $Id$ + * + * Multi aircrafts receiver, logger and broadcaster + * + * Copyright (C) 2004 CENA/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 U = Unix + +module ModemTransport = Serial.Transport(Modem.Protocol) +module Tele_Class = struct let name = "telemetry_ap" end +module AcInfo = struct let name = "aircraft_info" end +module Tele_Pprz = Pprz.Protocol(Tele_Class) +module AcInfo_Pprz = Pprz.Protocol(AcInfo) +module PprzTransport = Serial.Transport(Tele_Pprz) + + +let listen_pprz_modem = fun use_pprz_message tty -> + (*** let fd = Serial.opendev tty Serial.B4800 in + ***) prerr_endline tty; + let fd = U.stdin in (***) + let use_pprz_buf = fun buf -> + Debug.call 'T' (fun f -> fprintf f "use_pprz: %s\n" (Debug.xprint buf)); + use_pprz_message (Tele_Pprz.values_of_bin buf) in + let buffer = ref "" in + let use_modem_message = fun msg -> + Debug.call 'T' (fun f -> fprintf f "use_modem: %s\n" (Debug.xprint msg)); + match Modem.parse msg with + None -> () (* Only internal modem data *) + | Some data -> + let b = !buffer ^ data in + Debug.call 'T' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b)); + let x = PprzTransport.parse use_pprz_buf b in + buffer := String.sub b x (String.length b - x) + in + let scanner = Serial.input (ModemTransport.parse use_modem_message) in + let cb = fun _ -> + begin + try + scanner fd + with + e -> fprintf stderr "%s\n" (Printexc.to_string e) + end; + true in + + ignore (Glib.Io.add_watch [`IN] cb (Glib.Io.channel_of_descr fd)) + +let fos = float_of_string +let ios = int_of_string +let space = Str.regexp "[ \t]+" + +let (//) = Filename.concat +let logs_path = Env.paparazzi_home // "var" // "logs" +let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml") + +type port = Ivy of string | Modem of string +type aircraft = { + port : port; + mutable roll : float; + mutable pitch : float; + mutable east : float; + mutable north : float; + mutable gspeed : float; + mutable course : float; + mutable alt : float; + mutable climb : float; + mutable cur_block : int; + mutable cur_stage : int; +(* warning twin engines ?? *) + mutable throttle : float; + mutable rpm : float; + mutable temp : float; + mutable bat : float; + mutable amp : float; + mutable energy : float; + mutable ap_mode : int; + mutable ap_altitude : int; + mutable if_calib_mode : int; + mutable mcu1_status : int; + mutable lls_calib : int; + } + +(** The aircrafts store *) +let aircrafts = Hashtbl.create 3 + +(** Broadcast of the received aircrafts *) +let aircrafts_msg_period = 5000 (* ms *) +let aircraft_msg_period = 1000 (* ms *) +let send_aircrafts_msg = fun () -> + let t = U.gettimeofday () in + let names = String.concat "," (Hashtbl.fold (fun k v r -> k::r) aircrafts []) in + Ivy.send (sprintf "ground AIRCRAFTS %s" names) +(* Ivy.send (sprintf "YOUOPIIIII") *) + +(* Opens the log file *) +(* FIXME : shoud open also an associated config file *) +let logger = fun () -> + let d = U.localtime (U.gettimeofday ()) in + let name = sprintf "%02d_%02d_%02d__%02d_%02d_%02d.log" (d.U.tm_year mod 100) (d.U.tm_mon+1) (d.U.tm_mday) (d.U.tm_hour) (d.U.tm_min) (d.U.tm_sec) in + if not (Sys.file_exists logs_path) then begin + printf "Creating '%s'\n" logs_path; flush stdout; + ignore (Sys.command (sprintf "mkdir -p %s" logs_path)) + end; + open_out (logs_path // name) + + +let log_and_parse = fun log ac_name a msg values -> + let t = U.gettimeofday () in + let s = String.concat " " (List.map snd values) in + fprintf log "%.2f %s %s %s\n" t ac_name msg.Pprz.name s; flush log; + Ivy.send (sprintf "%s RAW %.2f %s %s" ac_name t msg.Pprz.name s); + let value = fun x -> try List.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in + let fvalue = fun x -> fos (value x) in + match msg.Pprz.name with + "GPS" -> + a.east <- fvalue "east" /. 100.; + a.north <- fvalue "north" /. 100.; + a.gspeed <- fvalue "speed"; + a.course <- fvalue "course"; + a.alt <- fvalue "alt"; + a.climb <- fvalue "climb" + | "ATTITUDE" -> + a.roll <- fvalue "phi"; + a.pitch <- fvalue "theta" + | "NAVIGATION" -> + a.cur_block <- ios (value "cur_block"); + a.cur_stage <- ios (value "cur_stage") + | "CLIMB_PID" -> + a.throttle <- fvalue "gaz" /. 9600. *. 100.; + a.rpm <- a.throttle *. 100. + | "BAT" -> + a.bat <- fvalue "voltage" /. 10. + | "PPRZ_MODE" -> + a.ap_mode <- ios (value "ap_mode"); + a.ap_altitude <- ios (value "ap_altitude"); + a.if_calib_mode <- ios (value "if_calib_mode"); + a.mcu1_status <- ios (value "mcu1_status"); + a.lls_calib <- ios (value "lls_calib") + | _ -> () + + +(** Callback for a message from a soft simulator *) +let sim_msg = fun log ac_name a m -> + try + let (msg_id, values) = Tele_Pprz.values_of_string m in + let msg = Tele_Pprz.message_of_id msg_id in + log_and_parse log ac_name a msg values + with + Pprz.Unknown_msg_name x -> + fprintf stderr "Unknown message %s from %s: %s\n" x ac_name m + +let soi = string_of_int + +let send_aircraft_msg = fun ac -> + try + let sof = fun f -> sprintf "%.1f" f in + let a = Hashtbl.find aircrafts ac in + let values = ["roll", sof (Geometry_2d.rad2deg a.roll); + "pitch", sof (Geometry_2d.rad2deg a.pitch); + "east", sof a.east; + "north", sof a.north; + "speed", sof a.gspeed; + "heading", sof (Geometry_2d.rad2deg a.course); + "alt", sof a.alt; + "climb", sof a.climb] in + let _, fp_msg = AcInfo_Pprz.message_of_name "FLIGHT_PARAM" in + Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message fp_msg values)); + + let values = ["cur_block", soi a.cur_block;"cur_stage", soi a.cur_stage] + and _, ns_msg = AcInfo_Pprz.message_of_name "NAV_STATUS" in + Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message ns_msg values)); + + let values = ["throttle", sof a.throttle;"rpm", sof a.rpm;"temp", sof a.temp;"bat", sof a.bat;"amp", sof a.amp;"energy", sof a.energy] + and _, es_msg = AcInfo_Pprz.message_of_name "ENGINE_STATUS" in + Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message es_msg values)); + + let values = ["mode", soi a.ap_mode; "v_mode", soi a.ap_altitude] + and _, as_msg = AcInfo_Pprz.message_of_name "AP_STATUS" in + Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message as_msg values)) + with + Not_found -> prerr_endline ac + +let new_aircraft = fun id -> + { port = id ; roll = 0.; pitch = 0.; east = 0.; north = 0.; gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode=0; ap_altitude=0; if_calib_mode=0; mcu1_status=0; lls_calib=0 } + +let register_aircraft = fun name a -> + Hashtbl.add aircrafts name a; + ignore (Glib.Timeout.add aircraft_msg_period (fun () -> send_aircraft_msg name; true)) + + +(** Callback of an identifying message from a soft simulator *) +let ident_msg = fun log id name -> + if not (Hashtbl.mem aircrafts name) then begin + let ac = new_aircraft (Ivy id) in + let b = Ivy.bind (fun _ args -> sim_msg log name ac args.(0)) (sprintf "^%s +(.*)" id) in + register_aircraft name ac + end + +(* Waits for new simulated aircrafts *) +let listen_sims = fun log -> + ignore (Ivy.bind (fun _ args -> ident_msg log args.(0) args.(1)) "^(.*) IDENT +(.*)") + +(* Server on the Ivy bus *) +let send_flight_plan = fun id -> + try + let conf = ExtXml.child conf_xml "aircraft" ~select:(fun x -> ExtXml.attrib x "name" = id) in + let f = ExtXml.attrib conf "flight_plan" in + Ivy.send (sprintf "ground FLIGHT_PLAN %s file://%s/conf/%s" id Env.paparazzi_home f) + with + Not_found -> + Ivy.send (sprintf "ground UNKNOWN %s" id) + +let send_config = fun id_ac id_req -> + try + prerr_endline (sprintf "[%s] [%s]\n" id_ac id_req); + let conf = ExtXml.child conf_xml "aircraft" ~select:(fun x -> ExtXml.attrib x "name" = id_ac) in + let fp = sprintf "%s/conf/%s" Env.paparazzi_home (ExtXml.attrib conf "flight_plan") and + af = sprintf "%s/conf/%s" Env.paparazzi_home (ExtXml.attrib conf "airframe") and + rc = sprintf "%s/conf/%s" Env.paparazzi_home (ExtXml.attrib conf "radio")in + let resp = sprintf "%s CONFIG_RES %s %s %s %s" id_ac id_req fp af rc in + Ivy.send (resp); + prerr_endline (resp) + with + Not_found -> + Ivy.send (sprintf "ground UNKNOWN %s" id_req) + +let server = fun () -> + ignore (Ivy.bind (fun _ args -> send_aircrafts_msg ()) "^ask AIRCRAFTS"); + ignore (Ivy.bind (fun _ args -> send_flight_plan args.(0)) "^ask FLIGHT_PLAN +(.*)"); + ignore (Ivy.bind (fun _ args -> send_config args.(0) args.(1)) "^(.*) CONFIG_REQ +(.*)") + +let handle_pprz_message = fun log a -> + let name = ref None (*** register_aircraft "log_twinstar" a; Some "log_twinstar" ***) in + fun (msg_id, values) -> + prerr_endline "handle_pprz_message"; + let msg = Tele_Pprz.message_of_id msg_id in + match !name with + None -> + if msg.Pprz.name = "IDENT" then + let n = List.assoc "id" values in + name := Some n; + register_aircraft n a + | Some ac_name -> + log_and_parse log ac_name a msg values + +let listen_link = fun log xml_link -> + match ExtXml.attrib xml_link "protocol" with + "pprz/modem" -> + (* Hyp: One single A/C on this channel *) + let port = ExtXml.attrib xml_link "port" in + let ac = new_aircraft (Modem port) in + listen_pprz_modem (handle_pprz_message log ac) port + | _ -> fprintf stderr "Warning: Ignoring link '%s'\n" (ExtXml.attrib xml_link "name") + + + +(* main loop *) +let _ = + let xml_ground = ExtXml.child conf_xml "ground" in + let ivy_bus = ref (ExtXml.attrib xml_ground "ivy_bus") in + let options = + [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf "Bus\tDefault is %s" !ivy_bus)] in + Arg.parse (options) + (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x) + "Usage: "; + + Ivy.init "Paparazzi receive" "READY" (fun _ _ -> ()); + Ivy.start !ivy_bus; + + + (* Opens the log file *) + let log = logger () in + + (* Waits for new simulated aircrafts *) + listen_sims log; + + (* Listen on links *) + List.iter (listen_link log) (Xml.children xml_ground); + + (* Sends periodically alive aircrafts *) + ignore (Glib.Timeout.add aircrafts_msg_period (fun () -> send_aircrafts_msg (); true)); + + server (); + + let loop = Glib.Main.create true in + while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done diff --git a/sw/ground_segment/visu3d/Help_Keys.txt b/sw/ground_segment/visu3d/Help_Keys.txt new file mode 100644 index 0000000000..32e2f2e21b --- /dev/null +++ b/sw/ground_segment/visu3d/Help_Keys.txt @@ -0,0 +1,26 @@ + Bouton gauche : + --------------- + +Rotation de la vue + + Bouton milieu : + --------------- + +Zoom/Unzoom en allant en avant/en arriere + + + Molette : + --------- + - zoom/unzoom + + Touches clavier : + ----------------- + + - Espace : lance/stoppe l'animation + - + et - du pave numerique : accelere ou ralentit l'animation + - Home : vue du dessus + - Page_Up / Page_Down : zoom/unzoom + - Touches flechees : deplacement de la vue + - Fleches du pave numerique : rotation de la vue + - r : affichage de la rosace + - F12 : capture ecran \ No newline at end of file diff --git a/sw/ground_segment/visu3d/Makefile b/sw/ground_segment/visu3d/Makefile new file mode 100644 index 0000000000..1b36cf18ca --- /dev/null +++ b/sw/ground_segment/visu3d/Makefile @@ -0,0 +1,48 @@ +OCAMLOPT0 = ocamlopt +OCAMLC = ocamlc + +MLFLAGS = -I +lablgtk2 -I +lablGL -I +camlimages -I ../../lib/ocaml + +OCAMLOPT = $(OCAMLOPT0) $(OCAMLOPT_OPTIONS) + +SRC = mapGL.ml + +OBJS= $(SRC:.ml=.cmo) + +LINK= $(OCAMLC) $(MLFLAGS) +LIBS_CI = ci_core.cma ci_gif.cma ci_jpeg.cma ci_tiff.cma ci_bmp.cma ci_ppm.cma ci_png.cma \ + ci_xpm.cma ci_ps.cma ci_freetype.cma +STDLIBS = unix.cma str.cmxa xml-light.cma lablgtk.cma lablgl.cma lablgtkgl.cma $(LIBS_CI) +ADD_LIBS = lib.cma xlib.cma glibivy-ocaml.cma +CLIBS = -cclib -lpthread + +all: mapGL.opt + +clean: + \rm -f *.cm* *.o *.a *~ *.opt *.out *.top *.output *obj *exe \ + stars_lexer.ml stars_parser.mli stars_parser.ml .depend + +# Executables +mapGL.out: $(OBJS) + $(OCAMLC) $(MLFLAGS) $(STDLIBS) gtkInit.cmo $(ADD_LIBS) -o $@ $(OBJS_3D) $(OBJS) $(CLIBS) + +mapGL.opt: $(OBJS:.cmo=.cmx) + $(OCAMLOPT) $(MLFLAGS) $(STDLIBS:.cma=.cmxa) gtkInit.cmx $(ADD_LIBS:.cma=.cmxa) -o $@ $(OBJS:.cmo=.cmx) $(CLIBS) + +# Do not edit below this line + +.depend: + ocamldep *.mli *.ml *.mly *.mll > .depend + +.SUFFIXES: .ml .mli .cmo .cmi .cmx + +.ml.cmo: + $(OCAMLC) $(MLFLAGS) -labels -w s -c $< +.mli.cmi: + $(OCAMLC) $(MLFLAGS) -labels -w s -c $< +.ml.cmx: + $(OCAMLOPT) $(MLFLAGS) -labels -w s -c $< + +ifneq ($(MAKECMDGOALS),clean) +-include .depend +endif diff --git a/sw/ground_segment/visu3d/TODO b/sw/ground_segment/visu3d/TODO new file mode 100644 index 0000000000..9f4ac6883e --- /dev/null +++ b/sw/ground_segment/visu3d/TODO @@ -0,0 +1,20 @@ + + +add waypoint icons +add aircraft icon + +add camera trace on ground + +manage aircraft track length (limit to n minutes) + +map video frame (photos) on ground surface +transform photos (translate, rotate, zoom) +save photos position and visibility for future reload. +manage photos (like layers in a cad program ) + +add fact_alti adjust + +support several aircrafts, maybe with an identification label (3D radar track ? ) +select aircraft, waypoints (would it be possible??) + +support viewpoint selection using function keys - viewpoints are defined in the flight plan ? diff --git a/sw/ground_segment/visu3d/mapGL.ml b/sw/ground_segment/visu3d/mapGL.ml new file mode 100644 index 0000000000..97dfc7bb62 --- /dev/null +++ b/sw/ground_segment/visu3d/mapGL.ml @@ -0,0 +1,493 @@ +(* + * $Id$ + * + * 3D OpenGL visualisation + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec, 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 Ocaml_tools +open Geometry_3d + +open Gtk_3d +open Latlong + +let fos = fun x -> + try + float_of_string x + with + Failure("float_of_string") -> failwith ("float_of_string: "^ x) +let float_attrib = fun xml a -> fos (ExtXml.attrib xml a) + +(* Version de l'appli *) +let version = "0.1" + +(* 1 point tous les 50m *) +let dx = 50 and dy = 50 + +(* Facteur d'echelle pour les altitudes *) +let fact_alti = 3 + +(* Taille de la fenetre d'affichage *) +let width = 800 and height = 600 + +let tolerance_alti = 100. + +let default_color_trajs = ref (`NAME "red") + +let home = Env.paparazzi_home +let (//) = Filename.concat +let default_path_SRTM = home // "data" // "SRTM" +let default_path_maps = home // "data" // "" +let default_path_traj = home // "var" // "" +let default_path_missions = home // "conf" + +let color_pixmaps = Hashtbl.create 101 + +let limits = ref ((min_float,min_float), (max_float, max_float)) + +let track_filter = fun points -> + let ((minx,miny), (maxx, maxy)) = !limits in + let rec loop = function + [] -> [] + | (t,x,y,a)::ps -> + if minx < x && x < maxx && miny < y && y < maxy + then (t,x,y,a)::loop ps + else loop ps in + loop points + +(* Fichier d'aide contenant les touches clavier utilisees *) +let filename_help_keys = Env.paparazzi_src // "conf" // "Help_Keys.txt" + +(* ============================================================================= *) +(* = Passage de couleur GTK vers GL = *) +(* = = *) +(* = color = couleur GTK (`NAME ou `RGB) a transformer = *) +(* ============================================================================= *) +let gtk_to_gl_color color = + let t = GDraw.color color in + ((float_of_int (Gdk.Color.red t))/.65535.0, + (float_of_int (Gdk.Color.green t))/.65535.0, + (float_of_int (Gdk.Color.blue t))/.65535.0) + +(* ============================================================================= *) +(* = Passage de couleur GL vers GTK = *) +(* = = *) +(* = (r, g, b) = couleur GL a transformer en equivalent GTK = *) +(* ============================================================================= *) +let gl_to_gtk_color (r, g, b) = + `RGB(int_of_float (r*.65535.0), int_of_float (g*.65535.0), + int_of_float (b*.65535.0)) + +(* ============================================================================= *) +(* = Lecture d'un fichier de trajectoire avec correction des points etranges = *) +(* ============================================================================= *) +let read_traj_file filename = + let traj = ref [] and prev_alti = ref None in + let corrige_alt alt = + let x = + match !prev_alti with + None -> alt + | Some prev_alti -> + if abs_float(alt-.prev_alti) + (try + if mode = "3" then (* Else no pertinent info available *) + let t = fos time + and utm_x = (fos utm_x)/.100. + and utm_y = (fos utm_y)/.100. + and alt = (fos alt) in + (* Filtrage des altitudes incorrectes *) + let alt = corrige_alt alt in + traj:=(t, utm_x, utm_y, alt)::!traj + with _ -> error_func ()) + | _ -> () + in + do_read_file filename match_func (fun () -> ()) ; + let traj' = track_filter (List.rev !traj) in + (traj', !default_color_trajs) + +(* ============================================================================= *) +(* = Ajout de la surface = *) +(* ============================================================================= *) +let add_surface view3d texture_file (min_x, min_y) (max_x, max_y) utm_zone = + (* Creation de la texture a partir d'une image *) + Printf.printf "Lecture texture..."; flush stdout ; + let texture_id = Gtk_3d.create_texture_from_image texture_file in + Printf.printf " OK\n"; flush stdout ; + + (* Creation d'une matrice contenant les elevations *) + let nx = (max_x-min_x)/dx+1 and ny = (max_y-min_y)/dy+1 in + let tab = Array.make_matrix ny nx {x3D=0.; y3D=0.; z3D=0.} in + + let y = ref max_y and i = ref 0 in + try + for i = 0 to ny - 1 do + let x = ref min_x in + for j = 0 to nx - 1 do + let alt = (Srtm.of_utm {utm_x = float !x; utm_y = float !y; utm_zone = utm_zone})*fact_alti in + tab.(i).(j) <- {x3D = float !x; y3D= float !y; z3D = float alt} ; + x:=!x+dx + done ; + y:=!y-dy + done ; + + (* Ajout de cette matrice a la vue 3D *) + view3d#add_object_surface_with_texture tab texture_id + with + Srtm.Tile_not_found s -> + failwith (Printf.sprintf "SRTM tile '%s' not found, you can download it with %s" s (Srtm.error s)) + +(* ============================================================================= *) +(* = Ajout d'une trajectoire = *) +(* ============================================================================= *) +let point3D = fun (_, utm_x, utm_y, alt) -> {x3D=utm_x; y3D=utm_y; z3D=alt*. float fact_alti} +let add_traj view3d (points, id) = + let l = List.map point3D points in + + let color = gtk_to_gl_color id in + view3d#add_object_line l color 2 false false + +(* Adding one more point to a track *) +let last_points = Hashtbl.create 11 +let add_point (view3d:Gtk_3d.widget_3d) (point, id) = + let p = point3D point in + try + let last = Hashtbl.find last_points id in + let color = gtk_to_gl_color id in + view3d#display (view3d#add_object_line [last;p] color 2 false false); + Hashtbl.replace last_points id p + with + Not_found -> + Hashtbl.add last_points id p + +(* ============================================================================= *) +(* = Load a map. Use SRTM elevation data to produce a 3d surface = *) +(* ============================================================================= *) +let load_surface view3d id_sol xml_map_file = + let min_x = ref max_int and min_y = ref max_int + and max_x = ref min_int and max_y = ref min_int + and texture_file = ref "" in + let xml = Xml.parse_file xml_map_file in + let texture_file = Xml.attrib xml "file" in + let texture_file = Filename.concat (Filename.dirname xml_map_file) texture_file in + let (_format, header) = Images.file_format texture_file in + let int_attrib x a = int_of_string (Xml.attrib x a) in + begin + match Xml.children xml with + p::_ -> + let utm_x = float_attrib p "utm_x" + and utm_y = float_attrib p "utm_y" + and x = float_attrib p "x" + and y = float_attrib p "y" + and scale = float_attrib xml "scale" in + min_x := truncate (utm_x -. scale *. x); + min_y := truncate (utm_y -. scale *. (float header.Images.header_height -. y)); + max_x := truncate (utm_x +. scale *. (float header.Images.header_width -. x)); + max_y := truncate (utm_y +. scale *. y) + | _ -> failwith "load_surface" + end; + begin + match !id_sol with + Some x -> view3d#delete_object x + | None -> () + end; + let utm_zone = try int_of_string (Xml.attrib xml "utm_zone") with _ -> Printf.fprintf stderr "Warning: utm_zone attribute not specified in '%s'; default is 31\n" xml_map_file; flush stderr; 31 in + id_sol:= Some (add_surface view3d texture_file (!min_x, !min_y) (!max_x, !max_y) utm_zone); + limits := ((float !min_x, float !min_y), (float !max_x, float !max_y)); + view3d#display_func + + +let load_mission = fun (view3d:Gtk_3d.widget_3d) xml -> + let wps = ExtXml.child xml "waypoints" in + let utm_x0 = float_attrib wps "utm_x0" + and utm_y0 = float_attrib wps "utm_y0" in + let display_waypoint = fun wp -> + let utm_x = float_attrib wp "x" +. utm_x0 + and utm_y = float_attrib wp "y" +. utm_y0 + and alt = float_attrib wp "alt" in + let p3d = point3D (0., utm_x, utm_y, alt) + and p3d_label = point3D (0., utm_x+.10., utm_y+.10., alt) in + view3d#display (view3d#add_object_point p3d p3d_label (ExtXml.attrib wp "name") (gtk_to_gl_color (`NAME "red")) true) in + List.iter display_waypoint (Xml.children wps) + +(* ============================================================================= *) +(* = Map loading callback = *) +(* ============================================================================= *) +let on_load_surface win view3d id_sol () = + let priv_load_surf xml_map_file = + load_surface view3d id_sol xml_map_file + in + try + Gtk_tools.open_file_dlg "Map calibration file" priv_load_surf None default_path_maps false + with x -> + Gtk_tools.error_box win "Read error" (Printexc.to_string x) + + +(* ============================================================================= *) +(* = Chargement d'une trajectoire = *) +(* ============================================================================= *) +let load_trajectory view3d lst_ids_trajs () = + let read_data f = + let new_traj = read_traj_file f in + lst_ids_trajs:=(add_traj view3d new_traj)::!lst_ids_trajs ; + (* Force la mise a jour de l'affichage *) + view3d#display_func + in + + Gtk_tools.open_file_dlg "Track" read_data None default_path_traj false + +(* ============================================================================= *) +(* = Recherche/Ajout d'une pixmap de couleur dans la table = *) +(* ============================================================================= *) +let get_color_pixmap win color = + let taille_x = 20 and taille_y = 8 in + try Hashtbl.find color_pixmaps color + with Not_found -> + let pm = Gtk_tools.rectangle_pixmap win color taille_x taille_y in + Hashtbl.add color_pixmaps color pm ; + pm + +(* ============================================================================= *) +(* = Selection de trajectoires = *) +(* ============================================================================= *) +let build_lst_traj tooltips view3d lst_ids_trajs () = + let get_id idx = try List.nth !lst_ids_trajs idx with _ -> (-1) in + + let (window,boite) = Gtk_tools.create_window "Liste des trajectoires" 450 300 in + let lst = Gtk_tools.create_managed_list + [("Id", 40); ("Sel.", 40); ("Color", 60)] boite#add + in + let buts = Gtk_tools.create_buttons + [("Hide", "Hide the track"); + ("Display", "Display the track"); + ("Color", "Change the color"); + ("Delete", "Delete the track") ; + ("Close", "Close the window")] tooltips boite#pack + in + let but_masque = List.nth buts 0 and but_aff = List.nth buts 1 + and but_couleur = List.nth buts 2 and but_del = List.nth buts 3 in + Gtk_tools.set_sensitive_list + [but_couleur; but_del; but_masque; but_aff] false ; + + let current_selection = ref (-1) and current_idx = ref "" in + let callback_traj index _ selection = + if selection then begin + current_selection:=get_id (int_of_string index) ; + current_idx:=index ; + let masquable = view3d#object_get_visibility !current_selection in + Gtk_tools.set_sensitive but_masque masquable ; + Gtk_tools.set_sensitive but_aff (not masquable) ; + Gtk_tools.set_sensitive_list [but_couleur; but_del] true ; + end else begin + current_selection:=(-1); current_idx:="" ; + Gtk_tools.set_sensitive_list + [but_couleur; but_del; but_masque; but_aff] false + end + in + let fill_list_traj = Gtk_tools.connect_managed_list + lst 0 callback_traj ("track", true, true) + in + let fill_list () = + let to_select = !current_idx in + current_selection:=(-1); current_idx:="" ; + let n = ref (-1) in + let l = List.map (fun id -> + incr n ; + [string_of_int !n; + (if view3d#object_get_visibility id then " x " else ""); ""] + ) !lst_ids_trajs in + fill_list_traj to_select l ; + let row = ref 0 in + List.iter (fun id -> + let c = gl_to_gtk_color (view3d#object_get_color id) in + (fst lst)#set_cell !row 2 ~pixmap:(get_color_pixmap window c) ; + incr row) !lst_ids_trajs + in + fill_list () ; + + let func_masque_aff affiche = + if !current_selection<>(-1) then begin + view3d#object_set_visibility !current_selection affiche ; + view3d#display_func ; + fill_list () + end + in + let change_color () = + if !current_selection<>(-1) then begin + Gtk_tools.select_color (fun color -> + view3d#object_set_color !current_selection (gtk_to_gl_color color) ; + view3d#display_func ; + fill_list ()) ; + end + in + let delete_traj () = + if !current_selection<>(-1) then begin + view3d#delete_object !current_selection; view3d#display_func ; + lst_ids_trajs:= + List.filter (fun id -> id <> !current_selection) !lst_ids_trajs ; + fill_list () + end + in + + Gtk_tools.create_buttons_connect buts + [(fun () -> func_masque_aff false); (fun () -> func_masque_aff true); + change_color; delete_traj; + (fun () -> window#destroy (); view3d#display_func)] ; + window#show () + +(* ============================================================================= *) +(* = Fenetre About = *) +(* ============================================================================= *) +let build_fen_about () = + (* Creation de la liste des fichiers de l'animation *) + let l = ref [] and max_pixmaps = 15 in + for i=1 to max_pixmaps do + l:=(Printf.sprintf "Pixmaps/avion%d.xpm" i)::!l + done ; + + let message = Printf.sprintf "Visu Drone v%s\n" version in + + Gtk_tools.animated_msg_box "About" message (List.rev !l) + +(* ============================================================================= *) +(* = Creation de l'interface = *) +(* ============================================================================= *) +let build_interface = fun map_file mission_file -> + let nb_menus = ref 0 in + + (* Liste des menus disponibles *) + let liste_menus = ["Map"; "Tracks"; "Parameters"] in + + (* Mise en place des couleurs correctes *) + Gtk_tools.init_colors () ; + + (* Initialisation de l'aide contextuelle *) + let tooltips = Gtk_tools.init_tooltips () in + + (* Creation d'une fenetre *) + let (window, vbox, factory, accel_group, menus, menu_help) = + Gtk_tools.create_window_with_menubar_help ("Visu Drone v"^version) + width height liste_menus in + window#connect#destroy ~callback:GMain.Main.quit; + + (* Creation du Widget OpenGL *) + let view3d = new widget_3d vbox#add false "" in + + (* Ajout des objets a la vue *) + let id_sol = ref None in + let lst_ids_trajs = ref [] in + + (* Creation des menus : Sol *) + let factory = new GMenu.factory menus.(!nb_menus) ~accel_group in + incr nb_menus ; + factory#add_item "Load Background" + ~callback:(on_load_surface window view3d id_sol) ; + + (* Creation des menus : Trajectoires *) + let factory = new GMenu.factory menus.(!nb_menus) ~accel_group in + incr nb_menus ; + factory#add_item "Load Track" ~callback:(load_trajectory view3d lst_ids_trajs) ; + factory#add_item "Edit Tracks" + ~callback:(build_lst_traj tooltips view3d lst_ids_trajs) ; + + (* Creation des menus : Parametres *) + let factory = new GMenu.factory menus.(!nb_menus) ~accel_group in + incr nb_menus ; + factory#add_item "Edit" ~callback:(fun () -> ()) ; + + (* Aide *) + let factory = new GMenu.factory menu_help in + factory#add_item "A propos" ~callback:build_fen_about ; + factory#add_item "Help keys/mouse" + ~callback:(fun () -> Gtk_tools.display_file filename_help_keys + "Aide touches clavier" 370 500 tooltips (Some "fixed")) ; + + (* Affichage de la fenetre principale *) + window#show () ; + +(* let gps_regexp = "([a-z]*) +GPS +[0-9]* +([0-9]*) +([0-9]*) +[0-9\\.]* +([0-9\\.]*)" in + ignore (Ivy.bind (fun _ args -> add_point view3d ((0., fos args.(1)/.100.,fos args.(2)/.100., fos args.(3)), `NAME "green")) gps_regexp); *) + + + let flight_param_regexp = "([a-z0-9]*) +FLIGHT_PARAM +[0-9\\.]* +[0-9\\.]* +([0-9\\.]*) +([0-9\\.]*) +[0-9\\.]* +[0-9\\.]* +([0-9\\.]*) +[0-9\\.]*" in + ignore (Ivy.bind (fun _ args -> + let name= args.(0) and + x = fos args.(1) and + y = fos args.(2) and + z = fos args.(3) in + (* Printf.fprintf stderr "############ %f %f %f\n" x y z; *) + if (Str.string_match (Str.regexp_string "twinstar1") name 0) then + add_point view3d ((0., x, y, z), `NAME "green"); + if (Str.string_match (Str.regexp_string "twinstar2") name 0) then + add_point view3d ((0., x, y, z), `NAME "blue"); +(* Printf.fprintf stderr "############\n"; *) + ) flight_param_regexp); + + + (* Loading an initial map *) + if map_file <> "" then begin + let xml_map_file = Filename.concat (default_path_maps) map_file in + load_surface view3d id_sol xml_map_file + end; + + (* Loading an initial mission *) + if mission_file <> "" then begin + let xml_file = Filename.concat (default_path_missions) mission_file in + load_mission view3d (Xml.parse_file xml_file) + end; + + (* Lancement de la mainloop *) + Gtk_tools.main_loop () + +(* ============================================================================= *) +(* = Programme principal = *) +(* ============================================================================= *) +let _ = + let ivy_bus = ref "127.255.255.255:2010" and + map_file = ref "" and + mission_file = ref "" in + let options = + [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"; + "-m", Arg.String (fun x -> map_file := x), "Map description file"; + "-f", Arg.String (fun x -> mission_file := x), "Mission description file"] in + Arg.parse (options) + (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x) + "Usage: "; + (* *) + Ivy.init "Paparazzi 3d visu" "READY" (fun _ _ -> ()); + Ivy.start !ivy_bus; + + Srtm.add_path default_path_SRTM; + + (* Lancement de l'interface *) + build_interface !map_file !mission_file + + +(* =============================== FIN ========================================= *) diff --git a/sw/ground_segment/wind/Makefile b/sw/ground_segment/wind/Makefile new file mode 100644 index 0000000000..4267b10be0 --- /dev/null +++ b/sw/ground_segment/wind/Makefile @@ -0,0 +1,68 @@ +# +# $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. +# + +include ../../../conf/Makefile.local + +all: wind.opt + +INCLUDES= -I ../../lib/ocaml -I +lablgtk2 + +OCAMLC= ocamlc -g $(INCLUDES) +OCAMLMLI= ocamlc $(INCLUDES) +OCAMLOPT= ocamlopt $(INCLUDES) +OCAMLDEP= ocamldep $(INCLUDES) + + + +wind.opt : wind.cmx + $(OCAMLOPT) -o $@ xml-light.cmxa glibivy-ocaml.cmxa lablgtk.cmxa str.cmxa lib.cmxa $< + strip $@ + +.SUFFIXES: +.SUFFIXES: .ml .mli .mly .mll .cmi .cmo .cmx .out .opt .p.cmx .popt + +.ml.cmo : + $(OCAMLC) -c $< +.mli.cmi : + $(OCAMLMLI) -c $< +.ml.cmx : + $(OCAMLOPT) -c $< +# To produce profiled objects +.ml.p.cmx : + $(OCAMLOPT) -p -c $< + mv $*.cmx $@ + mv $*.o $*.p.o +.cmo.out : + $(OCAMLC) -o $@ $< +# To produce profiled binaries +.p.cmx.popt : + $(OCAMLOPT) -p -o $@ $< +.cmx.opt : + $(OCAMLOPT) -o $@ $< + +clean: + \rm -f *.cmo *.cmi *.cmx *.o *~ *.opt *.out .depend *.popt + +.depend: + $(OCAMLDEP) *.mli *.ml > $@ + +include .depend diff --git a/sw/ground_segment/wind/wind.ml b/sw/ground_segment/wind/wind.ml new file mode 100644 index 0000000000..b0bbcb9cfe --- /dev/null +++ b/sw/ground_segment/wind/wind.ml @@ -0,0 +1,262 @@ +(* + * $Id$ + * + * Multi aircrafts receiver, logger and broadcaster + * + * Copyright (C) 2004 CENA/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. + * + *) + +(* + * + * Estimate wind by analysing aircrafts trajectories + * + * Author : Nicolas Barnier - barnier@recherche.enac.fr + * + *) + +let debug = false + +open Printf + +let (//) = Filename.concat +let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml") +let xml_ground = ExtXml.child conf_xml "ground" +let ivy_bus = ref (ExtXml.attrib xml_ground "ivy_bus") + +open Geometry_2d + +type point_val = {p : pt_2D; f : float} + +type triangle = {a: point_val; b: point_val; c: point_val} + +let bary t = barycenter [t.a.p; t.b.p; t.c.p] + +let init w_init step f = + let pb = vect_add w_init {x2D = step; y2D = 0.} + and pc = vect_add w_init {x2D = 0.; y2D = step} in + {a = {p = w_init; f = f w_init}; + b = {p = pb; f = f pb}; + c = {p = pc; f = f pc}} + +let shift pa fa t = {a = {p = pa; f = fa}; b = t.a; c = t.b} + +let shiftpv pf t = shift pf.p pf.f t + +let calcnew p b alpha = vect_add_mul_scal alpha b (vect_make b p) + +let triangle_sort t = + let abc = [|t.a; t.b; t.c|] in + Array.sort (fun t1 t2 -> compare t2.f t1.f) abc; + {a = abc.(0); b = abc.(1); c = abc.(2)} + +let norme2 p = p.x2D *. p.x2D +. p.y2D *. p.y2D + +let simplex p fmax step max_iter precision = + let f x = -. (fmax x) in + + let rec loop num_iter vs = + if num_iter < max_iter && norme2 (vect_make vs.a.p vs.c.p) > precision then begin + begin if debug then + let pa = cart2polar vs.a.p in + Printf.printf "%f %f %f\n" pa.theta2D pa.r2D (-. vs.a.f) end; + + let vb = bary vs in + let vr = calcnew vs.c.p vb (-1.) in + let fvr = f vr in + let new_vs = + if fvr > vs.a.f then + let ve = calcnew vs.c.p vb (-2.) in + let fve = f ve in + if fve > fvr then shift ve fve vs + else shift vr fvr vs + else + let vc = calcnew vs.c.p vb 0.5 in + let fvc = f vc in + if fvc > vs.b.f || fvr > vs.b.f then + let v = if fvr > fvc then {p = vr; f = fvr} else {p = vc; f = fvc} in + if v.f <= vs.b.f then {vs with c = v} + else if v.f > vs.a.f then shiftpv v vs + else {vs with b = v; c = vs.b} + else + let vcb = calcnew vs.b.p vs.a.p 0.5 + and vcc = calcnew vs.c.p vs.a.p 0.5 in + triangle_sort {vs with b = {p = vcb; f = f vcb}; c = {p = vcc; f = f vcc}} in + + loop (num_iter + 1) new_vs end + else vs.a in + + if debug then Printf.printf "%f %f %f\n" p.x2D p.y2D (fmax p); + let vs = init p step f in + let vs = triangle_sort vs in + loop 0 vs + + +let isotropic_mean wind speeds = + let n = Array.length speeds in + let air_speeds = Array.map (fun speed -> cart2polar (vect_sub speed wind)) speeds in + let weights = + Array.map + (fun air -> + let sum = + Array.fold_left + (fun acc airj -> + acc +. norm_angle_rad (abs_float (air.theta2D -. airj.theta2D)) /. m_pi) + 0. air_speeds in + sum /. (float (n-1))) + air_speeds in + let mean = ref 0. in + for i = 0 to n-1 do + mean := !mean +. vect_norm (vect_sub speeds.(i) wind) *. weights.(i) done; + !mean /. float n + +let isotropic_wind wind_init speeds precision = + let n = Array.length speeds in + let mean wind = + let air_speeds = Array.map (fun speed -> cart2polar (vect_sub speed wind)) speeds in + let weights = + Array.mapi + (fun i airi -> + let sum = ref 0. in + for j = 0 to n-1 do + if j <> i then + sum := !sum +. + norm_angle_rad (abs_float (airi.theta2D -. air_speeds.(j).theta2D)) /. m_pi + done; + !sum /. (float (n-1))) + air_speeds in + let sum_weights = Array.fold_left (+.) 0. weights in + + let mean = ref 0. in + for i = 0 to n-1 do + mean := !mean +. vect_norm (vect_sub speeds.(i) wind) *. weights.(i) done; + (!mean /. sum_weights, sum_weights, weights) in + + let nb_calls = ref 0 in + let cost wind = + incr nb_calls; + let (m, sum_weights, weights) = mean wind in + let sum = ref 0. in + for i = 0 to n-1 do + let err = weights.(i) *. (vect_norm (vect_sub speeds.(i) wind) -. m) in + sum := !sum +. err *. err done; + !sum /. sum_weights in + + let step = 2. and max_iter = 100 in + let wind = simplex wind_init cost step max_iter precision in + if debug then Printf.printf "nb calls: %d\n" !nb_calls; + + let (mean, _, _) = mean wind.p in + (wind.p, mean, wind.f) + + +(* val wind : Geometry_2d.pt_2D -> Geometry_2d.pt_2D array -> float + -> (Geometry_2d.pt_2Dfloat * float * float) *) +(** [wind wind_init speeds precision] returns the wind and air speed mean and std dev. *) + +let wind wind_init speeds precision = + let mean wind = + let sum = + Array.fold_left (fun acc speed -> acc +. vect_norm (vect_sub speed wind)) 0. speeds in + sum /. float (Array.length speeds) in + + let nb_calls = ref 0 in + let cost wind = + incr nb_calls; + let m = mean wind in + let sum = + Array.fold_left + (fun acc speed -> + let err = vect_norm (vect_sub speed wind) -. m in + acc +. err *. err) + 0. speeds in + sum /. float (Array.length speeds) in + + let step = 2. and max_iter = 100 in + let wind = simplex wind_init cost step max_iter precision in + if debug then Printf.printf "nb calls: %d\n" !nb_calls; + + (wind.p, mean wind.p, wind.f) + + + +let _ = + let options = + [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf "Bus\tDefault is %s" !ivy_bus)] in + Arg.parse (options) + (fun x -> Printf.fprintf stderr "Warning: Don't do anything with %s\n" x) + "Usage: "; + + let precision = 1e-3 in + let speeds = ref [] and wind_init = ref null_vector in + + let on_wind_command _ args = + if Str.string_match (Str.regexp "clear") args.(0) 0 then begin + speeds := []; + wind_init := null_vector + end + in + + let on_wind_clear _ args = speeds := []; wind_init := null_vector in + + let on_flight_param = fun _ args -> +(* Array.iter (printf "%s ") args; printf "\n%!"; *) + let r = float_of_string args.(4) + and theta = heading_of_to_angle_rad (deg2rad (float_of_string args.(5))) in + let speed = polar2cart {r2D = r; theta2D = theta} in + speeds := speed :: !speeds in + + let on_wind_req _ args = + let speeds = Array.of_list !speeds in + if Array.length speeds >= 3 then begin + let (wind, mean, stddev) = wind !wind_init speeds precision in + wind_init := wind; + let wind_polar = cart2polar wind in + let wind_cap_deg = rad2deg (wind_dir_from_angle_rad wind_polar.theta2D) in + Ivy.send + (sprintf "ground WIND_RES %s %f %f %f %f" args.(0) wind_cap_deg wind_polar.r2D mean stddev) + end in + + let on_wind_iso _ args = + let speeds = Array.of_list !speeds in + if Array.length speeds >= 3 then begin + let (wind, mean, stddev) = isotropic_wind !wind_init speeds precision in + wind_init := wind; + let wind_polar = cart2polar wind in + let wind_cap_deg = rad2deg (wind_dir_from_angle_rad wind_polar.theta2D) in + Ivy.send + (sprintf "ground WIND_RES %s %f %f %f %f" args.(0) wind_cap_deg wind_polar.r2D mean stddev) + end in + + let on_aircrafts = fun _ args -> + let aclist = args.(0) in + let first_ac = aclist (*String.sub aclist 0 (String.index aclist ',')*) in + ignore + (Ivy.bind on_flight_param + (sprintf "%s +FLIGHT_PARAM (.*) (.*) (.*) (.*) (.*) (.*) (.*) (.*)" first_ac)) in + + Ivy.init "Paparazzi Wind" "READY" (fun _ _ -> ()); + ignore (Ivy.bind on_aircrafts "ground AIRCRAFTS (.*)"); + ignore (Ivy.bind on_wind_req "WIND_REQ (.*)"); + ignore (Ivy.bind on_wind_iso "WIND_ISO (.*)"); + ignore (Ivy.bind on_wind_command "WIND_COMMAND (.*)"); + Ivy.start !ivy_bus; + + GMain.Main.main () diff --git a/sw/ground_segment/wind/wind.mli b/sw/ground_segment/wind/wind.mli new file mode 100644 index 0000000000..992137c2cb --- /dev/null +++ b/sw/ground_segment/wind/wind.mli @@ -0,0 +1,3 @@ +val wind : Geometry_2d.pt_2D -> Geometry_2d.pt_2D array -> float -> + (Geometry_2d.pt_2D * float * float) +(** [wind wind_init speeds precision] returns the wind, air speed mean and std dev *) diff --git a/sw/include/std.h b/sw/include/std.h new file mode 100644 index 0000000000..6103fdebd2 --- /dev/null +++ b/sw/include/std.h @@ -0,0 +1,38 @@ +#ifndef STD_H +#define STD_H +/* + * $Id$ + * + * Copyright (C) 2005 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. + * + * + * a couple of fundamentals used in the avr code + * + */ + +#include + +#define FALSE 0 +#define TRUE (!FALSE) + +/* Boolean values */ +typedef uint8_t bool_t; + +#endif /* STD_H */ diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile new file mode 100644 index 0000000000..b0f0a83865 --- /dev/null +++ b/sw/lib/ocaml/Makefile @@ -0,0 +1,94 @@ +# +# $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. +# + +INCLUDES= -I +lablgl -I +camlimages -I +lablgtk2 +OCAMLC=ocamlc -g $(INCLUDES) +OCAMLOPT=ocamlopt $(INCLUDES) + + +SRC = debug.ml env.ml serial.ml ocaml_tools.ml extXml.ml xml2h.ml latlong.ml srtm.ml wavecard.ml geometry_2d.ml geometry_3d.ml cserial.o convert.o ubx.ml pprz.ml +CMO = $(SRC:.ml=.cmo) +CMX = $(SRC:.ml=.cmx) + +XSRC = platform.ml gtkgl_Hack.ml ml_gtkgl_hack.o gtk_image.ml gtk_tools_icons.ml gtk_tools.ml gtk_draw.ml gtk_tools_GL.ml gtk_3d.ml mapCanvas.ml mapWaypoints.ml mapTrack.ml +XCMO = $(XSRC:.ml=.cmo) +XCMX = $(XSRC:.ml=.cmx) + + +all : lib.cma lib.cmxa xlib.cma xlib.cmxa xml_get.out + + +lib.cma : $(CMO) + ocamlmklib -custom -o lib str.cma xml-light.cma unix.cma $^ + +lib.cmxa : $(CMX) + ocamlmklib -custom -o lib $^ + +xlib.cma : $(XCMO) + ocamlmklib -custom -o xlib $^ + +xlib.cmxa : $(XCMX) + ocamlmklib -custom -o xlib $^ + +xml_get.out : lib.cma xml_get.cmo + $(OCAMLC) -o $@ str.cma xml-light.cma -I . $^ + +ignutm.opt : latlong.cmx ignutm.ml + $(OCAMLOPT) -o $@ -I +camlimages ci_core.cmxa ci_png.cmxa xml-light.cmxa $^ + +utm_of.opt : latlong.cmx utm_of.ml + $(OCAMLOPT) -o $@ $^ + +GTKCFLAGS := $(shell gtk-config --cflags) + +%.o : %.c + $(OCAMLC) -c $< + +ml_gtkgl_hack.o : ml_gtkgl_hack.c + $(OCAMLC) -c -ccopt "$(GTKCFLAGS)" $< + +%.cmo : %.ml + $(OCAMLC) -c $< + +%.cmx : %.ml + $(OCAMLOPT) -c $< + +%.cmi : %.mli + $(OCAMLC) $< + +%.cmi : %.ml + $(OCAMLC) $< + +clean : + rm -f *~ *.cm* *.out *.opt .depend *.a *.o *.so + + +# +# Dependencies +# + +.depend: + ocamldep *.ml* > .depend + +ifneq ($(MAKECMDGOALS),clean) +-include .depend +endif diff --git a/sw/lib/ocaml/convert.c b/sw/lib/ocaml/convert.c new file mode 100644 index 0000000000..27ba511548 --- /dev/null +++ b/sw/lib/ocaml/convert.c @@ -0,0 +1,46 @@ +/* + $Id$ + + Copyright (C) 2004 Pascal Brisset, Antoine Drouin + + Ocaml low level conversions + + 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. +*/ + +#include +#include +#include +#include +#include +#include "caml/mlvalues.h" +#include "caml/alloc.h" + +value c_float_of_indexed_bytes(value s, value index) +{ + float *x = (float*)(String_val(s) + Int_val(index)); + + return copy_double((double)(*x)); +} + +value c_int32_of_indexed_bytes(value s, value index) +{ + int32_t *x = (int32_t*)(String_val(s) + Int_val(index)); + + return copy_int32(*x); +} diff --git a/sw/lib/ocaml/cserial.c b/sw/lib/ocaml/cserial.c new file mode 100644 index 0000000000..91816f8ede --- /dev/null +++ b/sw/lib/ocaml/cserial.c @@ -0,0 +1,76 @@ +/* + $Id$ + Copyright (C) 2004 Pascal Brisset, Antoine Drouin + + Ocaml bindings for handling serial ports + + 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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static int baudrates[] = { B0, B50, B75, B110, B134, B150, B200, B300, B600, B1200, B1800, B2400, B4800, B9600, B19200, B38400, B57600, B115200, B230400 }; + + +/****************************************************************************/ +/* Open serial device for requested protocoll */ +/****************************************************************************/ +value c_init_serial(value device, value speed) +{ + struct termios orig_termios, cur_termios; + + int br = baudrates[Int_val(speed)]; + + int fd = open(String_val(device), O_RDWR); + + if (fd == -1) failwith("opening modem serial device : fd < 0"); + + if (tcgetattr(fd, &orig_termios)) failwith("getting modem serial device attr"); + cur_termios = orig_termios; + + /* input modes */ + cur_termios.c_iflag &= ~(IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK|ISTRIP|INLCR|IGNCR + |ICRNL |IXON|IXANY|IXOFF|IMAXBEL); + /* pas IGNCR sinon il vire les 0x0D */ + cur_termios.c_iflag |= BRKINT; + + /* output_flags */ + cur_termios.c_oflag &=~(OPOST|ONLCR|OCRNL|ONOCR|ONLRET); + + /* control modes */ + cur_termios.c_cflag &= ~(CSIZE|CSTOPB|CREAD|PARENB|PARODD|HUPCL|CLOCAL|CRTSCTS); + cur_termios.c_cflag |= CREAD|CS8|CLOCAL; + + /* local modes */ + cur_termios.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN); + cur_termios.c_lflag |= NOFLSH; + + if (cfsetispeed(&cur_termios, br)) failwith("setting modem serial device speed"); + + if (tcsetattr(fd, TCSADRAIN, &cur_termios)) failwith("setting modem serial device attr"); + + return Val_int(fd); +} diff --git a/sw/lib/ocaml/debug.ml b/sw/lib/ocaml/debug.ml new file mode 100644 index 0000000000..8a0e1524e3 --- /dev/null +++ b/sw/lib/ocaml/debug.ml @@ -0,0 +1,46 @@ + (* + * $Id$ + * + * Debugging facilities + * + * Copyright (C) 2004 CENA/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. + * + *) + +let level = ref (try Sys.getenv "PPRZ_DEBUG" with Not_found -> "") + let log = ref stderr + let call lev f = + assert( (* assert permet au compilo de tout virer avec l'option -noassert *) + if (String.contains !level '*' || String.contains !level lev) + then begin + f !log; + flush !log + end; + true) + +let xprint = fun s -> + let n = String.length s in + let a = String.make (3*n) ' ' in + for i = 0 to n - 1 do + let x = Printf.sprintf "%02x" (Char.code s.[i]) in + a.[3*i] <- x.[0]; + a.[3*i+1] <- x.[1] + done; + a diff --git a/sw/lib/ocaml/env.ml b/sw/lib/ocaml/env.ml new file mode 100644 index 0000000000..3fd89a95b7 --- /dev/null +++ b/sw/lib/ocaml/env.ml @@ -0,0 +1,37 @@ +(* + * $Id$ + * + * Configuration handling + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec, 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. + * + *) + +let paparazzi_src = + try + Sys.getenv "PAPARAZZI_SRC" + with + _ -> "/usr/share/paparazzi" + +let paparazzi_home = + try + Sys.getenv "PAPARAZZI_HOME" + with + _ -> Filename.concat (Sys.getenv "HOME") "paparazzi" diff --git a/sw/lib/ocaml/extXml.ml b/sw/lib/ocaml/extXml.ml new file mode 100644 index 0000000000..f449f34219 --- /dev/null +++ b/sw/lib/ocaml/extXml.ml @@ -0,0 +1,87 @@ +(* + * $Id$ + * + * Xml-Light extension + * + * Copyright (C) 2004 CENA/ENAC, Pascal Brisset + * + * 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. + * + *) + +exception Error of string + +let sep = Str.regexp "\\." + +let child xml ?select c = + let rec find = function + Xml.Element (tag, attributes, _children) as elt :: elts -> + if tag = c then + match select with + None -> elt + | Some p -> + if p elt then elt else find elts + else + find elts + | _ :: elts -> find elts + | [] -> raise Not_found in + + + let children = Xml.children xml in + + (* Let's try with a numeric index *) + try (Array.of_list children).(int_of_string c) with + Failure "int_of_string" -> (* Bad luck. Go through the children *) + find children + + +let get xml path = + let p = Str.split sep path in + let rec iter xml = function + [] -> failwith "ExtXml.get: empty path" + | [x] -> ( try if Xml.tag xml <> x then raise Not_found else xml with _ -> raise Not_found ) + | x::xs -> iter (child xml x) xs in + iter xml p + +let get_attrib xml path attr = + Xml.attrib (get xml path) attr + +let sprint_fields = fun () l -> + "<"^ + List.fold_right (fun (a, b) -> (^) (Printf.sprintf "%s=\"%s\" " a b)) l ">" + +let attrib = fun x a -> + try + Xml.attrib x a + with + Xml.No_attribute _ -> + raise (Error (Printf.sprintf "Error: Attribute '%s' expected in <%a>" a sprint_fields (Xml.attribs x))) + +let attrib_or_default = fun x a default -> + try Xml.attrib x a with _ -> default + + +let to_string_fmt = fun xml -> + let l = String.lowercase in + let rec lower = function + Xml.PCData _ as x -> x + | Xml.Element (t, ats, cs) -> + Xml.Element(l t, + List.map (fun (a,v) -> (l a, v)) ats, + List.map lower cs) in + Xml.to_string_fmt (lower xml) diff --git a/sw/lib/ocaml/geometry_2d.ml b/sw/lib/ocaml/geometry_2d.ml new file mode 100644 index 0000000000..9f96f18bc0 --- /dev/null +++ b/sw/lib/ocaml/geometry_2d.ml @@ -0,0 +1,920 @@ +(* + * $Id$ + * + * 2D Geometry + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(* Distance d'un point a une droite, a un segment, a un ensemble de segment *) +(* Projection d'un point sur une droite, segment, ensemble de segment *) + +(* Modules locaux *) + +let epsilon = 0.0001 + +(* Type contenant un point 2D *) +type pt_2D = {x2D : float; y2D : float} + +(* Vecteurs nuls en 2D *) +let null_vector = {x2D=0.; y2D=0.} + +(* Polygone 2D, non ferme par defaut *) +type poly_2D = pt_2D list + +(* Types d'intersection : *) +(* T_IN_SEGx : point d'intersection dans le segment x (extremites exclues) *) +(* T_ON_PTx : intersection sur le point x *) +(* T_OUT_SEG_PTx : intersection hors d'un segment. Le point d'intersection se *) +(* situe du cote du point x *) +type t_crossing = T_IN_SEG1 | T_IN_SEG2 | T_ON_PT1 | T_ON_PT2 | T_ON_PT3 +| T_ON_PT4 | T_OUT_SEG_PT1 | T_OUT_SEG_PT2 | T_OUT_SEG_PT3 | T_OUT_SEG_PT4 + +(* Type de polygone : convexe, concave ou indefini *) +type t_conv = CONVEX | CONCAVE | CONV_UNDEFINED + +(* Sens d'un polygone : sens horaire, anti-horaire et indefini *) +type t_ccw = CW | CCW | CCW_UNDEFINED + +(* Carre *) +let cc x = x*.x + +(* Type des points utilises pour la triangulation *) +type vertex = {pos : pt_2D; + num : int ; + mutable prev : int ; + mutable next : int ; + mutable ear : bool} + +(* ============================================================================= *) +(* = Manipulations d'angles = *) +(* ============================================================================= *) +let m_pi = 3.1415926535897932384626433832795 +let deg2rad d = (d*.m_pi)/.180. +let rad2deg d = (d*.180.)/.m_pi + +(* ============================================================================= *) +(* = Comparaison de points/vecteurs = *) +(* ============================================================================= *) +let point_same pt1 pt2 = (pt1.x2D = pt2.x2D) && (pt1.y2D = pt2.y2D) + +(* ============================================================================= *) +(* = Creation d'un vecteur = *) +(* ============================================================================= *) +let vect_make pt1 pt2 = {x2D=pt2.x2D -. pt1.x2D; y2D=pt2.y2D -. pt1.y2D} + +(* ============================================================================= *) +(* = Norme d'un vecteur = *) +(* ============================================================================= *) +let vect_norm v = sqrt((cc v.x2D) +. (cc v.y2D)) + +(* ============================================================================= *) +(* = Normalisation d'un vecteur = *) +(* ============================================================================= *) +let vect_normalize v = let n = vect_norm v in {x2D=v.x2D/.n; y2D=v.y2D/.n} + +(* ============================================================================= *) +(* = Force la norme d'un vecteur = *) +(* ============================================================================= *) +let vect_set_norm v norme = + let n = norme /. (vect_norm v) in {x2D=v.x2D*.n; y2D=v.y2D*.n} + +(* ============================================================================= *) +(* = Distance entre deux points = *) +(* ============================================================================= *) +let distance pt1 pt2 = vect_norm (vect_make pt1 pt2) + +(* ============================================================================= *) +(* = Rotation d'un vecteur d'un angle alpha en radians = *) +(* ============================================================================= *) +let vect_rotate_rad v alpha = + let c = cos alpha and s = sin alpha in + {x2D= v.x2D *. c -. v.y2D *. s; y2D= v.x2D *. s +. v.y2D *. c} + +(* ============================================================================= *) +(* = Rotation d'un vecteur d'un angle alpha en degres = *) +(* ============================================================================= *) +let vect_rotate v alpha = vect_rotate_rad v (deg2rad alpha) + +(* ============================================================================= *) +(* = Creation d'un vecteur normal au vecteur v (rotation de 90 degres positive)= *) +(* ============================================================================= *) +let vect_rotate_90 v = {x2D= -.v.y2D; y2D=v.x2D} + +(* ============================================================================= *) +(* = Ajoute deux vecteurs (ou d'un point et d'un vecteur) = *) +(* ============================================================================= *) +let vect_add u v = {x2D=u.x2D+.v.x2D; y2D=u.y2D+.v.y2D} + +(* ============================================================================= *) +(* = Soustraction de deux vecteurs (ou d'un point et d'un vecteur) = *) +(* ============================================================================= *) +let vect_sub u v = {x2D=u.x2D-.v.x2D; y2D=u.y2D-.v.y2D} + +(* ============================================================================= *) +(* = Multiplication d'un vecteur par un flottant = *) +(* ============================================================================= *) +let vect_mul_scal v m = {x2D=m*.v.x2D; y2D=m*.v.y2D} + +(* ============================================================================= *) +(* = Operation B=lamba.v+A = *) +(* ============================================================================= *) +let vect_add_mul_scal lambda a v = vect_add a (vect_mul_scal v lambda) + +(* ============================================================================= *) +(* = Vecteur oppose = *) +(* ============================================================================= *) +let vect_inverse v = vect_mul_scal v (-1.) + +(* ============================================================================= *) +(* = Milieu d'un segment = *) +(* ============================================================================= *) +let point_middle p1 p2 = {x2D=(p1.x2D+.p2.x2D)/.2.; y2D= (p1.y2D+.p2.y2D)/.2.} + +(* ============================================================================= *) +(* = Barycentre d'une liste de points avec ou sans coefficients = *) +(* ============================================================================= *) +let barycenter lst_pts = + let v = List.fold_left (fun p pt -> vect_add p pt) null_vector lst_pts in + vect_mul_scal v (1.0/.(float_of_int (List.length lst_pts))) + +let weighted_barycenter lst_pts lst_coeffs = + let (v, somme_coeffs) = + List.fold_left2 (fun (p, s) pt c -> (vect_add_mul_scal c p pt, s+.c)) + (null_vector, 0.0) lst_pts lst_coeffs in + vect_mul_scal v (1.0/.somme_coeffs) + +(* ============================================================================= *) +(* = Produit scalaire = *) +(* ============================================================================= *) +let dot_product u v = u.x2D*.v.x2D +. u.y2D*.v.y2D + +(* ============================================================================= *) +(* = Produit vectoriel = *) +(* ============================================================================= *) +let cross_product u v = u.x2D*.v.y2D -. u.y2D*.v.x2D + + +(* ============================================================================= *) +(* = = *) +(* = Projections = *) +(* = = *) +(* ============================================================================= *) + +(* ============================================================================= *) +(* = Projection d'un point pt sur une droite (a, u) = *) +(* ============================================================================= *) +let point_project_on_line pt a u = + let v = vect_make a pt in + let n = vect_norm u in + let lambda = ((dot_product u v)/.(cc n)) in + vect_add_mul_scal lambda a u + +(* ============================================================================= *) +(* = Projection d'un point sur un segment, si possible = *) +(* ============================================================================= *) +let point_project_on_segment pt a b = + let v = vect_make a pt and u = vect_make a b in + let n = vect_norm u in + let lambda = ((dot_product u v)/.(cc n)) in + if lambda>=0. && lambda <=1. then Some (vect_add_mul_scal lambda a u) + else None + +(* ============================================================================= *) +(* = Projection d'un point sur un ensemble de segments, si possible = *) +(* ============================================================================= *) +let point_project_on_segments_list pt lst_points = + let proj = ref None and dist = ref 0. in + let rec f l = + match l with + a::b::reste -> + (match point_project_on_segment pt a b with + None -> () + | Some p -> + (* Le point se projete sur le segment ab, on teste si la distance *) + (* de pt au segment ab est inferieure a la distance courante, si *) + (* oui alors le point p est le point recherche *) + let d = distance p pt in + (match !proj with + None -> dist:= d; proj:=Some p + | Some _ -> if d < !dist then begin dist:= d; proj:=Some p end + )) ; + f (b::reste) + | _ -> !proj + in + f lst_points + + +(* ============================================================================= *) +(* = = *) +(* = Distances = *) +(* = = *) +(* ============================================================================= *) + + +(* ============================================================================= *) +(* = Distance d'un point a une droite = *) +(* ============================================================================= *) +let distance_point_line pt a u = + let v = vect_make pt a in abs_float ((cross_product u v)/.(vect_norm u)) + +(* ============================================================================= *) +(* = Distance d'un point a un ensemble de segments = *) +(* ============================================================================= *) +let distance_point_segments_list pt lst_points = + match point_project_on_segments_list pt lst_points with + None -> None + | Some p -> Some (distance p pt) + + +(* ============================================================================= *) +(* = = *) +(* = Intersections = *) +(* = = *) +(* ============================================================================= *) + + +(* ============================================================================= *) +(* = Routine globale d'intersection = *) +(* = a = point + u = vecteur directeur de la premiere droite = *) +(* = b = point + v = vecteur directeur de la seconde droite = *) +(* ============================================================================= *) +let crossing_point a u c v = + let x = vect_make c a in + let num1 = cross_product x v and num2 = cross_product x u + and denom = -.(cross_product u v) in + + if denom = 0. then + (* Les deux vecteurs sont paralleles *) + None + else begin + let r = num1 /. denom and s = num2 /. denom in + let type_intersection_seg1 = + if abs_float r < epsilon then T_ON_PT1 + else if abs_float (r-.1.0) < epsilon then T_ON_PT2 + else if r<0.0 then T_OUT_SEG_PT1 + else if r>1.0 then T_OUT_SEG_PT2 + else T_IN_SEG1 + + and type_intersection_seg2 = + if abs_float s < epsilon then T_ON_PT3 + else if abs_float (s-.1.0) < epsilon then T_ON_PT4 + else if s<0.0 then T_OUT_SEG_PT3 + else if s>1.0 then T_OUT_SEG_PT4 + else T_IN_SEG2 + + and pt_intersection = vect_add_mul_scal r a u in + + Some (type_intersection_seg1, type_intersection_seg2, pt_intersection) + end + +(* ============================================================================= *) +(* = Test du type d'intersection = *) +(* ============================================================================= *) +let test_in_segment t = + (t=T_IN_SEG1)||(t=T_ON_PT1)||(t=T_ON_PT2)|| + (t=T_IN_SEG2)||(t=T_ON_PT3)||(t=T_ON_PT4) +let test_on_hl t = (test_in_segment t)||(t=T_OUT_SEG_PT4)||(t=T_OUT_SEG_PT2) + +(* ============================================================================= *) +(* = Teste l'intersection de deux segments (a,b) et (c,d) = *) +(* ============================================================================= *) +let crossing_seg_seg a b c d = + match crossing_point a (vect_make a b) c (vect_make c d) with + None -> false + | Some (type1, type2, pt) -> (test_in_segment type1)&&(test_in_segment type2) + +(* ============================================================================= *) +(* = Teste l'intersection d'un segment (a,b) et d'une demi-droite (c,v) = *) +(* ============================================================================= *) +let crossing_seg_hl a b c v = + match crossing_point a (vect_make a b) c v with + None -> false + | Some (type1, type2, pt) -> + (* OK si intersection sur la demi-droite *) + (test_in_segment type1) && (test_on_hl type2) + +(* ============================================================================= *) +(* = Teste l'intersection de deux demi-droites = *) +(* ============================================================================= *) +let crossing_hl_hl a u c v = + let inter = crossing_point a u c v in + match inter with + None -> false + | Some (type1, type2, pt) -> (test_on_hl type1) && (test_on_hl type2) + +(* ============================================================================= *) +(* = Teste l'intersection de deux droites et renvoie le point s'il existe = *) +(* ============================================================================= *) +let crossing_lines a u c v = + match crossing_point a u c v with + None -> (false, null_vector) + | Some (type1, type2, pt) -> (true, pt) + + +(* ============================================================================= *) +(* = = *) +(* = Polygones = *) +(* = par defaut ils sont consideres comme ouverts = *) +(* = = *) +(* ============================================================================= *) + + +(* ============================================================================= *) +(* = Teste si un polygone est ferme = *) +(* ============================================================================= *) +let poly_is_closed poly = + if poly=[] then false else point_same (List.hd poly) (List.hd (List.rev poly)) + +(* ============================================================================= *) +(* = Ferme un polygone [A; B; C; D] -> [A; B; C; D; A] = *) +(* ============================================================================= *) +let poly_close poly = + if poly = [] or poly_is_closed poly then poly else poly@[List.hd poly] + +(* ============================================================================= *) +(* = Ferme un polygone [A; B; C; D] -> [|A; B; C; D; A; B|] = *) +(* ============================================================================= *) +let poly_close2 poly = + if List.length poly < 2 then Array.of_list poly else begin + let poly = if poly_is_closed poly then poly else poly@[List.hd poly] in + Array.of_list (poly@[List.hd (List.tl poly)]) + end + +(* ============================================================================= *) +(* = Indique si un point est dans un polygone = *) +(* ============================================================================= *) +let point_in_poly pt poly = + let p = Array.of_list poly in + + let do_func {x2D=xi; y2D=yi} {x2D=xj; y2D=yj} {x2D=x; y2D=y} c = + if (((yi<=y) && (y do_func p0 p.(!j) pt is_in ; j := i) p ; + + (* Resultat *) + !is_in + +(* ============================================================================= *) +(* = Indique si un point est dans un cercle = *) +(* ============================================================================= *) +let point_in_circle pt (center, r) = distance pt center <= r + +(* ============================================================================= *) +(* = Calcul de l'enveloppe convexe d'un polygone = *) +(* ============================================================================= *) +let convex_hull poly = + let det a b = + match cross_product a b with 0.0 -> 0.0 | n when n>0.0 -> 1.0 | _ -> -1.0 + in + + let du_meme_cote a b c d = + let u = vect_make a b and v = vect_make a c and w = vect_make a d in + (det u v)*.(det u w)>0.0 + in + + let plus_proche a b c d = + (d=a) or ((not(c=a)) & + ((du_meme_cote b c d a) or ( + let u = vect_make b c and v = vect_make b d in + (det u v)=0.0 & (abs_float(u.x2D)+.abs_float(u.y2D)> + abs_float(v.x2D)+.abs_float(v.y2D))))) + in + + let extract_mini p l = + let rec aux reste vu mini = + match reste with + t::q -> + if (try(p mini t) with _ -> false) then aux q (t::vu) mini + else aux q (mini::vu) t + | [] -> mini,vu + in match l with + t::q -> aux q [] t + | [] -> raise Exit + in + + let f (x,y) = {x2D=x;y2D=y} in + let p a b = a.x2Db.y2D) in + let l2=poly in + let debut,_=extract_mini p l2 in + let rec itere a o liste sol = + let p = plus_proche a o in + let u,v=extract_mini p liste in + if (u=debut) then (List.rev((u::sol))) else (itere o u v (u::sol)) + in + itere {x2D=debut.x2D+.1.0;y2D=debut.y2D} debut l2 [debut] + +(* ============================================================================= *) +(* = Intersection d'un segment et d'un polygone (non ferme) = *) +(* ============================================================================= *) +let crossing_seg_poly a b poly = + (* Supprime les doublons dans une liste triee *) + let supprime_doublons_points l = + let (p, new_l) = List.fold_left (fun (old, lst) pt -> + match old with + None -> (Some pt, [pt]) + | Some p -> if point_same p pt then (old, lst) else (Some pt, pt :: lst) + ) (None, []) l in + List.rev new_l + in + + let u = vect_make a b and pol = Array.of_list poly + and lst_pts_inter = ref [] in + + for i = 0 to (Array.length pol-1) do + let c = pol.(i) and + (* Rappel : le polygone n'est pas ferme... *) + d = if i < (Array.length pol) -1 then pol.(i+1) else pol.(0) in + let inter = crossing_point a u c (vect_make c d) in + match inter with + None -> () (* Pas d'intersection entre le segment et l'arrete *) + | Some (type1, type2, pt) -> + if (test_in_segment type1) && (test_in_segment type2) then + (* L'intersection est bien sur les 2 segments *) + lst_pts_inter := pt :: !lst_pts_inter + done ; + + (* Suppression des doublons dans la liste des points d'intersection *) + (* Il y a des doublons si intersection sur un sommet du polygone *) + supprime_doublons_points !lst_pts_inter + +(* ============================================================================= *) +(* = Intersection sans prendre en compte les sommets = *) +(* ============================================================================= *) +let crossing_seg_poly_exclusive a b poly = + let u = vect_make a b and + pol = Array.of_list poly and + lst_pts_inter = ref [] in + + for i = 0 to (Array.length pol-1) do + let c = pol.(i) and + (* Rappel : le polygone n'est pas ferme... *) + d = if i < (Array.length pol) -1 then pol.(i+1) else pol.(0) in + let inter = crossing_point a u c (vect_make c d) in + match inter with + None -> () (* Pas d'intersection entre le segment et l'arrete *) + | Some (type1, type2, pt) -> + if (type1=T_IN_SEG1) && (type2=T_IN_SEG2) then + (* L'intersection est bien sur les 2 segments *) + lst_pts_inter := pt :: !lst_pts_inter ; + done ; + + !lst_pts_inter + +(* ============================================================================= *) +(* = Cercle circonscrit a un triangle = *) +(* ============================================================================= *) +let circumcircle {x2D=x1; y2D=y1} {x2D=x2; y2D=y2} {x2D=x3; y2D=y3} = + (* Determinants de matrices 3x3 *) + let eval_det a1 a2 a3 b1 b2 b3 c1 c2 c3 = + a1*.b2*.c3-.a1*.b3*.c2-.a2*.b1*.c3+.a2*.b3*.c1+.a3*.b1*.c2-.a3*.b2*.c1 in + let eval_det1 a1 a2 b1 b2 c1 c2 = eval_det a1 a2 1. b1 b2 1. c1 c2 1. in + + let a = eval_det1 x1 y1 x2 y2 x3 y3 in + let s1 = (cc x1)+.(cc y1) and s2 = (cc x2)+.(cc y2) and s3 = (cc x3)+.(cc y3) in + let bx = eval_det1 s1 y1 s2 y2 s3 y3 in + let by = -.(eval_det1 s1 x1 s2 x2 s3 x3) in + let c = -.(eval_det s1 x1 y1 s2 x2 y2 s3 x3 y3) in + let a = 2.*.a in + let xc = bx/.a and yc = by/.a in + let r = abs_float ((sqrt(bx*.bx+.by*.by-.2.*.a*.c))/.a) in + + (* Position du centre et rayon *) + ({x2D=xc; y2D=yc}, r) + +(* ============================================================================= *) +(* = Test sens horaire ou inverse = *) +(* ============================================================================= *) +let ccw_angle p0 p1 p2 = + let p = cross_product (vect_make p0 p1) (vect_make p1 p2) in + if p > 0. then CCW else if p < 0. then CW else CCW_UNDEFINED + +(* ============================================================================= *) +(* = Teste si un polygone est concave ou convexe = *) +(* = Il est convexe si toutes les arretes consecutives sont dans la meme sens = *) +(* ============================================================================= *) +let poly_test_convex l = + if List.length l > 2 then begin + (* l = [A; B; C; D] -> t = [|A; B; C; D; A; B|] *) + let t = poly_close2 l in + let n = Array.length t in + let sign = ccw_angle t.(0) t.(1) t.(2) and i = ref 1 in + while !i 2 then begin + let t = Array.of_list (poly_close l) in + if poly_test_convex l = CONVEX then ccw_angle t.(0) t.(1) t.(2) + else begin + let s = ref 0. in + for i = 0 to (Array.length t-2) do + s:= !s+.cross_product t.(i) t.(i+1) + done ; + if !s>0. then CCW else if !s<0. then CW else CCW_UNDEFINED + end + end else CCW_UNDEFINED + +(* ============================================================================= *) +(* = Surface d'un polygone (signee) = *) +(* ============================================================================= *) +let poly_signed_area poly = + (* On peut le faire avec des produits vectoriels mais la facon suivante est *) + (* plus efficace et plus precise *) + + if List.length poly < 2 then 0. else begin + let poly = poly_close2 poly in + let n = Array.length poly -2 and area = ref 0. in + for i = 1 to n do + area:=!area+.poly.(i).x2D*.(poly.(i+1).y2D-.poly.(i-1).y2D) + done ; + !area/.2. + end + +(* ============================================================================= *) +(* = Surface d'un polygone (non signee) = *) +(* ============================================================================= *) +let poly_area poly = abs_float (poly_signed_area poly) + +(* ============================================================================= *) +(* = Centroide d'un polygone = *) +(* ============================================================================= *) +let poly_centroid poly = + (* On peut trianguler et ponderer le centre de chaque triangle par sa *) + (* surface mais on peut faire plus efficace. Ici, on prend un point du *) + (* polygone (le premier par ex.) et on pondere l'aire (signee) des *) + (* triangles construits a partir de ce point. *) + + (* Centroide d'un triangle *) + let centroid_triangle p1 p2 p3 = + {x2D=(p1.x2D+.p2.x2D+.p3.x2D)/.3.; y2D=(p1.y2D+.p2.y2D+.p3.y2D)/.3.} in + + (* Aire signee d'un triangle, pas besoin de poly_area... *) + let area_triangle p1 p2 p3 = + (cross_product (vect_make p1 p2) (vect_make p1 p3))/.2. + in + + let rec f p0 l centroid = + match l with + p1::p2::reste -> + let new_centroid = vect_add_mul_scal (area_triangle p0 p1 p2) centroid + (centroid_triangle p0 p1 p2) in + f p0 (p2::reste) new_centroid + | _ -> + let area = poly_signed_area poly in + vect_mul_scal centroid (1./.area) + in + + match poly with + [] -> null_vector + | p::[] -> p + | p1::p2::[] -> point_middle p1 p2 + | _ -> f (List.hd poly) (List.tl poly) null_vector + +(* ============================================================================= *) +(* = = *) +(* = Triangulation de polygones = *) +(* = par defaut ils sont consideres comme ouverts = *) +(* = = *) +(* ============================================================================= *) + + +(* ============================================================================= *) +(* = Triangulation d'un polygone. Vielle version incorrecte dans certains cas = *) +(* ============================================================================= *) +let in_tesselation_old l0 = + (* Recherche des extremes et du centre *) + let {x2D=x; y2D=y} = List.hd l0 in + let xmin = ref x and xmax = ref x and ymin = ref y and ymax = ref y in + List.iter (fun {x2D=x; y2D=y} -> + if x< !xmin then xmin:=x; if x> !xmax then xmax:=x; + if y< !ymin then ymin:=y; if y> !ymax then ymax:=y) l0 ; + let dmax = max (!xmax -. !xmin) (!ymax -. !ymin) in + let pmid = point_middle {x2D= !xmin; y2D= !ymin} {x2D= !xmax; y2D= !ymax} in + + (* Recherche du triangle englobant (supertriangle) *) + let n = List.length l0 in + let t = Array.of_list (l0@[{x2D=pmid.x2D-.2.*.dmax; y2D=pmid.y2D-.dmax} ; + {x2D=pmid.x2D; y2D=pmid.y2D+.2.*.dmax} ; + {x2D=pmid.x2D+.2.*.dmax; y2D=pmid.y2D-.dmax}]) in + let triangles = ref [(n, n+1, n+2)] in + + (* Tous les points du contour sont inseres les uns apres les autres *) + Array.iteri (fun i point -> + let edges = ref [] in + + triangles := List.fold_left (fun l (p1, p2, p3) -> + (* Cercle circonscrit au triangle *) + let circle = circumcircle t.(p1) t.(p2) t.(p3) in + if point_in_circle point circle then begin + (* Ajout de 3 arretes et suppression du triangle en cours *) + edges := (p3,p1)::(p2,p3)::(p1,p2)::!edges ; l + end else (p1, p2, p3)::l) [] !triangles ; + + (* Creation de nouveaux triangles a partir du point courant pour les *) + (* arretes non multiples ou qui apparaissent un nombre impair de fois *) + let ledges = ref !edges in + List.iter (fun (n1, n2) -> + let l = List.find_all (fun (n01, n02) -> + (n01=n1&&n02=n2) or (n01=n2&&n02=n1)) !ledges in + if List.length l mod 2 <> 0 then begin + triangles:=(n1, n2, i)::!triangles; + (* Si l'arrete apparait un nombre impair de fois > 1 alors *) + (* on n'insere que ce triangle et pas les suivants, sinon *) + (* certains triangles apparaissent plusieurs fois *) + if List.length l>=3 then ledges:=!ledges@[(n1, n2)] + end) !edges) t ; + + let triangle_ok (p1, p2, p3) = + let check p1 p2 = + if p1-p2=1 or p2-p1=1 or (p1=0&&p2=n-1) or (p1=n-1&&p2=0) then true + else point_in_poly (point_middle t.(p1) t.(p2)) l0 + in + check p1 p2 && check p2 p3 && check p3 p1 + in + + (* Les triangles ayant des points du supertriangle sont elimines ainsi *) + (* que tous les triangles se trouvant a l'exterieur du contour initial *) + (* car ce cas arrive lorsque le contour original est concave... *) + let l = List.fold_left (fun l (p1, p2, p3) -> + if p1>=n or p2>=n or p3>=n or not (triangle_ok (p1, p2, p3)) then l + else (p1, p2, p3)::l) [] !triangles in + + (* Renvoie la liste des triangles CW *) + let l = List.map (fun (p1, p2, p3) -> + if ccw_angle t.(p1) t.(p2) t.(p3) = CW then (p1, p2, p3) else (p1, p3, p2)) l in + + (* Tableau des points et liste des triangles *) + (* Normalement, si n points differents au depart -> n-2 triangles en sortie *) + if List.length l0<>(List.length l)+2 then begin + Printf.printf "AAAA %d points %d triangles\n" (List.length l0) (List.length l); + flush stdout + end ; + (Array.of_list l0, l) + +(* ============================================================================= *) +(* = Triangulation d'un polygone = *) +(* ============================================================================= *) +let in_tesselation poly = + (* On teste si le polygone est bien CCW, s'il ne l'est pas on l'inverse *) + let (switched, l)= + match poly_test_ccw poly with + CW -> (true, List.rev poly) + | _ -> (false, poly) + in + + (* Creation du tableau des points sous la forme necessaire a la triangulation *) + let vertices = + let t = Array.of_list l and n = List.length l and vertices = ref [] in + Array.iteri (fun i pt -> + vertices:={pos=pt; num=i; ear=false; + prev=if i=0 then n-1 else i-1; + next=if i=n-1 then 0 else i+1}::!vertices) t ; + Array.of_list (List.rev !vertices) + in + + (* Fonction testant si les points d'indices n1 et n2 forment une diagonale *) + (* completement contenue dans le polygone *) + let is_diagonal n1 n2 = + let lefton a b c = cross_product (vect_make a b) (vect_make a c) >= 0. in + let left a b c = cross_product (vect_make a b) (vect_make a c) > 0. in + + let is_in_cone a b = + let a1=vertices.(a.next) and a0=vertices.(a.prev) in + + (* Point A convexe ? *) + if lefton a.pos a1.pos a0.pos then + (left a.pos b.pos a0.pos) && (left b.pos a.pos a1.pos) + else not ((lefton a.pos b.pos a1.pos) && (lefton b.pos a.pos a0.pos)) + in + + let a = vertices.(n1) and b = vertices.(n2) in + +(* AAA if is_in_cone a b && is_in_cone b a then begin *) + if is_in_cone a b or is_in_cone b a then begin + let rec f l = + match l with + c::reste -> + let c1 = vertices.(c.next) in + if c.num<>a.num && c1.num<>a.num && c.num<>b.num && c1.num<>b.num && + crossing_seg_seg a.pos b.pos c.pos c1.pos then false + else f reste + | [] -> true + in + f (Array.to_list vertices) + end else false + in + + (* Initialisation des oreilles *) + Array.iter (fun v1 -> v1.ear <- is_diagonal v1.prev v1.next) vertices ; + + (* Triangulation *) + let current_idx = ref 0 and earfound = ref false and lst_triangles = ref [] + and n = ref (Array.length vertices) in + while !n>=3 do + earfound:=false ; + let v2 = ref !current_idx and finished = ref false in + while not !finished && not !earfound do + if vertices.(!v2).ear then begin + (* Le point courant correspond a une oreille, on va le supprimer *) + earfound:=true ; + + (* 5 points consecutifs, v2 est au 'milieu' des 5 *) + let v1=vertices.(!v2).prev and v3=vertices.(!v2).next in + let v0=vertices.(v1).prev and v4=vertices.(v3).next in + + (* Sauvegarde du triangle. Pas sous la forme v1, v2, v3 sinon *) + (* il n'est pas CCW et donc pb de normale exterieure a l'affichage *) + lst_triangles := (v3, !v2, v1)::!lst_triangles ; + + (* Mise a jour des oreilles *) + vertices.(v1).ear <- is_diagonal v0 v3 ; + vertices.(v3).ear <- is_diagonal v1 v4 ; + + (* Suppression du point v2 *) + vertices.(v1).next <- v3 ; + vertices.(v3).prev <- v1 ; + current_idx:=v3 ; + + (* Un triangle de moins a chercher *) + decr n + end else v2:=vertices.(!v2).next ; + + (* C'est fini quand on revient sur le point initial *) + finished:= !v2 = !current_idx + done ; + done ; + + if not !earfound then begin Printf.printf "No ear !\n"; flush stdout end ; + + (* Si le polygone etait CW au depart, il a ete retourne et les numeros *) + (* des points ne sont alors pas dans le meme sens que le polygone passe *) + (* par l'utilisateur. On remet alors les numeros comme ils etaients lors *) + (* de l'appel a la fonction de triangulation *) + if switched then begin + let n = List.length poly in + lst_triangles:=List.map (fun (p1, p2, p3) -> (n-1-p1, n-1-p2, n-1-p3) + ) !lst_triangles + end ; + + !lst_triangles + +(* ============================================================================= *) +(* = Triangulation d'un polygone = *) +(* ============================================================================= *) +let tesselation l = + let t = Array.of_list l in + List.map (fun (p1, p2, p3) -> [t.(p1); t.(p2); t.(p3)]) (in_tesselation l) + +(* ============================================================================= *) +(* = Recherche des triangles fan dans une liste de triangles = *) +(* ============================================================================= *) +let in_tesselation_fans l = + let t = Array.of_list l in + let l = in_tesselation l in + let tt = Array.mapi (fun i x -> (i, 0)) t in + let add_val x = let (p, n) = tt.(x) in tt.(x) <- (p, n+1) in + List.iter (fun (p1, p2, p3) -> + add_val p1; add_val p2; add_val p3) l ; + let lst = List.fast_sort (fun (_, n1) (_, n2) -> n2-n1) (Array.to_list tt) in + + let tt2 = Array.create (Array.length tt) (0, []) in + let i = ref 0 in + List.iter (fun (x, _) -> tt2.(x) <- (!i, []); incr i) lst ; + List.iter (fun (p1, p2, p3) -> + let (t1, l1) = tt2.(p1) and (t2, l2) = tt2.(p2) and (t3, l3) = tt2.(p3) in + if t1 + let l0 = ref [] in + let add_element (a, b) = + let rec f deb fin = + match fin with + [] -> (a, b, [a; b])::!l0 + | (c, d, lst)::reste -> + if b=c then begin + (* Insertion avant *) + (List.rev ((a, d, a::lst)::deb))@reste + end else if a=d then begin + (* Insertion apres *) + (List.rev ((c, b, lst@[b])::deb))@reste + end else f ((c, d, lst)::deb) reste + in + l0:=f [] !l0 + in + let merge_lists () = + let rec in_merge (a, b, l1) ll0 ll = + match ll with + (c, d, l2)::reste -> + if b=c then + (true, ((a, d, l1@(List.tl l2))::ll0)@reste) + else if d=a then + (true, ((c, b, l2@(List.tl l1))::ll0)@reste) + else in_merge (a, b, l1) ((c, d, l2)::ll0) reste + | [] -> (false, ll0) + in + let rec f l ll = + match l with + l1::reste -> + let (merged, newl) = in_merge l1 [] reste in + if merged then f newl ll + else f reste (l1::ll) + | [] -> ll + in + l0:=f !l0 [] + in + + if l<>[] then begin + List.iter (fun x -> add_element x; merge_lists ()) l ; + List.iter (fun (_, _, l) -> + lst_fans := (i::l)::!lst_fans) !l0 + end) tt2 ; + + (t, !lst_fans) + +(* ============================================================================= *) +(* = Triangulation en triangles_fan = *) +(* ============================================================================= *) +(* effectue la triangulation du polygone en + triangle_fan OpenGL. En sortie est renvoyee une liste contenant des listes + de points. Chacune de ces listes de points contient soit 3 points (triangle) + soit plus de 3 points (pour un triangle_fan) + *) + +let tesselation_fans l = + let (t, l) = in_tesselation_fans l in + List.map (fun l -> List.map (fun x -> t.(x)) l) l + + + + + + +type pt_2D_polar = { r2D : float; theta2D : float; } + +let cart2polar p = {r2D = vect_norm p; theta2D = atan2 p.y2D p.x2D} + +let polar2cart p = {x2D = p.r2D *. cos p.theta2D; y2D = p.r2D *. sin p.theta2D} + + +(* grosses conneries d'avions *) + +let two_m_pi = 2. *. m_pi +let m_pi_two = m_pi /. 2. + +let wind_dir_from_angle_rad rad = + let w = ref (3. *. m_pi_two -. rad) in + while !w > two_m_pi do + w := !w -. two_m_pi done; + !w + +let heading_of_to_angle_rad angle = + let a = ref (5. *. m_pi_two -. angle) in + while !a >= two_m_pi do a := !a -. two_m_pi done; + !a + +let norm_angle_rad a = + let a = ref a in + while !a < -. m_pi do a := !a +. two_m_pi done; + while !a > m_pi do a := !a -. two_m_pi done; + !a + +let norm_heading_rad a = + let a = ref a in + while !a < 0. do a := !a +. two_m_pi done; + while !a > two_m_pi do a := !a -. two_m_pi done; + !a + +let oposite_heading_rad rad = + norm_heading_rad (rad +. m_pi) + + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/geometry_2d.mli b/sw/lib/ocaml/geometry_2d.mli new file mode 100644 index 0000000000..cbce16edb4 --- /dev/null +++ b/sw/lib/ocaml/geometry_2d.mli @@ -0,0 +1,303 @@ +(* + * $Id$ + * + * 2D Geometry + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(** Module de géométrie 2D + + Par défaut, les polygones sont considérés comme étant ouverts + + {e Yann Le Fablec, version 1.0, 17/04/2003} + *) + +(** {6 Types} *) + +(** Type point/vecteur 2D en coordonnees cartesiennes *) +type pt_2D = { x2D : float; y2D : float; } + +(** Type point/vecteur 2D en coordonnees polaires *) +type pt_2D_polar = { r2D : float; theta2D : float; } + +(** Vecteur nul en 2D *) +val null_vector : pt_2D + +(** Un polygone *) +type poly_2D = pt_2D list + +(** Types d'intersections pour le croisement entre deux segments + [\[P1, P2\]] et [\[P3, P4\]] *) +type t_crossing = + T_IN_SEG1 (** Dans le segment 1 *) + | T_IN_SEG2 (** Dans le segment 2 *) + | T_ON_PT1 (** Sur le premier point du segment 1 *) + | T_ON_PT2 (** Sur le second point du segment 1 *) + | T_ON_PT3 (** Sur le premier point du segment 1 *) + | T_ON_PT4 (** Sur le second point du segment 2 *) + | T_OUT_SEG_PT1 (** En dehors du segment 1, avant le premier point *) + | T_OUT_SEG_PT2 (** En dehors du segment 1, après le second point *) + | T_OUT_SEG_PT3 (** En dehors du segment 2, avant le premier point *) + | T_OUT_SEG_PT4 (** En dehors du segment 2, après le second point *) + +(** indique le type d'un polygone *) +type t_conv = CONVEX | CONCAVE | CONV_UNDEFINED + +(** indique le sens d'un polygone (horaire ou contre-horaire) *) +type t_ccw = CW | CCW | CCW_UNDEFINED + +(** {6 Conversions d'angles} *) + +(** [deg2rad angle_degres] donne l'angle correpondant en radians *) +val deg2rad : float -> float + +(** [rad2deg angle_radians] donne l'angle correpondant en degrés *) +val rad2deg : float -> float + +(** {6 Points} *) + +(** [point_same A B] teste l'égalité stricte des deux points *) +val point_same : pt_2D -> pt_2D -> bool + +(** [distance A B] évalue la distance entre les points [A] et [B] *) +val distance : pt_2D -> pt_2D -> float + +(** [point_middle A B] renvoie le milieu du segment formé par [A] et [B] *) +val point_middle : pt_2D -> pt_2D -> pt_2D + +(** [barycenter lst_pts] renvoie le barycentre des points *) +val barycenter : pt_2D list -> pt_2D + +(** [weighted_barycenter lst_pts lst_poids] renvoie le barycentre des points + pondérés par [lst_poids] *) +val weighted_barycenter : pt_2D list -> float list -> pt_2D + +(** {6 Tests d'inclusion} *) + +(** [point_in_poly pt poly] teste si le point se trouve dans le polygone *) +val point_in_poly : pt_2D -> poly_2D -> bool + +(** [point_in_circle pt (centre_cercle, rayon_cercle)] teste si le point + se trouve dans le cercle indiqué *) +val point_in_circle : pt_2D -> pt_2D * float -> bool + +(** {6 Vecteurs} *) + +(** [vect_make A B] crée le vecteur AB *) +val vect_make : pt_2D -> pt_2D -> pt_2D + +(** [vect_norm v] renvoie la norme du vecteur *) +val vect_norm : pt_2D -> float + +(** [vect_normalize v] normalise le vecteur *) +val vect_normalize : pt_2D -> pt_2D + +(** [vect_set_norm v norme] change [v] pour que sa norme soit [norme] *) +val vect_set_norm : pt_2D -> float -> pt_2D + +(** [vect_add u v] réalise la somme des deux vecteurs*) +val vect_add : pt_2D -> pt_2D -> pt_2D + +(** [vect_sub u v] renvoie la soustraction de [v] à [u] *) +val vect_sub : pt_2D -> pt_2D -> pt_2D + +(** [vect_mul_scal v scalaire] multiple le vecteur par un scalaire *) +val vect_mul_scal : pt_2D -> float -> pt_2D + +(** [vect_add_mul_scal lambda p v] renvoie le point [p] translaté du + vecteur [lambda.v] *) +val vect_add_mul_scal : float -> pt_2D -> pt_2D -> pt_2D + +(** [vect_rotate_rad v angle] tourne le vecteur de l'angle indiqué en radians *) +val vect_rotate_rad : pt_2D -> float -> pt_2D + +(** [vect_rotate v angle] tourne le vecteur de l'angle indiqué en degrés *) +val vect_rotate : pt_2D -> float -> pt_2D + +(** [vect_rotate_90 v] renvoie le vecteur normal à [v] (rotation de 90 degrés + dans le sens trigonométrique) *) +val vect_rotate_90 : pt_2D -> pt_2D + +(** [vect_inverse v] renvoie le vecteur opposé à [v] *) +val vect_inverse : pt_2D -> pt_2D + +(** {6 Produits} *) + +(** [dot_product u v] fournit le produit scalaire des deux vecteurs *) +val dot_product : pt_2D -> pt_2D -> float + +(** [cross_product u v] renvoie le produit vectoriel de [u] et [v] *) +val cross_product : pt_2D -> pt_2D -> float + +(** {6 Intersections de segments/droites} *) + +(** [crossing_point A u B v] teste l'intersection de deux droites : la première + passant par le point [A] et de vecteur directeur [u] et la seconde passant par [B] + et de vecteur directeur [v]. + + En sortie, deux possibilités : + - [None] s'il n'y a pas d'intersection + - [Some (type1, type2, point_intersection)] sinon. [type1] désigne le type + d'intersection sur la première droite et [type2] la meme information pour + la seconde droite. + *) +val crossing_point : + pt_2D -> + pt_2D -> pt_2D -> pt_2D -> (t_crossing * t_crossing * pt_2D) option + +(** [test_in_segment type_intersection] teste si l'intersection est dans le + segment 1 (extrémités incluses) *) +val test_in_segment : t_crossing -> bool + +(** [test_on_hl type_intersection] teste si l'intersection est sur la demi-droite + (extrémité incluse) *) +val test_on_hl : t_crossing -> bool + +(** [crossing_seg_seg A B C D] teste l'intersection des segments + [\[A,B\]] et [\[C,D\]] *) +val crossing_seg_seg : pt_2D -> pt_2D -> pt_2D -> pt_2D -> bool + +(** [crossing_seg_hl A B C u] teste l'intersection entre le segment [\[A,B\]] et + la droite passant par [C] et de vecteur directeur [u] *) +val crossing_seg_hl : + pt_2D -> pt_2D -> pt_2D -> pt_2D -> bool + +(** [crossing_hl_hl A u B v] teste l'intersection entre les deux demi-droites *) +val crossing_hl_hl : pt_2D -> pt_2D -> pt_2D -> pt_2D -> bool + +(** [crossing_lines A u B v] teste l'intersection entre les deux droites et renvoie le + point s'il y a effectivement intersection *) +val crossing_lines : pt_2D -> pt_2D -> pt_2D -> pt_2D -> bool * pt_2D + +(** {6 Distances} *) + +(** [distance_point_line P A u] renvoie la distance entre le point [P] et + la droite passant par [A] de vecteur directeur [u] *) +val distance_point_line : pt_2D -> pt_2D -> pt_2D -> float + +(** [distance_point_segments_list P lst_points] renvoie la distance mini entre [P] + et les segments formés par la liste de points [lst_points]. La distance peut + ne pas etre définie (auquel cas None est renvoyé) *) +val distance_point_segments_list : pt_2D -> poly_2D -> float option + +(** {6 Projections} *) + +(** [point_project_on_line P A u] renvoie la projection du point [P] sur + la droite passant par [A] de vecteur directeur [u] *) +val point_project_on_line : pt_2D -> pt_2D -> pt_2D -> pt_2D + +(** [point_project_on_segment P A B] renvoie la projection, si elle existe, + du point [P] sur le segment [\[A, B\]] *) +val point_project_on_segment : pt_2D -> pt_2D -> pt_2D -> pt_2D option + +(** [point_project_on_segments_list P lst_points] renvoie la projection, si elle existe, + de [P] sur les segments formés par la liste de points [lst_points] *) +val point_project_on_segments_list : pt_2D -> poly_2D -> pt_2D option + +(** {6 Polygones} *) + +(** [poly_is_closed poly] renvoie [TRUE] si le polygone est fermé *) +val poly_is_closed : poly_2D -> bool + +(** [poly_close poly] ferme le polygone s'il ne l'est pas deja. Ainsi + [\[A; B; C; D\]] devient [\[A; B; C; D; A\]] *) +val poly_close : poly_2D -> poly_2D + +(** [poly_close2 poly] effectue l'opération suivante : + [\[A; B; C; D\]] -> [\[|A; B; C; D; A; B|\]]. + Attention, ici le polygone retourné n'est pas une liste de points + mais un tableau de points *) +val poly_close2 : poly_2D -> pt_2D array + +(** [poly_area poly] renvoie l'aire du polygone *) +val poly_area : poly_2D -> float + +(** [poly_signed_area poly] renvoie l'aire signée du polygone *) +val poly_signed_area : poly_2D -> float + +(** [poly_centroid poly] renvoie le centroide du polygone *) +val poly_centroid : poly_2D -> pt_2D + +(** [ccw_angle A B C] indique le sens de l'angle formé par les vecteurs AB et BC *) +val ccw_angle : pt_2D -> pt_2D -> pt_2D -> t_ccw + +(** [poly_test_ccw poly] renvoie le sens horaire ou contre-horaire du polygone *) +val poly_test_ccw : poly_2D -> t_ccw + +(** [poly_test_convex poly] indique le type (convexe ou pas) du polygone *) +val poly_test_convex : poly_2D -> t_conv + +(** [convex_hull poly] fournit l'enveloppe convexe de la liste de points [poly] *) +val convex_hull : poly_2D -> pt_2D list + +(** [circumcircle A B C] renvoie le cercle circonscrit au triangle ABC sous la + forme [(C, r)] où [C] désigne le centre du cercle et [r] son rayon *) +val circumcircle : pt_2D -> pt_2D -> pt_2D -> pt_2D * float + +(** [crossing_seg_poly A B poly] renvoie la liste des intersections entre + le segment [\[AB\]] et le polygone. Les intersections peuvent se trouver + en [A], en [B] ou sur des sommets du polygone *) +val crossing_seg_poly : pt_2D -> pt_2D -> poly_2D -> pt_2D list + +(** [crossing_seg_poly_exclusive A B poly] meme chose que précédemment sauf + que [A], [B] et les sommets du polygones sont exclus *) +val crossing_seg_poly_exclusive : pt_2D -> pt_2D -> poly_2D -> pt_2D list + +(** {6 Triangulation de polygones} *) + +(** [in_tesselation poly] effectue la triangulation et renvoie une liste de + triplets indiquant les indices des points constituant les triangles *) +val in_tesselation : poly_2D -> (int * int * int) list + +(** [geom_tesselation polygone] effectue la triangulation du polygone et renvoie + la liste des triangles résultants. Ici un triangle est une liste de 3 points (!). + De plus, le polygone ne doit contenir aucun point en double. + *) +val tesselation : poly_2D -> poly_2D list + +(** [in_tesselation_fans poly] effectue la triangulation (en fans) et renvoie le tableau + contenant les points et une liste de triplets indiquant les indices des points + (dans le tableau) constituant les triangles_fan *) +val in_tesselation_fans : poly_2D -> pt_2D array * (int list) list + +(** [geom_tesselation_fans polygone] effectue la triangulation du polygone en + triangle_fan OpenGL. En sortie est renvoyée une liste contenant des listes + de points. Chacune de ces listes de points contient soit 3 points (triangle) + soit plus de 3 points (pour un triangle_fan). + De plus, le polygone ne doit contenir aucun point en double. + *) +val tesselation_fans : poly_2D -> poly_2D list + + +(** {6 Coordonnees polaires} *) + +val m_pi : float + +(** [cart2polar cart] *) +val cart2polar : pt_2D -> pt_2D_polar +val polar2cart : pt_2D_polar -> pt_2D + +val wind_dir_from_angle_rad : float -> float +val heading_of_to_angle_rad : float -> float +val norm_angle_rad : float -> float +val norm_heading_rad : float -> float +val oposite_heading_rad : float -> float diff --git a/sw/lib/ocaml/geometry_3d.ml b/sw/lib/ocaml/geometry_3d.ml new file mode 100644 index 0000000000..6867fe79e5 --- /dev/null +++ b/sw/lib/ocaml/geometry_3d.ml @@ -0,0 +1,610 @@ +(* + * $Id$ + * + * 3D Geometry + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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 Geometry_2d + +(* Type contenant un point 2D *) +type pt_3D = {x3D : float; y3D : float; z3D : float} + +(* Polygone 3D, non ferme par defaut *) +type poly_3D = pt_3D list + +(* Volume 3D *) +type volume_3D = poly_3D list + +(* Vecteurs nuls en 2D *) +let null_vector = {x3D=0.; y3D=0.; z3D=0.} + +(* Carre *) +let cc x = x*.x + +let epsilon = 0.0001 +type t_crossing3d = T_IN_SEG1 | T_IN_SEG2 | T_ON_PT1 | T_ON_PT2 | T_ON_PT3 +| T_ON_PT4 | T_OUT_SEG_PT1 | T_OUT_SEG_PT2 | T_OUT_SEG_PT3 | T_OUT_SEG_PT4 + +(* Type pour les differents axes *) +type t_axis3d = T_X3D | T_Y3D | T_Z3D + +(* ============================================================================= *) +(* = Determinant d'une matrice 3x3 = *) +(* ============================================================================= *) +let eval_det3 a1 a2 a3 b1 b2 b3 c1 c2 c3 = + a1*.b2*.c3-.a1*.b3*.c2-.a2*.b1*.c3+.a2*.b3*.c1+.a3*.b1*.c2-.a3*.b2*.c1 + +(* ============================================================================= *) +(* = Determinant d'une matrice 4x4 (par developpement en matrice 3x3 = *) +(* ============================================================================= *) +let eval_det4 a1 a2 a3 a4 b1 b2 b3 b4 c1 c2 c3 c4 d1 d2 d3 d4 = + let det1 = eval_det3 b2 b3 b4 c2 c3 c4 d2 d3 d4 + and det2 = eval_det3 b1 b3 b4 c1 c3 c4 d1 d3 d4 + and det3 = eval_det3 b1 b2 b4 c1 c2 c4 d1 d2 d4 + and det4 = eval_det3 b1 b2 b3 c1 c2 c3 d1 d2 d3 in + + a1*.det1-.a2*.det2+.a3*.det3-.a4*.det4 + +(* ============================================================================= *) +(* = Comparaison de points/vecteurs = *) +(* ============================================================================= *) +let point_same pt1 pt2 = + (pt1.x3D = pt2.x3D) && (pt1.y3D = pt2.y3D) && (pt1.z3D = pt2.z3D) + +(* ============================================================================= *) +(* = Creation d'un vecteur = *) +(* ============================================================================= *) +let vect_make pt1 pt2 = {x3D=pt2.x3D -. pt1.x3D; y3D=pt2.y3D -. pt1.y3D; + z3D=pt2.z3D -. pt1.z3D} + +(* ============================================================================= *) +(* = Norme d'un vecteur = *) +(* ============================================================================= *) +let vect_norm v = sqrt((cc v.x3D) +. (cc v.y3D) +. (cc v.z3D)) + +(* ============================================================================= *) +(* = Normalisation d'un vecteur = *) +(* ============================================================================= *) +let vect_normalize v = + let n = vect_norm v in {x3D=v.x3D/.n; y3D=v.y3D/.n; z3D=v.z3D/.n} + +(* ============================================================================= *) +(* = Force la norme d'un vecteur = *) +(* ============================================================================= *) +let vect_set_norm v norme = + let n = norme /. (vect_norm v) in {x3D=v.x3D*.n; y3D=v.y3D*.n; z3D=v.z3D*.n} + +(* ============================================================================= *) +(* = Distance entre deux points = *) +(* ============================================================================= *) +let distance pt1 pt2 = vect_norm (vect_make pt1 pt2) + +(* ============================================================================= *) +(* = Ajoute deux vecteurs (ou d'un point et d'un vecteur) = *) +(* ============================================================================= *) +let vect_add u v = {x3D=u.x3D+.v.x3D; y3D=u.y3D+.v.y3D; z3D=u.z3D+.v.z3D} + +(* ============================================================================= *) +(* = Soustraction de deux vecteurs (ou d'un point et d'un vecteur) = *) +(* ============================================================================= *) +let vect_sub u v = {x3D=u.x3D-.v.x3D; y3D=u.y3D-.v.y3D; z3D=u.z3D-.v.z3D} + +(* ============================================================================= *) +(* = Multiplication d'un vecteur par un flottant = *) +(* ============================================================================= *) +let vect_mul_scal v m = {x3D=m*.v.x3D; y3D=m*.v.y3D; z3D=m*.v.z3D} + +(* ============================================================================= *) +(* = Operation B=lamba.v+A = *) +(* ============================================================================= *) +let vect_add_mul_scal lambda a v = vect_add a (vect_mul_scal v lambda) + +(* ============================================================================= *) +(* = Vecteur oppose = *) +(* ============================================================================= *) +let vect_inverse v = vect_mul_scal v (-1.) + +(* ============================================================================= *) +(* = Milieu d'un segment = *) +(* ============================================================================= *) +let point_middle p1 p2 = {x3D=(p1.x3D+.p2.x3D)/.2.; y3D=(p1.y3D+.p2.y3D)/.2.; + z3D=(p1.z3D+.p2.z3D)/.2.} + +(* ============================================================================= *) +(* = Barycentre d'une liste de points avec ou sans coefficients = *) +(* ============================================================================= *) +let barycenter lst_pts = + let v = List.fold_left (fun p pt -> vect_add p pt) null_vector lst_pts in + vect_mul_scal v (1.0/.(float_of_int (List.length lst_pts))) + +let weighted_barycenter lst_pts lst_coeffs = + let (v, somme_coeffs) = + List.fold_left2 (fun (p, s) pt c -> (vect_add_mul_scal c p pt, s+.c)) + (null_vector, 0.0) lst_pts lst_coeffs in + vect_mul_scal v (1.0/.somme_coeffs) + +(* ============================================================================= *) +(* = Produit scalaire = *) +(* ============================================================================= *) +let dot_product u v = u.x3D*.v.x3D +. u.y3D*.v.y3D +. u.z3D*.v.z3D + +(* ============================================================================= *) +(* = Produit vectoriel = *) +(* ============================================================================= *) +let cross_product u v = {x3D=u.y3D*.v.z3D -. u.z3D*.v.y3D; + y3D=u.z3D*.v.x3D -. u.x3D*.v.z3D; + z3D=u.x3D*.v.y3D -. u.y3D*.v.x3D} + +(* ============================================================================= *) +(* = Normale unitaire a un deux vecteurs = *) +(* ============================================================================= *) +let normal u v = vect_normalize (cross_product u v) + +(* ============================================================================= *) +(* = Test d'un point sur un segment = *) +(* ============================================================================= *) +let point_on_segment p p1 p2 = + (* P est sur le segment P1 P2 si le produit vectoriel P1P^P1P2 *) + (* est nul (P sur la droite P1 P2) et que le produit scalaire *) + (* P1P.P1P2 est compris entre 0 et la norme de P1P2 au carre *) + let v = cross_product (vect_make p1 p) (vect_make p1 p2) in + let scal = dot_product (vect_make p1 p) (vect_make p1 p2) + and n = distance p1 p2 in + v=null_vector && scal>=0. && scal<=cc n + + +(* ============================================================================= *) +(* = = *) +(* = Intersections = *) +(* = = *) +(* ============================================================================= *) + + +(* ============================================================================= *) +(* = Intersection de deux droites 3D = *) +(* ============================================================================= *) +let crossing_point a u c v = + let w = vect_make a c in + let n = cross_product u v in + let n2 = vect_norm n in let n2 = cc n2 in + + (* Si s<>0 alors les vecteurs ne sont pas coplanaires *) + let s = dot_product n w in + + (* Si n est nul alors les vecteurs sont paralleles *) + if n2 = 0.0 or s <> 0. then None else begin + let r = (dot_product (cross_product w v) n)/.n2 + and s = (dot_product (cross_product w u) n)/.n2 in + let type_intersection_seg1 = + if abs_float r < epsilon then T_ON_PT1 + else if abs_float (r-.1.0) < epsilon then T_ON_PT2 + else if r<0.0 then T_OUT_SEG_PT1 + else if r>1.0 then T_OUT_SEG_PT2 + else T_IN_SEG1 + + and type_intersection_seg2 = + if abs_float s < epsilon then T_ON_PT3 + else if abs_float (s-.1.0) < epsilon then T_ON_PT4 + else if s<0.0 then T_OUT_SEG_PT3 + else if s>1.0 then T_OUT_SEG_PT4 + else T_IN_SEG2 + + and pt_intersection = vect_add_mul_scal r a u in + + Some (type_intersection_seg1, type_intersection_seg2, pt_intersection) + end + +(* ============================================================================= *) +(* = Test du type d'intersection = *) +(* ============================================================================= *) +let test_in_segment t = + (t=T_IN_SEG1)||(t=T_ON_PT1)||(t=T_ON_PT2)|| + (t=T_IN_SEG2)||(t=T_ON_PT3)||(t=T_ON_PT4) +let test_on_hl t = (test_in_segment t)||(t=T_OUT_SEG_PT4)||(t=T_OUT_SEG_PT2) + +(* ============================================================================= *) +(* = Teste l'intersection de deux segments (a,b) et (c,d) = *) +(* ============================================================================= *) +let crossing_seg_seg a b c d = + match crossing_point a (vect_make a b) c (vect_make c d) with + None -> false + | Some (type1, type2, pt) -> (test_in_segment type1)&&(test_in_segment type2) + +(* ============================================================================= *) +(* = Teste l'intersection d'un segment (a,b) et d'une demi-droite (c,v) = *) +(* ============================================================================= *) +let crossing_seg_hl a b c v = + match crossing_point a (vect_make a b) c v with + None -> false + | Some (type1, type2, pt) -> + (* OK si intersection sur la demi-droite *) + (test_in_segment type1) && (test_on_hl type2) + +(* ============================================================================= *) +(* = Teste l'intersection de deux demi-droites = *) +(* ============================================================================= *) +let crossing_hl_hl a u c v = + let inter = crossing_point a u c v in + match inter with + None -> false + | Some (type1, type2, pt) -> (test_on_hl type1) && (test_on_hl type2) + +(* ============================================================================= *) +(* = Teste l'intersection de deux droites et renvoie le point s'il existe = *) +(* ============================================================================= *) +let crossing_lines a u c v = + match crossing_point a u c v with + None -> (false, null_vector) + | Some (type1, type2, pt) -> (true, pt) + +(* ============================================================================= *) +(* = Intersection d'une droite (a, u) et d'un plan (c, d, e) = *) +(* ============================================================================= *) +let crossing_line_plane a u c d e = + let num = eval_det4 + 1. 1. 1. 1. + c.x3D d.x3D e.x3D a.x3D + c.y3D d.y3D e.y3D a.y3D + c.z3D d.z3D e.z3D a.z3D + and denom = eval_det4 + 1. 1. 1. 0. + c.x3D d.x3D e.x3D u.x3D + c.y3D d.y3D e.y3D u.y3D + c.z3D d.z3D e.z3D u.z3D + in + if denom=0. then None + else Some (vect_add_mul_scal (num/.denom) a u) + +(* ============================================================================= *) +(* = Intersection d'une demi-droite (a, u) et d'un plan (c, d, e) = *) +(* ============================================================================= *) +let crossing_hline_plane a u c d e = + let num = eval_det4 + 1. 1. 1. 1. + c.x3D d.x3D e.x3D a.x3D + c.y3D d.y3D e.y3D a.y3D + c.z3D d.z3D e.z3D a.z3D + and denom = eval_det4 + 1. 1. 1. 0. + c.x3D d.x3D e.x3D u.x3D + c.y3D d.y3D e.y3D u.y3D + c.z3D d.z3D e.z3D u.z3D + in + + if denom=0. then None else begin + let s = (-.num)/.denom in + if s >= 0. then Some (vect_add_mul_scal s a u) + else None + end + + +(* ============================================================================= *) +(* = = *) +(* = Polygones = *) +(* = = *) +(* = Ils sont consideres comme etant ouverts et plans = *) +(* = = *) +(* ============================================================================= *) + + +(* ============================================================================= *) +(* = Teste si un polygone est ferme = *) +(* ============================================================================= *) +let poly_is_closed poly = + if poly=[] then false else point_same (List.hd poly) (List.hd (List.rev poly)) + +(* ============================================================================= *) +(* = Ferme un polygone [A; B; C; D] -> [A; B; C; D; A] = *) +(* ============================================================================= *) +let poly_close poly = + if poly = [] or poly_is_closed poly then poly else poly@[List.hd poly] + +(* ============================================================================= *) +(* = Ferme un polygone [A; B; C; D] -> [|A; B; C; D; A; B|] = *) +(* ============================================================================= *) +let poly_close2 poly = + if List.length poly < 2 then Array.of_list poly else begin + let poly = if poly_is_closed poly then poly else poly@[List.hd poly] in + Array.of_list (poly@[List.hd (List.tl poly)]) + end + +(* ============================================================================= *) +(* = Transformation d'un point 3D en 2D en supprimant la coordonnee indiquee = *) +(* ============================================================================= *) +let pt_3d_to_pt_2d axis pt = + match axis with + T_X3D -> {x2D=pt.y3D; y2D=pt.z3D} + | T_Y3D -> {x2D=pt.x3D; y2D=pt.z3D} + | T_Z3D -> {x2D=pt.x3D; y2D=pt.y3D} + +(* ============================================================================= *) +(* = Transformation d'un polygone 3D en polygone 2D en supprimant une coord = *) +(* ============================================================================= *) +let poly_3d_to_poly_2d axis poly = List.map (pt_3d_to_pt_2d axis) poly + +(* ============================================================================= *) +(* = Test d'un point dans un plan defini par 3 points = *) +(* ============================================================================= *) +let point_in_plane pt a b c = + let det = eval_det4 + pt.x3D pt.y3D pt.z3D 1. + a.x3D a.y3D a.z3D 1. + b.x3D b.y3D b.z3D 1. + c.x3D c.y3D c.z3D 1. + in + abs_float det < epsilon + +(* ============================================================================= *) +(* = Determination des etendues du polygone sur chaque axe pour choisir le = *) +(* = de projection -> limite les pbs d'instabilite numerique = *) +(* ============================================================================= *) +let span_poly poly = + let set_min_max valeur min max = + if valeur < !min then min:=valeur else if valeur > !max then max:=valeur + in + + let p = List.hd (poly) in + let minx = ref p.x3D and maxx = ref p.y3D and miny = ref p.y3D + and maxy = ref p.y3D and minz = ref p.z3D and maxz = ref p.z3D in + List.iter (fun p -> + set_min_max p.x3D minx maxx ; + set_min_max p.y3D miny maxy ; + set_min_max p.z3D minz maxz) (List.tl poly) ; + (* Renvoie les etendues sur chaque axe *) + (!maxx-. !minx, !maxy-. !miny, !maxz-. !minz) + +(* ============================================================================= *) +(* = Projection d'un polygone 3D sur le plan ayant les plus grandes etendues = *) +(* ============================================================================= *) +let poly_3d_to_poly_2d_smallest_span poly = + let (dx, dy, dz) = span_poly poly in + let axis = + if dx= 3 then begin + (* Simplification du polygone *) + let (new_poly, axis) = poly_3d_to_poly_2d_smallest_span poly in + (* Simplification du point a tester suivant le meme axe *) + let new_pt = pt_3d_to_pt_2d axis pt in + (* Utilisation de la fonction 2D pour tester l'inclusion *) + Geometry_2d.point_in_poly new_pt new_poly + end else false + +(* ============================================================================= *) +(* = Normale unitaire au plan contenant un polygone = *) +(* ============================================================================= *) +let poly_normal poly = + if List.length poly >= 3 then begin + (* 3 points du polygone qui definissent le plan le contenant *) + let a = List.hd poly and b = List.hd (List.tl poly) + and c = List.hd (List.tl (List.tl poly)) in + (* Normale unitaire au plan contenant le polygone *) + normal (vect_make a b) (vect_make b c) + end else null_vector + +(* ============================================================================= *) +(* = Test d'un point dans un polygone 3D = *) +(* ============================================================================= *) +let point_in_poly pt poly = + if List.length poly >= 3 then begin + (* 3 points du polygone qui definissent le plan le contenant *) + let a = List.hd poly and b = List.hd (List.tl poly) + and c = List.hd (List.tl (List.tl poly)) in + + (* Le point est-il dans le plan contenant le polygone ? *) + if point_in_plane pt a b c then + (* Oui, on teste alors en projetant en 2D *) + point_in_poly_2D pt poly + else false + end else false + +(* ============================================================================= *) +(* = Aire signee d'un polygone 3D = *) +(* ============================================================================= *) +let poly_signed_area poly = + if List.length poly >= 3 then begin + let poly_closed = poly_close2 poly and vect = ref null_vector in + for i = 0 to (List.length poly)-1 do + vect := vect_add !vect (cross_product poly_closed.(i) poly_closed.(i+1)) + done ; + + (dot_product (poly_normal poly) !vect)/.2. + end else 0. + +(* ============================================================================= *) +(* = Aire d'un polygone 3D = *) +(* ============================================================================= *) +let poly_area poly = abs_float (poly_signed_area poly) + +(* ============================================================================= *) +(* = Evaluation du centroide d'un polygone 3D = *) +(* ============================================================================= *) +let poly_centroid poly = + (* On peut trianguler et ponderer le centre de chaque triangle par sa *) + (* surface mais on peut faire plus efficace. Ici, on prend un point du *) + (* polygone (le premier par ex.) et on pondere l'aire (signee) des *) + (* triangles construits a partir de ce point. *) + + (* Centroide d'un triangle *) + let centroid_triangle p1 p2 p3 = + {x3D=(p1.x3D+.p2.x3D+.p3.x3D)/.3.; + y3D=(p1.y3D+.p2.y3D+.p3.y3D)/.3.; + z3D=(p1.z3D+.p2.z3D+.p3.z3D)/.3.} in + + (* Normale au plan contenant le polygone *) + let n = poly_normal poly in + + (* Aire signee d'un triangle, pas besoin de poly_area... *) + let area_triangle p1 p2 p3 = + (dot_product n (cross_product (vect_make p1 p2) (vect_make p1 p3)))/.2. in + + let rec f p0 l centroid = + match l with + p1::p2::reste -> + let new_centroid = vect_add_mul_scal (area_triangle p0 p1 p2) centroid + (centroid_triangle p0 p1 p2) in + f p0 (p2::reste) new_centroid + | _ -> + let area = poly_signed_area poly in + vect_mul_scal centroid (1./.area) + in + + match poly with + [] -> null_vector + | p::[] -> p + | p1::p2::[] -> point_middle p1 p2 + | _ -> f (List.hd poly) (List.tl poly) null_vector + +(* ============================================================================= *) +(* = Teste si un point est contenu dans un volume = *) +(* ============================================================================= *) +let point_in_volume pt vol = + let t = Hashtbl.create 11 in + + (* Ajout des points a une hashtable pour compter les points en double/triple *) + let add_point pt = + try let nb = Hashtbl.find t pt in Hashtbl.replace t pt (nb+1) + with Not_found -> Hashtbl.add t pt 1 + in + + (* Teste si le point d'intersection est sur une des aretes du volume *) + let rec point_on_one_segment pt l = + match l with + poly::reste -> + let rec f l = + match l with + p1::p2::reste -> + if point_on_segment pt p1 p2 then true else f (p2::reste) + | _ -> false + in + if f (poly_close poly) then true else point_on_one_segment pt reste + | [] -> false + in + + (* Test supplementaire pour voir si les points d'intersections se trouvent sur *) + (* un des sommets ou une des aretes *) + let traite_pts_inter is_in = + Hashtbl.iter (fun pt n -> + (* n=2 -> arete, n=3 -> sommet *) + if (n=2 or n=3) && point_on_one_segment pt vol then is_in:=not !is_in) t + in + + let rec find_direction lst_faces = + match lst_faces with + poly::reste -> + (* On essaie avec la direction entre le point et le centroide *) + (* de la face courante *) + let centroid = poly_centroid poly in + let dir = vect_normalize (vect_make pt centroid) in + + (* Normale au plan du polygone pour avoir l'angle *) + let n = poly_normal poly in + + (* Rappel : les deux vecteurs dir et n sont normalises donc pas besoin *) + (* de diviser par le produit des normes pour avoir l'angle *) + let s = dot_product dir n in + + (* On conserve cette direction si l'angle est inferieur a ~85 degres *) + if abs_float s >=0.1 then dir + else find_direction reste + | [] -> {x3D=1.; y3D=0.; z3D=0.} + in + + (* Choix d'une 'bonne' direction *) + let dir = find_direction vol in + + (* On compte le nombre d'intersections entre la demi-droite issue du point *) + (* a tester de vecteur directeur dir avec le volume *) + + let is_in = ref false and list_inter = ref [] in + List.iter (fun poly_face -> + if List.length poly_face>=3 then begin + (* 3 points definissant le plan contenant la face *) + let a = List.hd poly_face and b = List.hd (List.tl poly_face) + and c = List.hd (List.tl (List.tl poly_face)) in + + (* Evaluation du point P' projete de P, suivant dir, sur le plan contenant *) + (* la face poly_face *) + match crossing_hline_plane pt dir a b c with + None -> () (* Pas d'intersection *) + | Some p -> + add_point p ; + + (* Le point projete est-il dans le polygone constituant la face ? *) + if point_in_poly_2D p poly_face then + (* Oui -> une intersection de plus *) + is_in:=not !is_in + end) vol ; + + (* Test supplementaires pour les sommets et les aretes *) + traite_pts_inter is_in ; + + (* Nombre impair d'intersections -> le point est dans le volume *) + !is_in + + +(* ============================================================================= *) +(* = = *) +(* = Triangulation de polygones 3D = *) +(* = par defaut ils sont consideres comme ouverts = *) +(* = = *) +(* ============================================================================= *) + + +(* ============================================================================= *) +(* = Triangulation d'un polygone = *) +(* ============================================================================= *) +let tesselation poly = + (* Projection en 2D pour utiliser la routine de tesselation 2D *) + let (l, _) = poly_3d_to_poly_2d_smallest_span poly in + + (* Tesselation 2D *) + let indices = Geometry_2d.in_tesselation l in + + (* On remet le polygone en 3D en utilisant les indices des points *) + let t = Array.of_list poly in + List.map (fun (p1, p2, p3) -> [t.(p1); t.(p2); t.(p3)]) indices + +(* ============================================================================= *) +(* = Triangulation en triangles_fan = *) +(* ============================================================================= *) +let tesselation_fans poly = + (* Projection en 2D pour utiliser la routine de tesselation 2D *) + let (l, _) = poly_3d_to_poly_2d_smallest_span poly in + + (* Tesselation 2D *) + let (_, indices) = Geometry_2d.in_tesselation_fans l in + + (* On remet le polygone en 3D en utilisant les indices des points *) + let t = Array.of_list poly in + List.map (fun l -> List.map (fun x -> t.(x)) l) indices + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/geometry_3d.mli b/sw/lib/ocaml/geometry_3d.mli new file mode 100644 index 0000000000..6e712eff4e --- /dev/null +++ b/sw/lib/ocaml/geometry_3d.mli @@ -0,0 +1,234 @@ +(* + * $Id$ + * + * 3D Geometry + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(** Module de géométrie 3D + + Par défaut, les polygones sont considérés comme étant ouverts + + {b Dépendences : Geometry_2d} + + {e Yann Le Fablec, version 1.0, 22/04/2003} + *) + +(** {6 Types} *) + +(** Types pour les différents axes *) +type t_axis3d = T_X3D | T_Y3D | T_Z3D + +(** Types pour les intersections *) +type t_crossing3d = T_IN_SEG1 | T_IN_SEG2 | T_ON_PT1 | T_ON_PT2 | T_ON_PT3 +| T_ON_PT4 | T_OUT_SEG_PT1 | T_OUT_SEG_PT2 | T_OUT_SEG_PT3 | T_OUT_SEG_PT4 + +(** Type point/vecteur 3D *) +type pt_3D = { x3D : float; y3D : float; z3D : float; } + +(** Vecteur nul en 3D *) +val null_vector : pt_3D + +(** Un polygone 3D *) +type poly_3D = pt_3D list + +(** Un volume 3D *) +type volume_3D = poly_3D list + +(** {6 Points} *) + +(** [point_same A B] teste l'égalité stricte des deux points *) +val point_same : pt_3D -> pt_3D -> bool + +(** [distance A B] évalue la distance entre les points [A] et [B] *) +val distance : pt_3D -> pt_3D -> float + +(** [point_middle A B] renvoie le milieu du segment formé par [A] et [B] *) +val point_middle : pt_3D -> pt_3D -> pt_3D + +(** [barycenter lst_pts] renvoie le barycentre des points *) +val barycenter : pt_3D list -> pt_3D + +(** [weighted_barycenter lst_pts lst_poids] renvoie le barycentre des points + pondérés par [lst_poids] *) +val weighted_barycenter : pt_3D list -> float list -> pt_3D + +(** [pt_3d_to_pt_2d axis pt] transforme le point 3D en point 2D en supprimant + la coordonnée indiquée *) +val pt_3d_to_pt_2d : t_axis3d -> pt_3D -> Geometry_2d.pt_2D + +(** [point_in_plane P A B C] indique si le point [P] est dans le plan défini + par les trois points [A], [B] et [C] *) +val point_in_plane : pt_3D -> pt_3D -> pt_3D -> pt_3D -> bool + +(** [point_on_segment P A B] indique si le point P se trouve sur le segment [\[A, B\]] *) +val point_on_segment : pt_3D -> pt_3D -> pt_3D -> bool + +(** {6 Vecteurs} *) + + +(** [vect_make A B] crée le vecteur AB *) +val vect_make : pt_3D -> pt_3D -> pt_3D + +(** [vect_norm v] renvoie la norme du vecteur *) +val vect_norm : pt_3D -> float + +(** [vect_normalize v] normalise le vecteur *) +val vect_normalize : pt_3D -> pt_3D + +(** [vect_set_norm v norme] change [v] pour que sa norme soit [norme] *) +val vect_set_norm : pt_3D -> float -> pt_3D + +(** [vect_add u v] réalise la somme des deux vecteurs*) +val vect_add : pt_3D -> pt_3D -> pt_3D + +(** [vect_sub u v] renvoie la soustraction de [v] à [u] *) +val vect_sub : pt_3D -> pt_3D -> pt_3D + +(** [vect_mul_scal v scalaire] multiple le vecteur par un scalaire *) +val vect_mul_scal : pt_3D -> float -> pt_3D + +(** [vect_add_mul_scal lambda p v] renvoie le point [p] translaté du + vecteur [lambda.v] *) +val vect_add_mul_scal : float -> pt_3D -> pt_3D -> pt_3D + +(** [vect_inverse v] renvoie le vecteur opposé à [v] *) +val vect_inverse : pt_3D -> pt_3D + +(** [normal u v] renvoie la normale unitaire du plan défini par + les vecteurs [u] et [v] *) +val normal : pt_3D -> pt_3D -> pt_3D + + +(** {6 Produits} *) + +(** [dot_product u v] fournit le produit scalaire des deux vecteurs *) +val dot_product : pt_3D -> pt_3D -> float + +(** [cross_product u v] renvoie le produit vectoriel de [u] et [v] *) +val cross_product : pt_3D -> pt_3D -> pt_3D + + +(** {6 Intersections} *) + + +(** [test_in_segment type_intersection] teste si l'intersection est dans le + segment (extrémités incluses) *) +val test_in_segment : t_crossing3d -> bool + +(** [test_on_hl type_intersection] teste si l'intersection est sur la demi-droite + (extrémité incluse) *) +val test_on_hl : t_crossing3d -> bool + +(** [crossing_point A u B v] teste l'intersection des droites [(A, u)] et [(B, v)] + + En sortie, deux possibilités : + - [None] s'il n'y a pas d'intersection + - [Some (type1, type2, point_intersection)] sinon. [type1] désigne le type + d'intersection sur la première droite et [type2] la meme information pour + la seconde droite. + *) +val crossing_point : pt_3D -> pt_3D -> pt_3D -> pt_3D -> + (t_crossing3d * t_crossing3d * pt_3D) option + +(** [crossing_seg_seg A B C D] teste l'intersection des segments + [\[A,B\]] et [\[C,D\]] *) +val crossing_seg_seg : pt_3D -> pt_3D -> pt_3D -> pt_3D -> bool + +(** [crossing_seg_hl A B C u] teste l'intersection entre le segment [\[A,B\]] et + la droite passant par [C] et de vecteur directeur [u] *) +val crossing_seg_hl : + pt_3D -> pt_3D -> pt_3D -> pt_3D -> bool + +(** [crossing_hl_hl A u B v] teste l'intersection entre les deux demi-droites *) +val crossing_hl_hl : pt_3D -> pt_3D -> pt_3D -> pt_3D -> bool + +(** [crossing_lines A u B v] teste l'intersection entre les deux droites et renvoie le + point s'il y a effectivement intersection *) +val crossing_lines : pt_3D -> pt_3D -> pt_3D -> pt_3D -> bool * pt_3D + +(** [crossing_line_plane A u C D E] teste l'intersection de la droite passant par [A] + et de vecteur directeur [u] avec le plan défini par les trois points [C], [D] et [E] *) +val crossing_line_plane : pt_3D -> pt_3D -> pt_3D -> pt_3D -> pt_3D -> pt_3D option + +(** [crossing_hline_plane A u C D E] teste l'intersection de la demi-droite issue de [A] + et de vecteur directeur [u] avec le plan défini par les trois points [C], [D] et [E] *) +val crossing_hline_plane : pt_3D -> pt_3D -> pt_3D -> pt_3D -> pt_3D -> pt_3D option + + +(** {6 Polygones} *) + +(** [poly_3d_to_poly_2d axis poly] projete le polygone 3D en 2D en supprimant les + coordonnées de l'axe indiqué *) +val poly_3d_to_poly_2d : t_axis3d -> poly_3D -> Geometry_2d.poly_2D + +(** [poly_3d_to_poly_2d_smallest_span poly] idem en choisissant l'axe le plus approprié + numériquement (i.e celui d'étendue la plus faible) *) +val poly_3d_to_poly_2d_smallest_span : poly_3D -> Geometry_2d.poly_2D*t_axis3d + +(** [span_poly poly] renvoie, pour chaque axe, l'étendue du polygone *) +val span_poly : poly_3D -> float*float*float + +(** [point_in_poly pt poly] teste si le point est dans le polygone 3D *) +val point_in_poly : pt_3D -> poly_3D -> bool + +(** [point_in_poly_2D pt poly] teste si le point est dans le polygone quand les deux + sont projetés en 2D (en supprimant la coordonnée la plus appropriée) *) +val point_in_poly_2D : pt_3D -> poly_3D -> bool + +(** [poly_area poly] renvoie l'aire du polygone *) +val poly_area : poly_3D -> float + +(** [poly_signed_area poly] renvoie l'aire signée du polygone *) +val poly_signed_area : poly_3D -> float + +(** [poly_centroid poly] renvoie le centroide du polygone *) +val poly_centroid : poly_3D -> pt_3D + +(** [poly_normal poly] renvoie la normale unitaire au plan contenant le polygone *) +val poly_normal : poly_3D -> pt_3D + + +(** {6 Triangulation de polygones} *) + + +(** [geom_tesselation polygone] effectue la triangulation du polygone et renvoie + la liste des triangles résultants. Ici un triangle est une liste de 3 points (!). + De plus, le polygone ne doit contenir aucun point en double. + *) +val tesselation : poly_3D -> poly_3D list + +(** [geom_tesselation_fans polygone] effectue la triangulation du polygone en + triangle_fan OpenGL. En sortie est renvoyée une liste contenant des listes + de points. Chacune de ces listes de points contient soit 3 points (triangle) + soit plus de 3 points (pour un triangle_fan). + De plus, le polygone ne doit contenir aucun point en double. + *) +val tesselation_fans : poly_3D -> poly_3D list + + +(** {6 Volumes} *) + + +(** [point_in_volume P volume] teste si le point [P] se trouve dans le volume + (ce dernier peut ne pas etre convexe) *) +val point_in_volume : pt_3D -> volume_3D -> bool diff --git a/sw/lib/ocaml/gtk_3d.ml b/sw/lib/ocaml/gtk_3d.ml new file mode 100644 index 0000000000..a65dc5324b --- /dev/null +++ b/sw/lib/ocaml/gtk_3d.ml @@ -0,0 +1,1443 @@ +(* + * $Id$ + * + * 3D display widget + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(* Scale + lighting = pb si pas de GlNormalize *) +(* Ne pas oublier de faire un area#make_current () avant d'appeler des *) +(* commandes OpenGL car s'il y a plusieurs fenetres ouvertes, il arrive *) +(* que les commandes ne soient pas envoyees dans la bonne... *) + +(* Modules gtk/Gdk *) +open GMain +open GdkKeysyms +open GlGtk +open Gtk_tools_GL +open Platform +open Gtkgl_Hack + +(* Modules locaux *) +open Geometry_2d +open Geometry_3d + +(* Nombre de widgets 3D crees *) +let nb_objects = ref 0 + +(* Temps en millisecondes pour l'animation *) +let tps_anim = 40 + +(* Exception levee lors de la recherche d'un objet d'identifiant inconnu *) +exception NO_SUCH_3D_OBJECT of int +(* Exception levee lorsqu'un autre objet est passe alors qu'un contour + pays est attendu *) +exception NOT_A_3D_OUTLINE of int +(* Idem avec une ligne *) +exception NOT_A_3D_LINE of int +(* Idem avec un point *) +exception NOT_A_3D_POINT of int + +(* Debugging *) +let debug_3d = false +(* Les fontes ne sont disponibles que sous Linux... *) +let fonts_available = not platform_is_win32 + +(* Une couleur OpenGL *) +type glcolor = float*float*float + +let glcolor_white = (1., 1., 1.) +let glcolor_black = (0., 0., 0.) +let color_rosace = (1., 0., 0.) + +(* Point 2D *) +type glpoint2d = float*float + +(* Point 3D *) +type glpoint3d = float*float*float + +(* Point ou vecteur 3D nul *) +let pt_null = (0., 0., 0.) + +(* Point ou vecteur 3D indefini *) +let pt_undefined = (9999.99, 9999.99, 9999.99) + +(* Objet non compile *) +let not_compiled_obj = Obj.magic ~-1 + +(* Type pour designer les axes *) +type t_coord = X_AXIS | Y_AXIS | Z_AXIS + +(* Type des action possibles a la souris dans un widget 3D *) +type t_action = ACTION_NONE | ACTION_ZOOM of (int*int) | ACTION_ROTATE of (int*int) + +(* Definition des curseurs utilises *) +let cursor_standard = Gdk.Cursor.create `LEFT_PTR +let cursor_zoom_up = Gdk.Cursor.create `BASED_ARROW_UP +let cursor_zoom_down = Gdk.Cursor.create `BASED_ARROW_DOWN +let cursor_wait = Gdk.Cursor.create `WATCH +let cursor_rotate = Gdk.Cursor.create `EXCHANGE + +(* Valeurs OpenGL pour utiliser une source de lumiere *) +let lights = [`lighting; `light0; `color_material] + +(* Types de fleche *) +type t_arrow = ARROW1 | ARROW2 + +(* Type de polygone *) +type t_triangulation = + NO_TRI of glpoint3d list (* polygone convexe non triangule *) + | TRI_WITH_FANS of (glpoint3d list) list (* triangule avec des triangles fans *) + | TRI_STD of glpoint3d list (* triangule avec des triangles simples *) + +(* Volume 3D *) +type vol3d = { + vol3d_contour : glpoint3d list ; (* Faces verticales (quad_strip) *) + vol3d_up : t_triangulation ; (* Face horizontale superieure *) + vol3d_down : t_triangulation ; (* Face horizontale inferieure *) + mutable vol3d_color : glcolor ; (* Couleur du volume *) + mutable vol3d_filled : bool (* Volume plein ou fil de fer *) + } + + +(* Volume d'une enveloppe 3D *) +type env3d ={ + env3d_contour : glpoint3d list ; (* Faces laterales (quad_strip) *) + mutable env3d_color : glcolor ; (* Couleur du volume *) + mutable env3d_filled : bool (* Volume plein ou fil de fer *) + } + +(* Volume d'une enveloppe 3D *) +type env3d_double ={ + env3d_double_contour_out : glpoint3d list ; (* Faces laterales externes (quad_strip) *) + env3d_double_contour_in : glpoint3d list ; (* Faces laterales internes (quad_strip) *) + mutable env3d_double_color_out : glcolor ; (* Couleur des faces externes *) + mutable env3d_double_color_in : glcolor ; (* Couleur des faces internes *) + mutable env3d_double_filled : bool (* Volume plein ou fil de fer *) + } + + +(* Contour 3D *) +type out3d = { + out3d_contour : glpoint3d list ; (* Liste des points du contour *) + mutable out3d_color_in : glcolor ; (* Couleur interieure *) + mutable out3d_color_out : glcolor ; (* Couleur du contour *) + mutable out3d_filled : bool (* Contour plein ou fil de fer *) + } + +(* Ligne 3D *) +type line3d = { + line3d_points : glpoint3d list ; (* Points de la ligne *) + mutable line3d_width : int ; (* Epaisseur *) + mutable line3d_color : glcolor ; (* Couleur *) + mutable line3d_with_bars : bool ; (* Barres verticales *) + mutable line3d_filled : bool (* Surface jusqu'au sol *) + } + +(* Fleche 3D *) +type arr3d = { + arr3d_contour : (glpoint3d list) list ; (* Faces verticales (quad_strip) *) + arr3d_pt : glpoint3d ; (* Deplacement et rotation de la fleche car *) + arr3d_vect : glpoint3d ; (* elle est creee le long de l'axe X. Il faut*) + arr3d_angle_xy : float ; (* donc la remettre dans la bonne direction *) + arr3d_angle_z : float ; + mutable arr3d_color : glcolor ; (* Couleur de la fleche *) + mutable arr3d_filled : bool (* Fleche pleine ? *) + } + +(* Point 3D *) +type point3d ={ + p3d_pos : glpoint3d ; (* Position 3D du point *) + p3d_pos2 : glpoint3d ; (* Position du nom du point *) + p3d_name : string ; (* Nom du point *) + mutable p3d_with_name : bool ; (* Affichage du nom *) + mutable p3d_color : glcolor (* Couleur du point *) + } + +(* Surface triangulee en 3D *) +type surf3d = { + s3d_pts : (glpoint3d*glcolor) array array ; + mutable s3d_filled : bool + } + +(* Surface triangulee en 3D avec une texture *) +type surf3d_tex = { + s3d_tex_pts : glpoint3d array array ; (* Tableau des points de la surface *) + s3d_tex_texture_id : GlTex.texture_id (* Id de la texture a appliquer *) + } + +(* Type contenant tous les objets 3D possibles *) +type tobj3d = + VOLUME1_3D of vol3d | OUTLINE_3D of out3d + | LINE_3D of line3d | ARROW_3D of arr3d + | POINT_3D of point3d | ENVELOPPE_3D_DOUBLE of env3d_double + | ENVELOPPE_3D of env3d | SURFACE_3D of surf3d + | SURFACE_3D_TEX of surf3d_tex + +(* Stockage d'un objet 3D *) +type obj3d = {o_obj : tobj3d ; (* L'objet *) + o_id : int ; (* Son identifiant unique *) + mutable o_compiled : GlList.t ; (* L'objet compile *) + mutable o_show : bool (* Objet affiche ou pas *) + } + +(* module OImages = OImage *) +(* module Images = Image *) + +(* ============================================================================= *) +(* = Creation d'une texture a partir d'une image = *) +(* ============================================================================= *) +let make_image filename = + let img = + match OImages.tag (OImages.load filename []) with + OImages.Rgb24 rgb24 -> + rgb24 + | OImages.Index8 img | OImages.Index16 img -> + let rgb = img#to_rgb24 in + img#destroy; + rgb + | _ -> failwith "Gtk_3d.make_image" in + let w = img#width and h = img#height in + let image = GlPix.create `ubyte ~format:`rgb ~width:w ~height:h in + for i = 0 to h - 1 do + for j = 0 to w - 1 do + let pixel = img#get j i in (* pixel is a Color.rgb *) + let red = pixel.Images.r in + let green = pixel.Images.g in + let blue = pixel.Images.b in + Raw.sets (GlPix.to_raw image) ~pos:(3*(i*w+j)) + [| red; green; blue |] + done + done; + image + +let create_texture_from_image texture_filename = + let texture = make_image texture_filename in + let id = GlTex.gen_texture () in + GlTex.bind_texture `texture_2d id; + GluMisc.build_2d_mipmaps texture; + id + +(* ============================================================================= *) +(* = Infos OpenGL = *) +(* ============================================================================= *) +(* renvoie les informations relatives a la version d'OpenGL utilisee *) +let get_gl_infos () = + let l = [("Vendor", `vendor); ("Renderer", `renderer); + ("Version", `version); ("Extensions", `extensions)] in + let s = ref "" in + List.iter (fun (str, t) -> s:=!s^str^" : "^(GlMisc.get_string t)^"\n") l ; + !s + +(* ============================================================================= *) +(* = Affichages si mode debug = *) +(* ============================================================================= *) +(* [do_msg msg] affiche le message [msg] si {!Gtk_3d.debug_3d} est vrai *) +let do_msg msg = if debug_3d then begin Printf.printf "%s\n" msg;flush stdout end + +(* ============================================================================= *) +(* = Angle modulo 360 = *) +(* ============================================================================= *) +(* [mod_360 angle] angle modulo 360 degres *) +let mod_360 angle = mod_float angle 360. + +(* ============================================================================= *) +(* = Manipulations de coordonnees = *) +(* ============================================================================= *) +(* [get_coord (x, y, z) axis] renvoie la composante [x], [y] ou [z] suivant + l'axe indique *) +let get_coord (x, y, z) axis = match axis with X_AXIS->x | Y_AXIS->y | Z_AXIS->z + +(* [add_coord (x, y, z) axis delta] ajoute [delta] a la coordonnee definie + par [axis] *) +let add_coord (x, y, z) axis d = + match axis with + X_AXIS -> (x+.d, y, z) | Y_AXIS -> (x, y+.d, z) | Z_AXIS -> (x, y, z+.d) + +(* [add_coord_360 (x, y, z) axis delta] meme chose modulo 360 *) +let add_coord_360 (x, y, z) axis d = + match axis with + X_AXIS -> (mod_360 (x+.d), y, z) + | Y_AXIS -> (x, mod_360 (y+.d), z) + | Z_AXIS -> (x, y, mod_360 (z+.d)) + +(* ============================================================================= *) +(* = Encapsulation de fonctions 3D = *) +(* ============================================================================= *) +let glpoint3d_of_pt_3d u = (u.x3D, u.y3D, u.z3D) +let glpoint3d_to_pt_3d (x, y, z) = {x3D=x; y3D=y; z3D=z} +let glpoint3d_of_pt_2d z u = (u.x2D, u.y2D, z) +let glpoint3d_of_pt_3d_lst l = List.map glpoint3d_of_pt_3d l + +(* ============================================================================= *) +(* = Normale unitaire a un triplet de points = *) +(* ============================================================================= *) +(* [geom_normal A B C] renvoie la normale unitaire a un triplet de point *) +let geom_normal p1 p2 p3 = + let p1 = glpoint3d_to_pt_3d p1 and p2 = glpoint3d_to_pt_3d p2 + and p3 = glpoint3d_to_pt_3d p3 in + let n = Geometry_3d.normal (Geometry_3d.vect_make p1 p2) + (Geometry_3d.vect_make p1 p3) in + glpoint3d_of_pt_3d n + +let geom_scal_mult n (x, y, z) = (x*.n, y*.n, z*.n) + +(* Fermeture d'un polygone *) +let geom_close_poly l = if l=[] then [] else l@[List.hd l] + + +(* ============================================================================= *) +(* = Encapsulation de fonctions OpenGL = *) +(* ============================================================================= *) +(* polygones remplis *) +let set_gl_fillpoly () = GlDraw.polygon_mode ~face:`both `fill +(* polygones vides (contour uniquement) *) +let unset_gl_fillpoly () = GlDraw.polygon_mode ~face:`both `line + +(* [set_color color] met a jour de la couleur de dessin/remplissage *) +let set_color color = GlDraw.color color +(* [set_faded_color color pct] applique la couleur plus sombre de [pct]% *) +let set_faded_color (r, g, b) pct = + let p = (float_of_int pct)/.100. in GlDraw.color (r*.p, g*.p, b*.p) + +(* [set_3d_points type_objet_opengl lst_pts] creation d'une liste de points 3D *) +let set_3d_points typ l = + GlDraw.begins typ ; List.iter GlDraw.vertex3 l ; GlDraw.ends () + +let set_3d_points_with_color typ l = + GlDraw.begins typ ; + List.iter (fun ((p, c), n) -> + set_color c; GlDraw.normal3 n; GlDraw.vertex3 p) l ; + GlDraw.ends () + +let set_3d_points_with_texture typ l (dx, dy) (x1, y1) = + GlDraw.begins typ ; + List.iter (fun ((x,y,z), n) -> + GlDraw.normal3 n; GlTex.coord2 ((x-.x1)/.dx, (y-.y1)/.dy) ; + GlDraw.vertex3 (x,y,z)) l ; + GlDraw.ends () + +(* [set_3d_points_quad_strip_with_normal lst_pts] creation d'une liste de + points 3D [quad_strip] avec les normales *) +let set_3d_points_quad_strip_with_normal l = + let t = Array.of_list l in + GlDraw.begins `quad_strip ; + Array.iteri (fun i pt -> + (* Tous les 2 points -> normale au quad_strip courant *) + if i mod 2=0 then + if i=0 or i=List.length l-2 then GlDraw.normal3 (geom_normal pt t.(1) t.(2)) + else GlDraw.normal3 (geom_normal pt t.(i+1) t.(i+2)) ; + GlDraw.vertex3 pt) t ; + GlDraw.ends () + +(* normale vers le haut *) +let normal_up () = GlDraw.normal3 (0., 0., 1.) +(* normale vers le bas *) +let normal_down () = GlDraw.normal3 (0., 0., -.1.) + +(* [rotate axis angle] effectue une rotation suivant un axe donne *) +let rotate axis angle = + match axis with + X_AXIS -> GlMat.rotate ~angle ~x:1.0 () + | Y_AXIS -> GlMat.rotate ~angle ~y:1.0 () + | Z_AXIS -> GlMat.rotate ~angle ~z:1.0 () + +(* [rotate_some angles axis_list] rotation suivant les axes + indiques par [axis_list]. + La liste [angles] donne la valeur pour chaque axe concerne *) +let rotate_some angles axis_list = + List.iter (fun axis -> rotate axis (get_coord angles axis)) axis_list +(* [rotate_all angles] rotation suivant tous les axes *) +let rotate_all angles = rotate_some angles [X_AXIS; Y_AXIS; Z_AXIS] + + +(* ============================================================================= *) +(* = Calcul des normales pour les surfaces 3D avec ou sans texture = *) +(* ============================================================================= *) +let get_surface_normals tt = + let geom_normal_in p1 p2 p3 = + let p1 = glpoint3d_to_pt_3d p1 and p2 = glpoint3d_to_pt_3d p2 + and p3 = glpoint3d_to_pt_3d p3 in + Geometry_3d.normal (Geometry_3d.vect_make p1 p2) (Geometry_3d.vect_make p1 p3) + in + + Array.mapi (fun i t0 -> + Array.mapi (fun j p -> + if i>0 && i < Array.length tt-1 then begin + if j>0 && j < Array.length t0-1 then begin + let n1 = geom_normal_in p tt.(i-1).(j) tt.(i).(j-1) + and n2 = geom_normal_in p tt.(i-1).(j+1) tt.(i-1).(j) + and n3 = geom_normal_in p tt.(i).(j+1) tt.(i-1).(j+1) + and n4 = geom_normal_in p tt.(i+1).(j) tt.(i).(j+1) + and n5 = geom_normal_in p tt.(i+1).(j-1) tt.(i+1).(j) + and n6 = geom_normal_in p tt.(i).(j-1) tt.(i+1).(j-1) in + let n = Geometry_3d.vect_add n1 n2 in + let n = Geometry_3d.vect_add n n3 in + let n = Geometry_3d.vect_add n n4 in + let n = Geometry_3d.vect_add n n5 in + let n = Geometry_3d.vect_add n n6 in + let n = Geometry_3d.vect_normalize n in + glpoint3d_of_pt_3d n + end else (0., 0., 1.) + end else (0., 0., 1.)) t0) tt + +(* ============================================================================= *) +(* = Manipulation des objets = *) +(* ============================================================================= *) +(* [get_object_color objet] renvoie la couleur de l'objet *) +let get_object_color obj = + match obj with + OUTLINE_3D o -> o.out3d_color_out + | LINE_3D l -> l.line3d_color + | VOLUME1_3D v -> v.vol3d_color + | ENVELOPPE_3D e -> e.env3d_color + | ENVELOPPE_3D_DOUBLE e -> e.env3d_double_color_out + | ARROW_3D a -> a.arr3d_color + | POINT_3D p -> p.p3d_color + | SURFACE_3D s -> glcolor_white + | SURFACE_3D_TEX s -> glcolor_white + +(* [set_object_color objet color] met a jour la couleur de l'objet *) +let set_object_color obj color = + match obj with + OUTLINE_3D o -> o.out3d_color_out <- color + | LINE_3D l -> l.line3d_color <- color + | VOLUME1_3D v -> v.vol3d_color <- color + | ENVELOPPE_3D e -> e.env3d_color <- color + | ENVELOPPE_3D_DOUBLE e -> e.env3d_double_color_out<- color + | ARROW_3D a -> a.arr3d_color <- color + | POINT_3D p -> p.p3d_color <- color + | _ -> () + +(* [get_outline_in_color objet id] renvoie la couleur de remplissage d'un objet + de type [OUTLINE_3D]. Si l'objet n'est pas de ce type, l'exception + {!Gtk_3d.NOT_A_3D_OUTLINE} est levee *) +let get_outline_in_color obj id = + match obj with OUTLINE_3D o -> o.out3d_color_in + | _ -> raise (NOT_A_3D_OUTLINE id) +(* [set_outline_in_color objet color id] met a jour la couleur de remplissage pour + un objet de type [OUTLINE_3D]. Si l'objet n'est pas de ce type, l'exception + {!Gtk_3d.NOT_A_3D_OUTLINE} est levee *) +let set_outline_in_color obj color id = + match obj with OUTLINE_3D o -> o.out3d_color_in <- color + | _ -> raise (NOT_A_3D_OUTLINE id) + +(* [get_object_fill objet] indique si l'objet est rempli ou en fil de fer *) +let get_object_fill obj = + match obj with + OUTLINE_3D o -> o.out3d_filled + | LINE_3D l -> l.line3d_filled + | VOLUME1_3D v -> v.vol3d_filled + | ENVELOPPE_3D e -> e.env3d_filled + | ENVELOPPE_3D_DOUBLE e -> e.env3d_double_filled + | ARROW_3D a -> a.arr3d_filled + | POINT_3D p -> false + | SURFACE_3D s -> s.s3d_filled + | SURFACE_3D_TEX s -> true + +(* [set_object_filled objet filled] force l'objet en mode plein ou fil de fer *) +let set_object_fill obj filled = + match obj with + OUTLINE_3D o -> o.out3d_filled <- filled + | LINE_3D l -> l.line3d_filled <- filled + | VOLUME1_3D v -> v.vol3d_filled <- filled + | ENVELOPPE_3D e -> e.env3d_filled <- filled + | ENVELOPPE_3D_DOUBLE e -> e.env3d_double_filled <- filled + | ARROW_3D a -> a.arr3d_filled <- filled + | POINT_3D p -> () + | SURFACE_3D s -> s.s3d_filled <- filled + | SURFACE_3D_TEX s -> () + +(* [get_line_width objet id] renvoie l'epaisseur d'un objet ligne. Si l'objet + passe n'est pas du type [LINE_3D] alors l'exception {!Gtk_3d.NOT_A_3D_LINE} + est levee *) +let get_line_width obj id = + match obj with LINE_3D l -> l.line3d_width | _ -> raise (NOT_A_3D_LINE id) +(* [set_line_width objet width id] met a jour l'epaisseur d'un objet ligne. Si l'objet + passe n'est pas du type [LINE_3D] alors l'exception {!Gtk_3d.NOT_A_3D_LINE} + est levee *) +let set_line_width obj width id = + match obj with LINE_3D l -> l.line3d_width<-width|_ -> raise (NOT_A_3D_LINE id) +(* [get_line_bars objet id] indique si l'objet est affiche avec des barres + verticales si cet objet est du type [LINE_3D]. Si ca n'est pas le cas, + l'exception {!Gtk_3d.NOT_A_3D_LINE} est levee *) +let get_line_bars obj id = + match obj with LINE_3D l -> l.line3d_with_bars | _ -> raise (NOT_A_3D_LINE id) +(* [set_line_bars objet bars id] met a jour l'affichage des barres + verticales d'un objet de type [LINE_3D]. S'il n'est pas de ce type alors + l'exception {!Gtk_3d.NOT_A_3D_LINE} est levee *) +let set_line_bars obj bars id = + match obj with LINE_3D l ->l.line3d_with_bars<-bars|_ ->raise (NOT_A_3D_LINE id) +(* [get_point_name objet id] indique si le nom de l'objet [POINT_3D] est + affiche. Si l'objet passe n'est pas du type [POINT_3D], + l'exception {!Gtk_3d.NOT_A_3D_POINT} est levee*) +let get_point_name obj id = + match obj with POINT_3D p -> p.p3d_with_name | _ -> raise (NOT_A_3D_POINT id) +(* [set_point_name objet name id] met a jour l'affichage ou pas du nom. + Si l'objet passe n'est pas du type [POINT_3D], + l'exception {!Gtk_3d.NOT_A_3D_POINT} est levee*) +let set_point_name obj name id = + match obj with POINT_3D p ->p.p3d_with_name <-name|_ ->raise (NOT_A_3D_POINT id) + + + +(* ============================================================================= *) +(* = Objet d'affichage 3D = *) +(* = pack = ou mettre le widget = *) +(* = with_status_bar = creation d'une barre d'infos optionnelle = *) +(* = n = nom de la fenetre = *) +(* ============================================================================= *) +(* [widget_3d pack with_status_bar name] cree un widget d'affichage 3D + + - [pack] indique où mettre le widget + - [with_status_bar] permet la creation d'une barre d'infos optionnelle + - [name] designe le nom a donner a la zone d'affichage (eventuellement affichee + en haut a gauche de la zone) + *) +class widget_3d pack with_status_bar n = + (* Creation de la GtkGlArea avec ou sans barre d'infos *) + let (area, setstatus) = + if with_status_bar then begin + let v = Gtk_tools.create_vbox pack in + let a = GlGtk.area [`RGBA; `DOUBLEBUFFER; `DEPTH_SIZE 1] ~packing:v#add () in + let s = GMisc.statusbar ~packing:v#pack () in + let ss = s#new_context ~name:"status" in + (a, fun msg -> ignore(ss#push msg)) + end else + (GlGtk.area [`RGBA; `DOUBLEBUFFER; `DEPTH_SIZE 1] ~packing:pack (), + fun msg -> ()) + in + + object (self) + + (* Numero du widget 3D *) + val nb = !nb_objects + + (* Nom de la fenetre eventuellement affiche en haut a gauche *) + val mutable name = n + (* Indique si le nom de la fenetre doit etre affiche *) + val mutable show_name = true + + (* Contient la largeur de la fenetre apres redimensionnement *) + val mutable width = -1 + (* Contient la hauteur de la fenetre apres redimensionnement *) + val mutable height = -1 + (* Position utilisateur *) + val mutable depl = (0., 0., -.1.5) + (* Rotation utilisateur *) + val mutable rot = (290., 0., 0.) + (* Position de la source de lumiere *) + val mutable lightpos = (-.0.3, -.0.3, 0.6) + (* Rotation de la source de lumiere *) + val mutable lightrot = pt_null + + (* Couleur du fond *) + val mutable back_color = glcolor_black + + (* Point central de la scene *) + val mutable extents = pt_null + (* Rayon de la scene *) + val mutable rs = 1. + (* Extremes minis *) + val mutable extreme_min = pt_undefined + (* Extremes maxis *) + val mutable extreme_max = pt_undefined + + (* indique si l'initialisation a ete faite *) + val mutable done_init = false + (* Action souris en cours s'il y en a une *) + val mutable current_action = ACTION_NONE + (* indique si les lumieres sont utilisees *) + val mutable use_lights = true + (* indique si la source de lumiere est affichee *) + val mutable show_light = false + (* smoothing ? *) + val mutable use_smooth = true + + (* repertoire pour les captures ecran *) + val mutable screenshot_path = "Captures/" + (* nom par defaut de la capture *) + val mutable screenshot_name = "capture3d.png" + (* format par defaut des captures *) + val mutable screenshot_format = Gtk_image.PNG + + (* [triangle_fan] ou [triangle] pour afficher les surfaces triangulees *) + val mutable use_fans_for_tesselation = true + + (* increment d'eloignement lie aux touches *) + val dist_incr = -.0.1 + (* increment de rotation lie aux touches *) + val rot_incr = 2. + + (* Rotation pendant l'animation *) + val rot_anim = 0.1 + + (* Compteur des objets pour les identifiants uniques *) + val mutable cpt_obj = 0 + (* Liste des objets 3D definis dans le widget 3D *) + val mutable objects = ([]:obj3d list) + + (* indique si la rosace doit etre affichee *) + val mutable show_rosace = true + (* Objet rosace *) + val mutable rosace = None + + (* Fonte OpenGL si disponible (i.e sous Unix) *) + val mutable fontbase = Obj.magic ~-1 + + (* Timer utilise pour l'animation *) + val mutable animation_timer = None + + (* cree un nouvel identifiant *) + method private get_new_id = let n = cpt_obj in cpt_obj<-cpt_obj+1; n + + (* Indique a OpenGL que la fenetre est la fenetre courante + dans laquelle doivent etre effectuees les commandes OpenGL + A faire absolument avant d'ajouter des objets pour que la + bonne fenetre recoive les commandes OpenGL qui suivent... *) + method private make_current = area#make_current () + + (* Mise a jour de la barre d'infos *) + method private set_status = + let msg = ref "" in + let add txt = msg:=if !msg="" then txt else !msg^" "^txt in + let (x, y, z) = rot in + add (Printf.sprintf "X=%.0f Y=%.0f Z=%.0f" x y z) ; + let (_, _, z) = depl in + add (Printf.sprintf "Dist=%.1f" (-.z)) ; + add (if use_lights then "Lights on " else "Lights off") ; + add (if use_smooth then "Smooth on " else "Smooth off") ; + setstatus !msg + + (* force l'utilisation de la lumiere *) + method lights_on = use_lights <- true; self#update_lights + (* annule l'utilisation de la lumiere *) + method lights_off = use_lights <- false; self#update_lights + (* change l'etat d'utilisation de la lumiere *) + method lights_switch = use_lights <- not use_lights; self#update_lights + (* met a jour le widget pour utiliser ou pas les lumieres suivant + la valeur de [use_lights] *) + method private update_lights = + do_msg (if use_lights then "Lights on" else "Lights off") ; + List.iter (if use_lights then Gl.enable else Gl.disable) lights ; + self#setup; self#display_func + + (* force l'utilisation du lissage *) + method smooth_on = use_smooth <- true; self#update_smooth + (* annule l'utilisation du lissage *) + method smooth_off = use_smooth <- false; self#update_smooth + (* change l'etat d'utilisation du lissage *) + method smooth_switch = use_smooth <- not use_smooth; self#update_smooth + (* met a jour le widget 3D pour appliquer ou pas le lissage suivant la + valeur de [use_smooth] *) + method private update_smooth = + do_msg (if use_smooth then "Smooth on" else "Smooth off") ; + GlDraw.shade_model (if use_smooth then `smooth else `flat) ; + self#set_status ; self#display_func + + (* affiche la rosace *) + method rosace_on = show_rosace <- true; self#display_func + (* masque la rosace *) + method rosace_off = show_rosace <- false; self#display_func + (* change l'etat d'affichage de la rosace suivant la valeur de [show_rosace] *) + method rosace_switch = show_rosace <- not show_rosace; self#display_func + + (* Modification de la vue et redessin *) + method change_and_redraw = self#setup; self#display_func + + (* Rotation/deplacement de la position utilisateur *) + method rotate_view r = rot<-r; self#change_and_redraw + method move_view d = depl<-d; self#change_and_redraw + + (* Modification du curseur *) + method private set_cursor c = + Gtk_tools.set_cursor area#misc#window c + method private reset_cursor = + Gtk_tools.set_cursor area#misc#window cursor_standard + + (* [scale_point point] met a l'echelle un point *) + method private scale_point pt = geom_scal_mult (1./.rs) pt + (* [scale_points lst_points] met a l'echelle une liste de points *) + method private scale_points l = List.map self#scale_point l + + (* [draw_triangulation t] dessine une face polygonale triangulee ou + pas en effectuant en plus la mise a l'echelle *) + method private draw_triangulation t = + match t with + NO_TRI l -> set_3d_points `polygon (self#scale_points l) + | TRI_STD l -> set_3d_points `triangles (self#scale_points l) + | TRI_WITH_FANS l -> List.iter (fun l -> + set_3d_points (if List.length l>3 then `triangle_fan else `triangles) + (self#scale_points l)) l + + (* [create_pyramid (x, y, z) size] cree une pyramide de hauteur + [size] centree en [(x, y, z)] *) + method private create_pyramid (x, y, z) size = + let d = size*.sqrt(3.)/.2. in + let a=d/.3. and b = (2.*.d)/.3. in + let p0 = (x, y, z+.b) and p1 = (x, y+.b, z-.a) + and p2 = (x-.size/.2., y-.a, z-.a) and p3 = (x+.size/.2., y-.a, z-.a) in + set_3d_points `triangle_fan [p0; p1; p2; p3; p1] ; + set_3d_points `triangles [p2; p1; p3] + + (* [compile_one objet] met a l'echelle et compile l'objet *) + method private compile_one o = + GlList.delete o.o_compiled ; + let compiled = GlList.create `compile in + (match o.o_obj with + OUTLINE_3D o -> + let pts = self#scale_points o.out3d_contour in + if o.out3d_filled then begin + set_gl_fillpoly () ; set_color o.out3d_color_in ; + (* Ici il faudrait trianguler le polygone si necessaire... *) + set_3d_points `polygon pts + end ; + GlDraw.line_width 2. ; + unset_gl_fillpoly () ; set_color o.out3d_color_out ; + set_3d_points `polygon pts + | LINE_3D l -> + Gl.disable `cull_face ; + let pts = self#scale_points l.line3d_points in + if l.line3d_filled or l.line3d_with_bars then begin + let lfill = List.flatten (List.map (fun (a, b, c) -> + [(a, b, 0.); (a, b, c)]) pts) in + if l.line3d_filled then begin + set_gl_fillpoly () ; set_faded_color l.line3d_color 70 ; + set_3d_points `quad_strip lfill + end ; + if l.line3d_with_bars then begin + GlDraw.line_width 2. ; + unset_gl_fillpoly () ; set_faded_color l.line3d_color 85 ; + set_3d_points `quad_strip lfill + end + end ; + GlDraw.line_width (float_of_int l.line3d_width) ; + set_color l.line3d_color ; + set_3d_points `line_strip pts ; + Gl.enable `cull_face + | VOLUME1_3D v -> + if v.vol3d_filled then set_gl_fillpoly () else begin + GlDraw.line_width 2. ; unset_gl_fillpoly () + end ; + (* Faces verticales *) + GlLight.color_material ~face:`front `ambient_and_diffuse ; + set_color v.vol3d_color ; + set_3d_points_quad_strip_with_normal (self#scale_points v.vol3d_contour) ; + + (* Dessin des faces inferieure et superieure avec une couleur plus sombre *) + if v.vol3d_filled then begin + GlLight.color_material ~face:`front `ambient_and_diffuse ; + set_faded_color v.vol3d_color 75 ; + normal_up () ; self#draw_triangulation v.vol3d_up ; + normal_down () ; self#draw_triangulation v.vol3d_down + end + | ENVELOPPE_3D e -> + (* Faces tjs visibles: Gl.disable `cull_face ; *) + if e.env3d_filled then set_gl_fillpoly () else begin + GlDraw.line_width 2. ; unset_gl_fillpoly () + end ; + + (* Faces verticales *) + GlLight.color_material ~face:`front `ambient_and_diffuse ; + set_color e.env3d_color ; + set_3d_points_quad_strip_with_normal (self#scale_points e.env3d_contour); + (*si faces env tjs visibles: Gl.enable `cull_face *) + | ENVELOPPE_3D_DOUBLE e -> + (* Faces tjs visibles: Gl.disable `cull_face ; *) + if e.env3d_double_filled then set_gl_fillpoly () else begin + GlDraw.line_width 2. ; unset_gl_fillpoly () + end ; + + (* Faces verticales externes *) + GlLight.color_material ~face:`front `ambient_and_diffuse ; + set_color e.env3d_double_color_out ; + set_3d_points_quad_strip_with_normal + (self#scale_points e.env3d_double_contour_out); + + (* Faces verticales internes *) + GlLight.color_material ~face:`front `ambient_and_diffuse ; + set_color e.env3d_double_color_in ; + set_3d_points_quad_strip_with_normal + (self#scale_points e.env3d_double_contour_in) + + (*si faces env tjs visibles: Gl.enable `cull_face *) + | ARROW_3D a -> + if a.arr3d_filled then set_gl_fillpoly () else begin + GlDraw.line_width 2. ; unset_gl_fillpoly () + end ; + GlLight.color_material ~face:`front `ambient_and_diffuse ; + set_color a.arr3d_color ; + + GlMat.push () ; + (* Deplace la fleche sur la pointe de la fleche *) + GlMat.translate3 (self#scale_point a.arr3d_pt) ; + (* Tourne la fleche pour qu'elle soit orientee comme voulu *) + GlMat.rotate3 a.arr3d_vect ~angle:a.arr3d_angle_xy ; + (* Tourne selon Z pour remettre la fleche comme il faut *) + rotate Z_AXIS a.arr3d_angle_z ; + + (* Mise a l'echelle des points de la fleche *) + let l = List.map self#scale_points a.arr3d_contour in + (* Faces 'verticales' *) + set_3d_points_quad_strip_with_normal (List.flatten l) ; + if a.arr3d_filled then begin + GlLight.color_material ~face:`front `ambient_and_diffuse ; + set_faded_color a.arr3d_color 75 ; + let (up, down) = + (List.rev_map (fun l -> List.hd (List.tl l)) l, + List.map (fun l -> List.hd l) l) in + (* Faces 'horizontales' *) + normal_up () ; set_3d_points `polygon up ; + normal_down () ; set_3d_points `polygon down + end ; + GlMat.pop () + | POINT_3D p -> + let (x0, y0, z0) = self#scale_point p.p3d_pos + and (x, y, z) = self#scale_point p.p3d_pos2 in + set_gl_fillpoly () ; set_color p.p3d_color ; + let size = 0.02 in + self#create_pyramid (x0, y0, z0) size ; + + if p.p3d_with_name then begin + GlDraw.line_width 2.0 ; + set_3d_points `line_strip [(x, y, z); (x0, y0, z0+.size)] ; + if fonts_available then begin + GlPix.raster_pos ~x ~y ~z:(z+.0.01) () ; + Gtkgl_Hack.gl_print_string fontbase p.p3d_name + end + end ; + + | SURFACE_3D_TEX s -> + normal_up () ; + let tt = Array.map (Array.map self#scale_point) s.s3d_tex_pts in + (* On recentre les coordonnees dans la texture pour la voir en entier *) + (* sur toute la surface *) + let (x1,y1,_) = tt.(0).(0) and ttt = tt.(Array.length tt-1) in + let (x2,y2,_) = ttt.(Array.length ttt-1) in + let delta = (x2-.x1, y2-.y1) and first = (x1, y1) in + + let t_normals = get_surface_normals tt in + Gl.enable `texture_2d; GlDraw.shade_model `flat ; + (* On utilise la texture indiquee *) + GlTex.bind_texture `texture_2d s.s3d_tex_texture_id; + Array.iteri (fun i t0 -> + if i + l:=(tt.(i+1).(j), t_normals.(i+1).(j))::(p, t_normals.(i).(j))::!l) t0 ; + set_3d_points_with_texture `triangle_strip (List.rev !l) delta first + end) tt ; + Gl.disable `texture_2d ; + GlDraw.shade_model (if use_smooth then `smooth else `flat) + + | SURFACE_3D s -> + if s.s3d_filled then set_gl_fillpoly () else begin + GlDraw.line_width 2. ; unset_gl_fillpoly () + end ; + GlLight.color_material ~face:`front `ambient_and_diffuse ; + normal_up () ; + let t = Array.map (Array.map (fun (p, c) -> (self#scale_point p, c))) s.s3d_pts in + + let t_normals = get_surface_normals (Array.map (Array.map fst) t) in + Array.iteri (fun i t0 -> + if i + l:=(t.(i+1).(j), t_normals.(i+1).(j))::(p, t_normals.(i).(j))::!l) t0 ; + set_3d_points_with_color `triangle_strip (List.rev !l) + end) t + ) ; + GlList.ends () ; + o.o_compiled <- compiled + + (* recompilation de tous les objets contenus dans le widget *) + method private recompile_all_objects = List.iter self#compile_one objects + (* [make_and_compile objet] compile uniquement l'objet indique *) + method private make_and_compile o = self#make_current ; self#compile_one o + + (* [add_object objet] ajoute l'objet : compilation de cet objet et eventuelle + recompilation des autres s'il sort des extremes precedents *) + method private add_object o = + self#make_current ; + let new_o = {o_obj=o; o_id=self#get_new_id; + o_compiled=not_compiled_obj; o_show=true} in + objects <- new_o::objects ; + (* Recherche des valeurs extremes de cet objet si necessaire *) + let old_rs = rs in + let (do_it, l) = + match o with + OUTLINE_3D o -> (false, []) + | LINE_3D l -> (true, l.line3d_points) + | VOLUME1_3D v -> (true, v.vol3d_contour) + | ENVELOPPE_3D e -> (true, e.env3d_contour) + | ENVELOPPE_3D_DOUBLE e -> (true, e.env3d_double_contour_out) + | ARROW_3D a -> (false, []) + | POINT_3D p -> (true, [p.p3d_pos; p.p3d_pos2]) + | SURFACE_3D s -> + let l = Array.to_list (Array.map (fun t -> Array.to_list t) s.s3d_pts) in + (true, List.map fst (List.flatten l)) + | SURFACE_3D_TEX s -> + let l = Array.to_list (Array.map (fun t -> Array.to_list t) s.s3d_tex_pts) in + (true, List.flatten l) + in + if do_it then self#get_extremes l ; + if old_rs<>rs then + (* Il faut recompiler les autres objets car les extremes ont change... *) + self#recompile_all_objects + else + (* Compilation uniquement de cet objet *) + self#compile_one new_o ; + + (* Renvoie l'identifiant du nouvel objet cree *) + new_o.o_id + + (* [get_extremes pts_list] recherche les points extremes d'un objet, met + a jour les valeurs pour la scene et reaffiche si necessaire *) + method private get_extremes pts_list = + if pts_list <> [] then begin + let (p1, p2) = + if extreme_min=pt_undefined && extreme_max=pt_undefined then + (List.hd pts_list, List.hd pts_list) + else (extreme_min, extreme_max) + in + + let minmax coord l = + let dep = (get_coord p1 coord, get_coord p2 coord) in + List.fold_left (fun (i, a) e0 -> + let e = get_coord e0 coord in (min e i, max e a)) dep l + in + + let (minx, maxx) = minmax X_AXIS pts_list + and (miny, maxy) = minmax Y_AXIS pts_list + and (minz, maxz) = minmax Z_AXIS pts_list in + let cc i a = (a +. i) /. 2. in + let r = max (maxx -. minx) (maxy -. miny) in + rs <- max r (maxz -. minz) ; + extreme_min <- (minx, miny, minz) ; + extreme_max <- (maxx, maxy, maxz) ; + let new_extents = self#scale_point (-.(cc minx maxx), -.(cc miny maxy), + -.(cc minz maxz)) in + if new_extents<>extents then begin + extents<-new_extents ; self#setup + end + end + + (* Volume simple du type secteur ou seul le contour et les niveaux *) + (* inf et sup suffisent *) + method add_object_volume_simple contour zmin zmax color filled = + (* Le contour doit etre oriente dans le sens contre-horaire (cull_face) *) + let contour = + if poly_test_ccw contour = CW then contour else List.rev contour in + let l0 = geom_close_poly ((List.map (fun p -> [glpoint3d_of_pt_2d zmin p; + glpoint3d_of_pt_2d zmax p]) + ) contour) in + + (* Triangulation ou pas des faces superieures et inferieures *) + let (down, up) = + if poly_test_convex contour = CONVEX then + (NO_TRI (List.map (fun l -> List.hd l) l0), + NO_TRI (List.rev_map (fun l -> List.hd (List.tl l)) l0)) + else begin + if use_fans_for_tesselation then begin + let fans = Geometry_2d.tesselation_fans contour in + let l1 = List.map (List.map (glpoint3d_of_pt_2d zmin)) fans + and l2 = List.map (fun lst -> + let l = List.map (glpoint3d_of_pt_2d zmax) lst in + (* Le premier point ne doit pas se trouver en dernier *) + (* car c'est le pivot du triangle_fan *) + (List.hd l)::(List.rev (List.tl l))) fans in + (TRI_WITH_FANS l1, TRI_WITH_FANS l2) + end else begin + let triangles = List.flatten (Geometry_2d.tesselation contour) in + let l1 = List.map (glpoint3d_of_pt_2d zmin) triangles + and l2 = List.rev_map (glpoint3d_of_pt_2d zmax) triangles in + (TRI_STD l1, TRI_STD l2) + end + end + in + self#add_object (VOLUME1_3D {vol3d_contour = List.flatten l0; + vol3d_up = up; + vol3d_down = down; + vol3d_color = color; + vol3d_filled = filled}) + (* Enveloppe 3D : faces laterales liant un contour_haut et un contour_bas *) + (* 2 contours = meme nombre de points, oriente dans le sens contre-horaire *) + method add_object_enveloppe contour_haut contour_bas color filled = + let add_p p_bas p_haut res = + (glpoint3d_of_pt_3d p_bas)::(glpoint3d_of_pt_3d p_haut)::res + in + let cw_poly c = + let contour_2d = List.map (pt_3d_to_pt_2d T_Z3D) c in + if (poly_test_ccw contour_2d = CW) then c else List.rev c + in + let l_bas = geom_close_poly (cw_poly contour_bas) in + let l_haut = geom_close_poly (cw_poly contour_haut) in + let l0 = + try List.fold_right2 add_p l_bas l_haut [] + with x-> Printf.printf "\nadd_object_enveloppe.fold... : "; raise x + in + try + self#add_object (ENVELOPPE_3D {env3d_contour = l0; + env3d_color = color; + env3d_filled = filled}) + with x -> + Printf.printf "\nadd_object_enveloppe. self#add_object "; raise x + + (* Enveloppe 3D : faces laterales liant un contour_haut et un contour_bas *) + (* 2 contours = meme nombre de points, oriente dans le sens contre-horaire *) + method add_object_enveloppe_double contour_haut contour_bas + color_out color_in filled = + let add_p p_bas p_haut res = + (glpoint3d_of_pt_3d p_bas)::(glpoint3d_of_pt_3d p_haut)::res + in + let cw_poly c = + let contour_2d = List.map (pt_3d_to_pt_2d T_Z3D) c in + if (poly_test_ccw contour_2d = CW) then c else List.rev c + in + let l_bas = geom_close_poly (cw_poly contour_bas) in + let l_haut = geom_close_poly (cw_poly contour_haut) in + let l0 = + try List.fold_right2 add_p l_bas l_haut [] + with x-> Printf.printf "\nadd_object_enveloppe.fold... : "; raise x + in + let l_inside = + try List.fold_right2 add_p (List.rev l_bas) (List.rev l_haut) [] + with x-> Printf.printf "\nadd_object_enveloppe.foldinside... : "; raise x + in + try + self#add_object (ENVELOPPE_3D_DOUBLE {env3d_double_contour_out = l0; + env3d_double_contour_in = l_inside; + env3d_double_color_out = color_out; + env3d_double_color_in = color_in; + env3d_double_filled = filled}) + with x -> + Printf.printf "\nadd_object_enveloppe. self#add_object "; raise x + + (* Fleche (flux) dont la pointe est placee en pt0 si sens est vrai *) + (* Ici, inutile de tesseler les faces inf et sup *) + method add_object_arrow pt0 vdir sens ep lg color filled + arrow_type = + let pts = + if arrow_type=ARROW1 then + [(0., 0.); (2.*.ep, ep); (2.*.ep, ep/.2.); (lg, ep/.2.); + (lg, (-.ep)/.2.); (2.*.ep, (-.ep)/.2.); (2.*.ep, -.ep)] + else + [(0., 0.); (2.5*.ep, ep); (2.*.ep, ep/.2.); (lg, ep/.2.); + (lg, (-.ep)/.2.); (2.*.ep, (-.ep)/.2.); (2.5*.ep, -.ep)] + in + + let pts = if sens then pts else List.map (fun (x, y) -> (x-.lg, y)) pts in + + let pts = List.map (fun (x, y) -> {x2D=x; y2D=y}) pts in + + let pts = if poly_test_ccw pts = CW then pts else List.rev pts in + let l = List.map (fun p -> + [glpoint3d_of_pt_2d ((-.ep)/.2.) p; glpoint3d_of_pt_2d (ep/.2.) p]) pts in + let dd = + if sens then Geometry_3d.vect_make pt0 vdir + else Geometry_3d.vect_make vdir pt0 + in + let h = Geometry_3d.vect_norm dd + and d = sqrt(dd.x3D*.dd.x3D+.dd.y3D*.dd.y3D) in + let alpha = + if dd.y3D>0. then acos (dd.x3D/.d) else -. (acos (dd.x3D/.d)) + in + let beta = + if dd.z3D>0. then acos (d/.h) else -.(acos (d/.h)) + in + + self#add_object (ARROW_3D {arr3d_contour = geom_close_poly l; + arr3d_pt = glpoint3d_of_pt_3d pt0 ; + arr3d_vect = (dd.y3D, -.dd.x3D, 0.) ; + arr3d_angle_xy = rad2deg beta ; + arr3d_angle_z = rad2deg alpha ; + arr3d_color = color; + arr3d_filled = filled}) + + method add_object_outline contour cin cout filled = + let l = geom_close_poly (glpoint3d_of_pt_3d_lst contour) in + self#add_object (OUTLINE_3D {out3d_contour = l ; + out3d_color_in = cin ; + out3d_color_out = cout ; + out3d_filled = filled}) + + method add_object_line l color line_width with_bars fill = + self#add_object (LINE_3D {line3d_points = glpoint3d_of_pt_3d_lst l; + line3d_width = line_width; + line3d_color = color; + line3d_with_bars = with_bars; + line3d_filled = fill}) + + method add_object_point pos pos2 name color with_name = + self#add_object (POINT_3D {p3d_pos = glpoint3d_of_pt_3d pos; + p3d_pos2 = glpoint3d_of_pt_3d pos2; + p3d_name = name; + p3d_with_name = with_name; + p3d_color = color}) + + method add_object_surface_with_texture tab texture_id = + let t = Array.map (Array.map glpoint3d_of_pt_3d) tab in + self#add_object (SURFACE_3D_TEX {s3d_tex_pts = t; + s3d_tex_texture_id = texture_id}) + + method add_object_surface tab fill = + let t = Array.map (Array.map (fun (p, c) -> (glpoint3d_of_pt_3d p, c))) tab in + self#add_object (SURFACE_3D {s3d_pts = t; s3d_filled = fill}) + + (* cree l'objet rosace *) + method private create_rosace = + self#make_current ; + let compiled = GlList.create `compile in + GlDraw.line_width 2.0 ; + GlDraw.color color_rosace ; + GlDraw.begins `line_strip ; + List.iter GlDraw.vertex2 [(0.0, -0.05); (0.0, 0.05)] ; + GlDraw.ends () ; + GlDraw.begins `line_strip ; + List.iter GlDraw.vertex2 [(-0.05, 0.0); (0.05, 0.0)] ; + GlDraw.ends () ; + GlDraw.begins `line_strip ; + List.iter GlDraw.vertex2 [(-0.01, 0.04); (0.0, 0.05); (0.01, 0.04)] ; + GlDraw.ends () ; + if fonts_available then begin + (* Affichage du Nord sur la rosace si la fonte est disponible *) + GlPix.raster_pos ~x:(-.0.01) ~y:0.07 ~z:0.0 () ; + Gtkgl_Hack.gl_print_string fontbase "N" + end ; + GlList.ends () ; + rosace <- Some compiled ; + compiled + + (* initialisation lors de la creation du widget *) + method private init_func () = + if not done_init then begin + do_msg "Init 3D" ; + List.iter Gl.enable [`depth_test; `cull_face] ; + GlDraw.cull_face `back; + GlDraw.front_face `ccw; + + List.iter (if use_lights then Gl.enable else Gl.disable) lights ; + GlDraw.shade_model (if use_smooth then `smooth else `flat) ; + + if fonts_available then fontbase <- load_bitmap_font "8x13" ; + + GlPix.store (`unpack_alignment 1); + List.iter (GlTex.parameter ~target:`texture_2d) + [ `wrap_s `repeat; `wrap_t `repeat; `mag_filter `linear; `min_filter `linear ]; + GlTex.env (`mode `decal); + + done_init <- true + end + + method private reshape_func ~width:w ~height:h = + if not done_init then self#init_func () ; + do_msg (Printf.sprintf "Reshape 3D w=%d h=%d" w h) ; + width <- w; height <- h ; + GlDraw.viewport ~x:0 ~y:0 ~w ~h; + self#setup + + (* met a jour de la vue et de la source de lumiere *) + method private setup = + do_msg "Setup" ; + GlMat.mode `projection; + GlMat.load_identity (); + let aspect = float width /. float height and view_fovs = 1. in + GluMat.perspective ~fovy:(45. *. view_fovs) ~aspect + ~z:(0.1, (rs*.sqrt(2.)+.1.)); + + GlMat.mode `modelview; + GlMat.load_identity (); + + (* Deplace et tourne la position de l'utilisateur *) + GlMat.translate3 depl ; + rotate_all rot ; + + (* Place les objets au centre de la scene *) + GlMat.translate3 extents ; + + (* Positionnement de la lumiere *) + if use_lights then begin + GlMat.push (); + rotate_all lightrot ; + let (x, y, z)= lightpos in + List.iter (GlLight.light ~num:0) [`position (x, y, z, 1.); + `ambient (1., 1., 1., 1.); + `diffuse (1., 1., 1., 1.)] ; + GlMat.pop () + end ; + + self#set_status + + method set_name n = name <- n; self#display_func + method set_show_name set = show_name <- set; self#display_func + + method display = fun o -> + GlList.call (self#get_object o).o_compiled; Gl.flush (); area#swap_buffers () + (* force l'affichage (apres ajout d'un objet par exemple) *) + method display_func = + do_msg "Display 3D" ; + self#make_current ; + (* Efface l'ecran *) + GlClear.color back_color ~alpha:0.0; GlClear.clear [`color;`depth]; + (* Passage des objets *) + List.iter (fun o -> if o.o_show then GlList.call o.o_compiled) objects ; + + let aspect = float width /. float height in + + (* Affichage du nom si c'est demande *) + if fonts_available && name<>"" && show_name then begin + GlMat.push (); + GlMat.load_identity (); + set_color (1., 1., 1.) ; + GlPix.raster_pos ~x:(-.4.*.aspect) ~y:(4.) ~z:(-.10.0) () ; + Gtkgl_Hack.gl_print_string fontbase name ; + GlMat.pop () + end ; + + (* Affichage de la rosace si necessaire *) + if show_rosace then begin + (* Creation de la rosace si ca n'est pas deja fait... *) + let r = match rosace with None -> self#create_rosace|Some r -> r in + GlMat.push (); + GlMat.load_identity (); + GlMat.translate3 (-.0.5*.aspect, 0.5, -.1.5) ; + rotate_some rot [X_AXIS; Z_AXIS] ; + GlList.call r ; + GlMat.pop () + end ; + + (* Affichage de la sphere representant la lumiere *) + if show_light then begin + GlMat.push (); + rotate_all lightrot ; + GlMat.translate3 lightpos; + set_color glcolor_white ; + let radius = 0.03 in + GluQuadric.sphere ~radius ~stacks:5 ~slices:5 (); + GlMat.pop () + end ; + + (* Affichage *) + Gl.flush (); area#swap_buffers () + + + (* [mouse_press ev] traite un evenement correspondant a l'appui sur un bouton *) + method private mouse_press ev = + let mouse_pos = Gtk_tools.get_mouse_pos_click ev in + match (Gtk_tools.test_mouse_but ev) with + Gtk_tools.B_GAUCHE -> + self#set_cursor cursor_rotate ; + current_action <- (ACTION_ROTATE mouse_pos); true + | Gtk_tools.B_MILIEU -> + current_action <- (ACTION_ZOOM mouse_pos); true + | Gtk_tools.B_DROIT -> false + | _ -> false + + (* [mouse_move ev] traite les mouvements souris *) + method private mouse_move ev = + area#misc#grab_focus () ; + let mouse_pos = Gtk_tools.get_mouse_pos_move ev in + match current_action with + ACTION_NONE -> false + | ACTION_ZOOM old_pos -> + let dy = (snd mouse_pos)-(snd old_pos) in + self#set_cursor (if dy<0 then cursor_zoom_up else cursor_zoom_down) ; + self#incr_dist (dist_incr*.(float_of_int dy)) ; + current_action <- (ACTION_ZOOM mouse_pos) ; true + | ACTION_ROTATE old_pos -> + let dz = (fst mouse_pos)-(fst old_pos) + and dx = (snd mouse_pos)-(snd old_pos) in + let r = add_coord_360 rot X_AXIS (float_of_int dx) in + self#rotate_view (add_coord_360 r Z_AXIS (float_of_int dz)) ; + current_action <- (ACTION_ROTATE mouse_pos) ; true + + (* [mouse_release ev] traite un evenement de relachement de bouton *) + method private mouse_release ev = + (match current_action with ACTION_NONE -> () | _ -> self#reset_cursor) ; + current_action <- ACTION_NONE ; + true + + (* Mouvement de la molette de la souris sous Windows *) + method private mouse_wheel ev = + match GdkEvent.Scroll.direction ev with + `UP -> self#incr_dist (-.dist_incr) ; true + | `DOWN -> self#incr_dist dist_incr ; true + | `LEFT -> false + | `RIGHT -> false + + method private start_stop_animation = + match animation_timer with + None -> + let timeout _ = self#incr_rotz rot_anim; true in + animation_timer <- Some (Timeout.add ~ms:tps_anim ~callback:timeout) + | Some t -> Timeout.remove t ; animation_timer <- None + + (* [key_pressed key] teste la touche pressee dans la zone de dessin + et effectue l'action associee le cas echeant *) + method private key_pressed key = + let keys_list = + [([_Page_Up], fun () -> self#incr_dist (-.dist_incr)) ; + ([_Page_Down], fun () -> self#incr_dist dist_incr) ; + ([_KP_Down; _KP_2], fun () -> self#incr_rotx rot_incr) ; + ([_KP_Up; _KP_8], fun () -> self#incr_rotx (-.rot_incr)) ; + ([_KP_Left; _KP_4], fun () -> self#incr_rotz (-.rot_incr)) ; + ([_KP_Right; _KP_6], fun () -> self#incr_rotz rot_incr) ; + ([_Down], fun () -> self#move_y (dist_incr/.3.)) ; + ([_Up], fun () -> self#move_y ((-.dist_incr)/.3.)) ; + ([_Left], fun () -> self#move_x (dist_incr/.3.)) ; + ([_Right], fun () -> self#move_x ((-.dist_incr)/.3.)) ; + ([_l], fun () -> self#lights_switch) ; + ([_s], fun () -> self#smooth_switch) ; + ([_r], fun () -> self#rosace_switch) ; + ([_Home], fun () -> self#rotate_view pt_null) ; + ([_a], fun () -> self#incr_light_rot rot_incr) ; + ([_z], fun () -> self#incr_light_rot (-.rot_incr)) ; + ([_L], fun () -> show_light <- not show_light; self#change_and_redraw) ; + ([_F12], fun () -> self#screenshot screenshot_format + (screenshot_path^screenshot_name)); + ([_space], fun () -> self#start_stop_animation); + ([_Escape], fun () -> self#redo_all); + ([_i], fun () -> Printf.printf "%s" (get_gl_infos ()); flush stdout); + ([_n], fun () -> self#set_show_name (not show_name))] in + (* Recherche la fonction associee a la touche presse s'il y en a une *) + let rec check_keys lst = + match lst with + (keys, func)::reste -> + if List.mem key keys then begin func ();true end else check_keys reste + | [] -> false + in + check_keys keys_list + + (* Modification de la position de l'utilisateur et des angles de vue *) + method incr_dist d = self#move_view (add_coord depl Z_AXIS d) + method incr_rotx d = self#rotate_view (add_coord_360 rot X_AXIS d) + method incr_rotz d = self#rotate_view (add_coord_360 rot Z_AXIS d) + + method move_x d = self#move_view (add_coord depl X_AXIS d) + method move_y d = self#move_view (add_coord depl Y_AXIS d) + + (* tourne la lumiere *) + method incr_light_rot d = lightrot<-add_coord_360 lightrot Z_AXIS d; + self#change_and_redraw + + (* [set_screenshot def_format def_path def_filename] met a jour les + parametres de capture d'ecran *) + method set_screenshot def_format def_path def_filename = + screenshot_path <- def_path; screenshot_name <- def_filename; + screenshot_format <- def_format + + (* [screenshot format filename] ouvre une boite de capture ecran *) + method screenshot format filename = + Gtk_tools.screenshot_box filename format area#misc#window + None 0 0 width height + + (* Manipulations generales des objets *) + method private get_object id = + try List.find (fun o -> o.o_id=id) objects + with Not_found -> raise (NO_SUCH_3D_OBJECT id) + method delete_object id = + let found = ref None in + let new_l = List.fold_left (fun l obj -> + if obj.o_id=id then begin found:=Some obj; l end else obj::l) [] objects in + match !found with + None -> raise (NO_SUCH_3D_OBJECT id) + | Some obj -> objects <- List.rev new_l; GlList.delete obj.o_compiled + method object_set_color id new_color = + let o = self#get_object id in + set_object_color o.o_obj new_color; self#make_and_compile o + method object_get_color id = + get_object_color (self#get_object id).o_obj + method object_set_visibility id visible = + let o = self#get_object id in o.o_show <- visible; self#make_and_compile o + method object_get_visibility id = (self#get_object id).o_show + method object_set_fill id filled = + let o = self#get_object id in + set_object_fill o.o_obj filled; self#make_and_compile o + method object_get_fill id = + get_object_fill (self#get_object id).o_obj + + (* Fonctions specifiques a certains objets *) + method line_get_width id = + get_line_width (self#get_object id).o_obj id + method line_set_width id width = + let o = self#get_object id in + set_line_width o.o_obj width id; self#make_and_compile o + method line_get_with_bars id = + get_line_bars (self#get_object id).o_obj id + method line_set_with_bars id withbars = + let o = self#get_object id in + set_line_bars o.o_obj withbars id; self#make_and_compile o + method outline_set_in_color id new_color = + let o = self#get_object id in + set_outline_in_color o.o_obj new_color id; self#make_and_compile o + method outline_get_in_color id = + get_outline_in_color (self#get_object id).o_obj id + method point_get_with_name id = + get_point_name (self#get_object id).o_obj id + method point_set_with_name id withname = + let o = self#get_object id in + set_point_name o.o_obj withname id; self#make_and_compile o + + method destroy_all_objects = + List.iter (fun o -> GlList.delete o.o_compiled) objects ; + objects <- [] ; + (match rosace with None -> () | Some r -> GlList.delete r) ; + unload_bitmap_font fontbase + + method private redo_all = + self#setup; self#recompile_all_objects; + (match rosace with None -> () | Some r -> GlList.delete r) ; + if show_rosace then ignore(self#create_rosace) ; + self#display_func + + method set_back_color c = back_color <- c ; self#display_func + + initializer + ignore(area#connect#realize ~callback:self#init_func) ; + ignore(area#connect#display ~callback:(fun () -> self#display_func)) ; + ignore(area#connect#reshape ~callback:self#reshape_func) ; + area#misc#realize (); + + incr nb_objects ; + + (* Indispensable pour que lorsque l'on entre dans la fenetre, les *) + (* prochaines commandes OpenGL soient bien appliquees dans la vue *) + (* correspondante dans le cas ou plusieurs vues ont ete ouvertes *) + ignore(area#event#connect#focus_in (fun _ -> self#make_current; false)) ; + + (* Callbacks des evenements souris *) + Gtk_tools_GL.glarea_mouse_connect area + self#mouse_press self#mouse_move self#mouse_release ; + + let scroll_cb = fun ev -> + match GdkEvent.get_type ev with + | `SCROLL -> self#mouse_wheel (GdkEvent.Scroll.cast ev) + | _ -> false in + + (* Reactions aux mouvements de la molette souris *) + ignore(area#event#connect#any ~callback:scroll_cb) ; + + (* Attachement des callbacks pour les evenements clavier *) + Gtk_tools_GL.glarea_key_connect area self#key_pressed (fun k -> (); false) + end + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/gtk_3d.mli b/sw/lib/ocaml/gtk_3d.mli new file mode 100644 index 0000000000..64ebc45799 --- /dev/null +++ b/sw/lib/ocaml/gtk_3d.mli @@ -0,0 +1,252 @@ +(* + * $Id$ + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(** 3D display widget + + {b Dépendences : Platform, Gtk_tools_GL, Geometry_2d, Geometry_3d} + *) + +(** {6 Exceptions} *) + +exception NO_SUCH_3D_OBJECT of int +(** Exception levée lors de la recherche d'un objet d'identifiant inconnu *) + +exception NOT_A_3D_OUTLINE of int +(** Exception levée lorsqu'un autre objet est passé alors qu'un contour + pays est attendu *) + +exception NOT_A_3D_LINE of int +(** Idem avec une ligne *) + +exception NOT_A_3D_POINT of int +(** Idem avec un point *) + +type glcolor = float * float * float +(** Une couleur OpenGL *) + +type t_arrow = ARROW1 | ARROW2 +(** Types de fleche *) + +(** [create_texture_from_image texture_filename] crée une texture à partir d'un fichier image. L'identifiant + de la texture créée est renvoyé. *) +val create_texture_from_image : string -> GlTex.texture_id + +(** [widget_3d pack with_status_bar name] crée un widget d'affichage 3D + + - [pack] indique où mettre le widget + - [with_status_bar] permet la création d'une barre d'infos optionnelle + - [name] désigne le nom à donner à la zone d'affichage (éventuellement affichée + en haut à gauche de la zone) +*) +class widget_3d : + (GObj.widget -> unit) -> + bool -> + string -> + object + + (** {6 Gestion de l'affichage} *) + + method display_func : unit + (** force l'affichage (utile après un ajout ou une modification d'objet) *) + method change_and_redraw : unit + (** prend en compte des modifications sur la position utilisateur (rotation + de la scène, éloignement) et force l'affichage *) + method incr_dist : float -> unit + (** [incr_dist delta] effectue une modification sur l'éloignement de + l'utilisateur par rapport à la scène *) + method incr_rotx : float -> unit + (** [incr_rotx delta_angle] tourne la scène de [delta_angle] (en degrés) + suivant l'axe X *) + method incr_rotz : float -> unit + (** [incr_rotz delta_angle] tourne la scène de [delta_angle] (en degrés) + suivant l'axe Z *) + + method move_x : float -> unit + (** [move_x d] déplacement de la vue sur l'axe X *) + + method move_y : float -> unit + (** [move_y d] déplacement de la vue sur l'axe Y *) + + method move_view : Gl.point3 -> unit + (** [move_view point] place l'utilisateur à la position indiquée par [point] *) + method rotate_view : float * float * float -> unit + (** [rotate_view (angle_x, angle_y, angle_z)] effectue les rotations de la scène + indiquées pour chaque axe *) + + method incr_light_rot : float -> unit + (** [incr_light_rot delta_angle] ajoute [delta_angle] à la rotation sur l'axe Z + de la source de lumière *) + + method set_name : string -> unit + (** [set_name name] modifie le nom associé au widget 3D *) + method set_show_name : bool -> unit + (** [set_show_name show] affiche/masque le nom associé au widget 3D *) + + method set_back_color : glcolor -> unit + (** [set_back_color couleur] met à jour la couleur de fond de la zone de dessin 3D *) + + (** {6 Rosace} *) + + method rosace_off : unit + (** masque la rosace *) + method rosace_on : unit + (** affiche la rosace *) + method rosace_switch : unit + (** change l'état d'affichage de la rosace suivant la valeur de [show_rosace] *) + + (** {6 Ajout/suppression d'objets} *) + + method destroy_all_objects : unit + (** Destruction de tous les objets existants *) + + method add_object_arrow : + Geometry_3d.pt_3D -> Geometry_3d.pt_3D -> bool -> float -> float -> glcolor -> + bool -> t_arrow -> int + (** [add_object_arrow ptA ptB sens ep lg color filled arrow_type] crée une fleche : + - [ptA] indique le point où se trouve la pointe de la fleche ou la + seconde extrémité + - [ptB] second point qui donne la direction de la fleche + - [sens] est vrai si [ptA] désigne la pointe de la fleche, faut s'il désigne + l'autre extrémité de la fleche + - [ep] épaisseur de la flèche + - [lg] sa longueur + - [color] la couleur + - [filled] indique si la fleche est remplie ou en fil de fer + - [arrow_type] désigne le type de la fleche + + L'identifiant de l'objet créé est renvoyé + *) + method add_object_line : + Geometry_3d.pt_3D list -> glcolor -> int -> bool -> bool -> int + (** [add_object_line lst_points color line_width with_bars fill] crée un objet + [LINE_3D] où + + - [lst_points] indique la liste des points de la ligne + - [color] désigne sa couleur + - [line_width] épaisseur + - [with_bars] indique si la ligne est affichée avec des barres verticales + - [fill] indique si la surface définie par la ligne et sa projection au + sol est affichée + + L'identifiant de l'objet créé est renvoyé + *) + + method add_object_outline : + Geometry_3d.pt_3D list -> glcolor -> glcolor -> bool -> int + (** [add_object_outline contour color_in color_out filled] crée un objet + [OUTLINE_3D] et renvoie son identifiant *) + method add_object_point : + Geometry_3d.pt_3D -> Geometry_3d.pt_3D -> string -> glcolor -> bool -> int + (** [add_object_point pos pos2 name color with_name] crée un [POINT_3D] avec : + - [pos] indique la position 3D du point + - [pos2] indique la position 3D où doit etre affiché son nom + - [name] le nom associé au point + - [color] la couleur du point + - [with_name] indique si le nom du point doit etre affiché + + L'identifiant de l'objet créé est renvoyé + *) + method add_object_volume_simple : + Geometry_2d.pt_2D list -> float -> float -> glcolor -> bool -> int + (** [add_object_volume_simple contour zmin zmax color filled] crée un objet + [VOLUME_3D] où : + + - [contour] désigne la surface (2D) définissant les faces inférieure + et supérieure du volume + - [zmin] et [zmax] indiquent repsectivement l'altitude min et l'altitude max + du volume + - [color] désigne la couleur à lui appliquer + - [filled] permet d'afficher le volume plein ou en mode fil de fer + *) + + method add_object_enveloppe : + Geometry_3d.pt_3D list -> + Geometry_3d.pt_3D list -> glcolor -> bool -> int + (** [add_object_enveloppe contour_haut contour_bas color filled] *) + + method add_object_enveloppe_double : + Geometry_3d.pt_3D list -> + Geometry_3d.pt_3D list -> glcolor -> glcolor -> bool -> int + (** [add_object_enveloppe_double contour_haut contour_bas + color_out color_in filled] *) + + method add_object_surface : (Geometry_3d.pt_3D*glcolor) array array -> bool -> int + (** [add_object_surface points filled] ajoute une surface, pleine ou pas suivant + [filled]. Elle est définie par la matrice de point [points] qui contient les + coordonnées et la couleur de chaque point de la grille *) + + method add_object_surface_with_texture : Geometry_3d.pt_3D array array -> GlTex.texture_id -> int + (** [add_object_surface_with_texture points texture_id] effectue la même opération + que la fonction précédente sauf qu'ici, on ne précise pas de couleur pour les + différents points mais une texture à appliquer sur la surface obtenue. *) + + method delete_object : int -> unit + (** [delete_object id] supprime l'objet dont l'identifiant est [id]. Si cet + objet n'existe pas, l'exception {!Gtk_3d.NO_SUCH_3D_OBJECT} est levée *) + + (** {6 Manipulation des objets} *) + + method display : int -> unit + method object_get_color : int -> glcolor + method object_get_fill : int -> bool + method object_get_visibility : int -> bool + method object_set_color : int -> glcolor -> unit + method object_set_fill : int -> bool -> unit + method object_set_visibility : int -> bool -> unit + method outline_get_in_color : int -> glcolor + method outline_set_in_color : int -> glcolor -> unit + method point_get_with_name : int -> bool + method point_set_with_name : int -> bool -> unit + method line_get_width : int -> int + method line_get_with_bars : int -> bool + method line_set_width : int -> int -> unit + method line_set_with_bars : int -> bool -> unit + + (** {6 Lighting/Smoothing} *) + + method lights_off : unit + (** annule l'utilisation de la lumière *) + method lights_on : unit + (** force l'utilisation de la lumière *) + method lights_switch : unit + (** change l'état d'utilisation de la lumière *) + + method smooth_off : unit + (** annule l'utilisation du lissage *) + method smooth_on : unit + (** force l'utilisation du lissage *) + method smooth_switch : unit + (** change l'état d'utilisation du lissage *) + + + (** {6 Captures écran} *) + + method set_screenshot : + Gtk_image.format_capture -> string -> string -> unit + (** [set_screenshot def_format def_path def_filename] met à jour les + paramètres de capture d'écran *) + + method screenshot : Gtk_image.format_capture -> string -> unit + (** [screenshot format filename] ouvre une boite de capture écran *) + end diff --git a/sw/lib/ocaml/gtk_draw.ml b/sw/lib/ocaml/gtk_draw.ml new file mode 100644 index 0000000000..ae53980752 --- /dev/null +++ b/sw/lib/ocaml/gtk_draw.ml @@ -0,0 +1,210 @@ +(* + * $Id$ + * + * GTK drawing in a pixmap + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + + +(* ============================================================================= *) +(* = Fonction de casting, indispensable pour pouvoir utiliser n'importe quel = *) +(* = type de drawable ('window, 'pixmap) pour dessiner = *) +(* ============================================================================= *) +let gd_do_cast p = (p:GDraw.drawable) + +(* ============================================================================= *) +(* = GDraw.color -> (r, g, b) en entiers = *) +(* ============================================================================= *) +let gd_rgb_of_color color = + let t = GDraw.color color in + (Gdk.Color.red t, Gdk.Color.green t, Gdk.Color.blue t) + +(* ============================================================================= *) +(* = GDraw.color -> (r, g, b) en flottants = *) +(* ============================================================================= *) +let gd_float_rgb_of_color color = + let (r, g, b) = gd_rgb_of_color color in + (float_of_int r, float_of_int g, float_of_int b) + +(* ============================================================================= *) +(* = Creation d'une GDraw.color a partir de ces composantes (r, g, b) = *) +(* ============================================================================= *) +let gd_color_of_rgb (r, g, b) = `RGB (r, g, b) + +(* ============================================================================= *) +(* = Creation d'une GDraw.color a partir de ces composantes (r, g, b) = *) +(* ============================================================================= *) +let gd_color_of_float_rgb (r, g, b) = `RGB (int_of_float r, int_of_float g, + int_of_float b) +(* ============================================================================= *) +(* = Mise a jour de la couleur de dessin = *) +(* ============================================================================= *) +let gd_set_color p color = + (gd_do_cast p)#set_foreground color + +(* ============================================================================= *) +(* = Mise a jour de la couleur de dessin a n% de la valeur indiquee = *) +(* ============================================================================= *) +let gd_set_faded_color p color pct = + let pct = (float_of_int pct)/.100. and (r, g, b) = gd_float_rgb_of_color color in + (gd_do_cast p)#set_foreground (gd_color_of_float_rgb (r*.pct, g*.pct, b*.pct)) + +(* ============================================================================= *) +(* = Modification du style de trace = *) +(* ============================================================================= *) +(* On est oblige de preciser width, alors qu'il ne doit normalement pas servir *) +(* Si on ne le met pas, les pointilles n'apparaissent pas partout !!! *) +let gd_set_style_solid p = + (gd_do_cast p)#set_line_attributes ~style:`SOLID ~width:1 () +let gd_set_style_dash p = + (gd_do_cast p)#set_line_attributes ~style:`ON_OFF_DASH ~width:1 () +let gd_set_style_double_dash p = + (gd_do_cast p)#set_line_attributes ~style:`DOUBLE_DASH ~width:1 () + +(* ============================================================================= *) +(* = Modification du mode de trace = *) +(* ============================================================================= *) +(*let gd_set_mode_xor p = (gd_do_cast p)#set_gc_xor +let gd_set_mode_std p = (gd_do_cast p)#set_gc_copy*) + +(* ============================================================================= *) +(* = Modification de l'epaisseur du trace = *) +(* ============================================================================= *) +let gd_set_line_width p width = (gd_do_cast p)#set_line_attributes ~width:width () + +(* ============================================================================= *) +(* = Centrage d'un texte a afficher = *) +(* ============================================================================= *) +let gd_center_text (x, y) string font = + let (w, h) = Gtk_tools.string_width_height font string in + (x-w/2, y+h/2) + +(* ============================================================================= *) +(* = Dessin d'un texte non centre = *) +(* ============================================================================= *) +let gd_draw_non_centered_text p (x, y) text font = + (gd_do_cast p)#string text ~font:font ~x:x ~y:y + +(* ============================================================================= *) +(* = Dessin d'un texte centre = *) +(* ============================================================================= *) +let gd_draw_text p pos_text text font = + let (x, y) = gd_center_text pos_text text font in + (gd_do_cast p)#string text ~font:font ~x:x ~y:y + +(* ============================================================================= *) +(* = Dessin d'un segment = *) +(* ============================================================================= *) +let gd_draw_segment p (x1, y1) (x2, y2) = + (gd_do_cast p)#line ~x:x1 ~y:y1 ~x:x2 ~y:y2 + +(* ============================================================================= *) +(* = Dessin d'un pixel = *) +(* ============================================================================= *) +let gd_draw_point p (x, y) = (gd_do_cast p)#point ~x:x ~y:y + +(* ============================================================================= *) +(* = Dessin d'une ligne = *) +(* ============================================================================= *) +let gd_draw_line p pts = (gd_do_cast p)#lines pts + +(* ============================================================================= *) +(* = Dessin d'un polygone = *) +(* ============================================================================= *) +let gd_draw_polygon p pts = (gd_do_cast p)#polygon ~filled:false pts + +(* ============================================================================= *) +(* = Dessin d'un polygone plein = *) +(* ============================================================================= *) +let gd_draw_filled_polygon p pts = (gd_do_cast p)#polygon ~filled:true pts + +(* ============================================================================= *) +(* = Dessin d'un cercle = *) +(* ============================================================================= *) +let gd_draw_circle p (x, y) r = + (gd_do_cast p)#arc ~filled:false ~x:(x-r) ~y:(y-r) ~width:(2*r) ~height:(2*r) () + +(* ============================================================================= *) +(* = Dessin d'un cercle plein = *) +(* ============================================================================= *) +let gd_draw_filled_circle p (x, y) r = + (gd_do_cast p)#arc ~filled:true ~x:(x-r) ~y:(y-r) ~width:(2*r) ~height:(2*r) () + +(* ============================================================================= *) +(* = Dessin d'un rectangle = *) +(* ============================================================================= *) +let gd_draw_rect p (x1, y1, x2, y2) = + (gd_do_cast p)#rectangle ~filled:false ~x:x1 ~y:y1 + ~width:(x2-x1) ~height:(y2-y1) () + +(* ============================================================================= *) +(* = Dessin d'un rectangle plein = *) +(* ============================================================================= *) +let gd_draw_filled_rect p (x1, y1, x2, y2) = + (gd_do_cast p)#rectangle ~filled:true ~x:x1 ~y:y1 ~width:(x2-x1) ~height:(y2-y1) () + +(* ============================================================================= *) +(* = Dessin d'un triangle = *) +(* ============================================================================= *) +let gd_draw_triangle p (x, y) size = + let size0 = int_of_float ((float_of_int size) *. 1.5) and + size1 = int_of_float ((float_of_int size) *. 0.5) in + (gd_do_cast p)#polygon ~filled:false + [(x, y-size); (x-size0, y+size1); (x+size0, y+size1)] + +(* ============================================================================= *) +(* = Dessin d'un triangle plein = *) +(* ============================================================================= *) +let gd_draw_filled_triangle p (x, y) size = + let size0 = int_of_float ((float_of_int size) *. 1.5) and + size1 = int_of_float ((float_of_int size) *. 0.5) in + (gd_do_cast p)#polygon ~filled:true + [(x, y-size); (x-size0, y+size1); (x+size0, y+size1)] + +(* ============================================================================= *) +(* = Efface une pixmap = *) +(* ============================================================================= *) +let gd_clear p color width height = + gd_set_color p color ; + gd_draw_filled_rect (gd_do_cast p) (0, 0, width, height) + +(* ============================================================================= *) +(* = Place le fond statique dans la pixmap de dessin = *) +(* ============================================================================= *) +let gd_set_background_pixmap p dest = (gd_do_cast dest)#put_pixmap ~x:0 ~y:0 p + +(* ============================================================================= *) +(* = Place une pixmap transparente dans le dessin = *) +(* ============================================================================= *) +let gd_put_transp_pixmap p dest x y = + (* Indispensable d'utiliser le masque pour la transparence *) + (match p#mask with + None -> () | + Some m -> (gd_do_cast dest)#set_clip_origin ~x:x ~y:y; dest#set_clip_mask m) ; + + (* Mise en place du pixmap transparent *) + dest#put_pixmap ~x:x ~y:y p#pixmap ; + + (* On enleve le masque *) + (match p#mask with None -> () | Some m -> prerr_endline "TODO (Gtk_draw): dest#unset_clip_mask") + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/gtk_draw.mli b/sw/lib/ocaml/gtk_draw.mli new file mode 100644 index 0000000000..24e8f1af89 --- /dev/null +++ b/sw/lib/ocaml/gtk_draw.mli @@ -0,0 +1,137 @@ +(** Module de dessin GTK + + {b Dépendences : Gtk_Tools} + *) + + +(** {6 Fonctions utilitaires} *) + + +(** [gd_do_cast drawable] : fonction de casting d'un drawable ('window ou 'pixmap) + pour pouvoir y dessiner *) +val gd_do_cast : GDraw.drawable -> GDraw.drawable + + +(** {6 Divers} *) + + +(** [gd_clear drawable color width height] efface une pixmap avec la couleur + [color] en dessinant un rectangle plein de taille [width]*[height] *) +val gd_clear : GDraw.drawable -> GDraw.color -> int -> int -> unit + +(** [gd_set_background_pixmap pixmap drawable] place la [pixmap] en fond de la + zone de dessin [drawable] *) +val gd_set_background_pixmap : Gdk.pixmap -> GDraw.drawable -> unit + +(** [gd_set_background_pixmap pixmap drawable x y] place la [pixmap] dans la zone + de dessin à la position [(x, y)]. Cette pixmap est transparente *) +val gd_put_transp_pixmap : GDraw.pixmap -> GDraw.drawable -> int -> int -> unit + +(** {6 Transformations de couleurs} *) + + +(** [gd_rgb_of_color color] renvoie un triplet (r, g, b) donnant les composantes + entières rouge, verte et bleue de [color]. Les composantes RGB sont dans + l'intervalle [\[0, 65535\]] *) +val gd_rgb_of_color : GDraw.color -> int * int * int + +(** [gd_float_rgb_of_color color] renvoie un triplet (r, g, b) donnant les + composantes flottantes rouge, verte et bleue de [color] *) +val gd_float_rgb_of_color : GDraw.color -> float * float * float + +(** [gd_color_of_rgb (r, g, b)] crée une [GDraw.color] à partir des composantes + entières *) +val gd_color_of_rgb : 'a * 'b * 'c -> [> `RGB of 'a * 'b * 'c] + +(** [gd_color_of_float_rgb (r, g, b)] crée une [GDraw.color] à partir des + composantes flottantes *) +val gd_color_of_float_rgb : + float * float * float -> [> `RGB of int * int * int] + + +(** {6 Modification du tracé} *) + + +(** [gd_set_color drawable color] met à jour la couleur de dessin *) +val gd_set_color : GDraw.drawable -> GDraw.color -> unit + +(** [gd_set_faded_color drawable color pct] met à jour la couleur de dessin avec + une couleur à [pct]% de [color] *) +val gd_set_faded_color : GDraw.drawable -> GDraw.color -> int -> unit + +(** [gd_set_style_solid drawable] met le style de dessin normal *) +val gd_set_style_solid : GDraw.drawable -> unit + +(** [gd_set_style_dash drawable] met le style de dessin pointillés *) +val gd_set_style_dash : GDraw.drawable -> unit + +(** [gd_set_style_double_dash drawable] met le style de dessin en doubles + pointillés *) +val gd_set_style_double_dash : GDraw.drawable -> unit + +(** [gd_set_mode_xor drawable] fixe le mode dessin en XOR *) +(*val gd_set_mode_xor : GDraw.drawable -> unit*) + +(** [gd_set_mode_std drawable] remet le dessin en mode normal *) +(*val gd_set_mode_std : GDraw.drawable -> unit*) + +(** [gd_set_line_width drawable width] fixe l'épaisseur des lignes *) +val gd_set_line_width : GDraw.drawable -> int -> unit + + +(** {6 Texte} *) + + +(** [gd_center_text (x, y) chaine fonte] centre le texte contenu dans [chaine] + a la position [(x, y)] *) +val gd_center_text : int * int -> string -> Gdk.font -> int * int + +(** [gd_draw_non_centered_text drawable (x, y) chaine fonte] dessine le texte + contenu dans [chaine] à la position [(x, y)] sans le centrer *) +val gd_draw_non_centered_text : + GDraw.drawable -> int * int -> string -> Gdk.font -> unit + +(** [gd_draw_text drawable (x, y) chaine fonte] dessine le texte + contenu dans [chaine] centré sur la position [(x, y)] *) +val gd_draw_text : + GDraw.drawable -> int * int -> string -> Gdk.font -> unit + + +(** {6 Formes géométriques} *) + + +(** [gd_draw_point drawable (x, y)] dessine un point à la position indiquée *) +val gd_draw_point : GDraw.drawable -> int * int -> unit + +(** [gd_draw_segment drawable (x1, y1) (x2, y2)] dessine un segment entre les + points [(x1, y1)] et [(x2, y2)] *) +val gd_draw_segment : GDraw.drawable -> int * int -> int * int -> unit + +(** [gd_draw_line drawable liste_points] dessine une ligne *) +val gd_draw_line : GDraw.drawable -> (int * int) list -> unit + +(** [gd_draw_polygon drawable liste_points] dessine un polygone non rempli *) +val gd_draw_polygon : GDraw.drawable -> (int * int) list -> unit + +(** [gd_draw_filled_polygon drawable liste_points] dessine un polygone rempli *) +val gd_draw_filled_polygon : GDraw.drawable -> (int * int) list -> unit + +(** [gd_draw_circle drawable (x, y) rayon] dessine un cercle non rempli *) +val gd_draw_circle : GDraw.drawable -> int * int -> int -> unit + +(** [gd_draw_filled_circle drawable (x, y) rayon] dessine un cercle rempli *) +val gd_draw_filled_circle : GDraw.drawable -> int * int -> int -> unit + +(** [gd_draw_rect drawable (x1, y1, x2, y2)] dessine un rectangle non rempli + dont les points extrèmes sont [(x1, y1)] et [(x2, y2)] *) +val gd_draw_rect : GDraw.drawable -> int * int * int * int -> unit + +(** [gd_draw_filled_rect drawable (x1, y1, x2, y2)] dessine un rectangle rempli + dont les points extrèmes sont [(x1, y1)] et [(x2, y2)] *) +val gd_draw_filled_rect : GDraw.drawable -> int * int * int * int -> unit + +(** [gd_draw_triangle drawable (x, y) size] dessine un triangle non rempli *) +val gd_draw_triangle : GDraw.drawable -> int * int -> int -> unit + +(** [gd_draw_filled_triangle drawable (x, y) size] dessine un triangle rempli *) +val gd_draw_filled_triangle : GDraw.drawable -> int * int -> int -> unit diff --git a/sw/lib/ocaml/gtk_image.ml b/sw/lib/ocaml/gtk_image.ml new file mode 100644 index 0000000000..922fa8016f --- /dev/null +++ b/sw/lib/ocaml/gtk_image.ml @@ -0,0 +1,350 @@ +(* + * $Id$ + * + * Images utils + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(* Modules CamlImages *) +open Images +open Png +open Gif +open Jpeg +open Xpm +open Tiff + +open Platform + +(* Formats de sauvegarde pris en compte *) +type format_capture = PNG | GIF | JPEG | TIFF | BMP | PPM | POSTSCRIPT + +(* Formats disponibles suivant qu'on est sous Unix/Windows *) +let formats_capture_dispos = + if platform_is_unix then [PNG; GIF; JPEG; TIFF; BMP; PPM; POSTSCRIPT] + else [BMP; TIFF; PPM] (* Windows *) + +(* Indique si le format typ est disponible *) +let is_format_capture_dispo typ = + try ignore(List.find (fun t -> t=typ) formats_capture_dispos) ; true + with Not_found -> false + +(* Etats lors de la sauvegarde *) +type progress_save = INIT | SAVING | FINISHED + +(* [extended_string_of_format_capture format] fournit une chaine correspondant + au format : [PNG] -> "PNG"; [POSTSCRIPT] -> "Postscript" *) +let extended_string_of_format_capture format = + match format with + PNG -> "PNG" + | GIF -> "GIF" + | JPEG -> "JPEG" + | TIFF -> "TIFF" + | BMP -> "BMP" + | PPM -> "PPM" + | POSTSCRIPT -> "Postscript" + +(* [string_of_format_capture format] fournit une chaine correspondant + au format : [PNG] -> "PNG"; [POSTSCRIPT] -> "PS" *) +let string_of_format_capture format = + match format with + PNG -> "PNG" + | GIF -> "GIF" + | JPEG -> "JPG" + | TIFF -> "TIFF" + | BMP -> "BMP" + | PPM -> "PPM" + | POSTSCRIPT -> "PS" + +(* [format_capture_of_string chaine] renvoie le type {!Capture.format_capture} + correspondant a la chaine *) +let format_capture_of_string s = + match s with + "PNG" -> PNG + | "GIF" -> GIF + | "JPG" -> JPEG + | "TIFF" -> TIFF + | "BMP" -> BMP + | "PPM" -> PPM + | "PS" -> POSTSCRIPT + | _ -> PNG + +(* [string_of_extension format] renvoie l'extension correspondant au format. + i.e [PNG] -> ".png" *) +let string_of_extension format = + match format with + PNG -> ".png" + | GIF -> ".gif" + | JPEG -> ".jpg" + | TIFF -> ".tiff" + | BMP -> ".bmp" + | PPM -> ".ppm" + | POSTSCRIPT -> ".ps" + +(* ============================================================================= *) +(* = Transforme un entier en valeur (R,G,B) = *) +(* ============================================================================= *) +(* [rgb_of_int entier] transforme un entier en valeur (r, g, b) *) +let rgb_of_int v = + let r = v/65536 in + let reste = v-(r*65536) in + let g = reste/256 and + b = reste mod 256 in + {r=r; g=g; b=b} + +(* ============================================================================= *) +(* = Transforme une Gdk.Image en Image = *) +(* ============================================================================= *) +(* [image_of_gdkimage gdkimge largeur hauteur progress_func] transforme une + [Gdk.Image] en [Image] *) +let image_of_gdkimage gdkimg width height progress_func = + let total = 1.0/.float_of_int (height) and cpt = ref 0.0 in + + let img = Rgb24.create width height in + for y = 0 to height-1 do + for x = 0 to width-1 do + Rgb24.set img x y (rgb_of_int (Gdk.Image.get_pixel gdkimg x y)) ; + done ; + cpt := !cpt +. 1.0 ; + + (* Appel a la fonction de progression si necessaire *) + match progress_func with + None -> () + | Some f -> f INIT (!cpt*.total) + done ; + Rgb24(img) + +(* ============================================================================= *) +(* = Recuperation de la fonction de sauvegarde correspondant au format voulu = *) +(* ============================================================================= *) +(* [get_save_func format] renvoie la fonction de sauvegarde correspondant au + format indique *) +let get_save_func format = + match format with + PNG -> Png.save + | GIF -> Gif.save_image + | JPEG -> Jpeg.save + | TIFF -> Tiff.save + | BMP -> Bmp.save + | PPM -> Ppm.save + | POSTSCRIPT -> Ps.save + +(* ============================================================================= *) +(* = Recuperation de la fonction de chargement correspondant au format voulu = *) +(* ============================================================================= *) +let get_load_func format = + match format with + PNG -> Png.load + | JPEG -> Jpeg.load + | TIFF -> Tiff.load + | BMP -> Bmp.load + | PPM -> Ppm.load + | POSTSCRIPT -> Ps.load + | GIF -> (* Cas particulier pour les images GIF *) + let save_gif filename opts = + let sequence = Gif.load filename opts in + let frame = List.hd sequence.frames in + Index8 frame.frame_bitmap + in + save_gif + +(* ============================================================================= *) +(* = Recuperation du nom avec extension pour le format voulu = *) +(* ============================================================================= *) +let set_filename_extension filename format = + let extension = string_of_extension format in + let lg = String.length extension in + (* Test si l'extension est deja presente. Si elle ne l'est pas, on l'ajoute... *) + if (String.length filename) > lg && + (String.sub filename ((String.length filename)-lg) lg) = extension then + filename + else + filename^extension + +(* ============================================================================= *) +(* = Fonction de remplacement d'une extension = *) +(* = = *) +(* = filename = nom du fichier courant = *) +(* = old_format = ancien format = *) +(* = new_format = nouveau format = *) +(* ============================================================================= *) +let update_extension_capture filename old_format new_format = + let old_ext = string_of_extension old_format in + let lg = String.length old_ext in + if (String.sub filename ((String.length filename)-lg) lg) = old_ext then begin + (* Il faut supprimer l'ancienne extension *) + let f = String.sub filename 0 ((String.length filename)-lg) in + set_filename_extension f new_format ; + end else + (* Ajout de la nouvelle extension *) + set_filename_extension filename new_format + +(* ============================================================================= *) +(* = Effectue la capture proprement dite = *) +(* = = *) +(* = drawable = la pixmap ou fenetre contenant l'image a sauver = *) +(* = x = coordonnee x du point en haut a gauche = *) +(* = y = coordonnee y du point en haut a gauche = *) +(* = width = largeur de l'image = *) +(* = height = hauteur de l'image = *) +(* = filename = nom du fichier = *) +(* = format = format de sauvegarde (de type format_capture) = *) +(* = progress_func = fonction appelee lors de la progression (float -> unit) = *) +(* ============================================================================= *) +let capture_part draw x y width height filename format progress_func = + (* Creation d'un Gdk.Image a partir d'un Drawable pour pouvoir utiliser *) + (* la fonction get *) + let gdk_image = Gdk.Image.get draw x y width height in + + (* Transformation en Image *) + let image = image_of_gdkimage gdk_image width height progress_func in + (* On n'a plus besoin de la Gdk.Image *) + Gdk.Image.destroy gdk_image ; + + (* Sauvegarde de l'Image dans un fichier au format voulu *) + let save_func = get_save_func format in + + (* Ajout de l'extension appropriee si necessaire au nom de fichier *) + let filename_save = set_filename_extension filename format in + + (* Appel a la fonction de progression si necessaire *) + begin + match progress_func with + None -> save_func filename_save [] image + | Some f -> + save_func filename_save [Save_Progress(f SAVING)] image ; + (* Fin de la sauvegarde *) + f FINISHED 0.0 + end + +(* ============================================================================= *) +(* = Fonction principale de capture a partir d'un bout de pixmap = *) +(* = = *) +(* = window = la fenetre mere du drawable suivant = *) +(* = drawable = la pixmap ou fenetre contenant l'image a sauver = *) +(* = x = coordonnee x du point en haut a gauche = *) +(* = y = coordonnee y du point en haut a gauche = *) +(* = width = largeur de l'image = *) +(* = height = hauteur de l'image = *) +(* = filename = nom du fichier = *) +(* = format = format de sauvegarde (de type format_capture) = *) +(* = progress_func = fonction appelee lors de la progression (float -> unit) = *) +(* = caption = None ou Some(texte legende, couleur) = *) +(* ============================================================================= *) +let capture_part_with_caption window drawable x y width height filename + format progress_func caption = + let draw = + match caption with + None -> drawable (* Pas de legende *) + | Some (caption_text, caption_color, contour_color, back_color, font) -> + (* Copie de la pixmap initiale pour pouvoir y rajouter la legende *) + let depth = window#misc#visual_depth and w = window#misc#window in + let pix = Gdk.Pixmap.create ~window:w ~width:(width+x) ~height:(height+y) + ~depth:depth () in + let pixmap = new GDraw.pixmap pix in + pixmap#put_pixmap ~x:x ~y:x drawable ; + + (* Ajout de la legende *) + let taille_texte = Gdk.Font.string_width font caption_text and + taille_texte2 = Gdk.Font.string_height font caption_text in + let x0 = x+(width/2)-taille_texte/2-10 and + y0 = y+5 and + taille_x = taille_texte+20 and + taille_y = taille_texte2+10 in + + pixmap#set_foreground back_color ; + pixmap#rectangle ~filled:true ~x:x0 ~y:y0 ~width:taille_x ~height:taille_y () ; + pixmap#set_foreground contour_color ; + pixmap#rectangle ~filled:false ~x:x0 ~y:y0 ~width:taille_x ~height:taille_y () ; + pixmap#set_foreground caption_color ; + pixmap#string caption_text ~font:font + ~x:(x0+taille_x/2-taille_texte/2) ~y:(y0+taille_y/2+taille_texte2/2) ; + + (* Renvoie le nouveau drawable qui contient la legende *) + pix + in + capture_part draw x y width height filename format progress_func + +(* ============================================================================= *) +(* = Fonction principale de capture a partir d'une pixmap complete = *) +(* = = *) +(* = window = la fenetre mere du drawable suivant = *) +(* = drawable = la pixmap ou fenetre contenant l'image a sauver = *) +(* = width = largeur de l'image = *) +(* = height = hauteur de l'image = *) +(* = filename = nom du fichier = *) +(* = format = format de sauvegarde (de type format_capture) = *) +(* = progress_func = fonction appelee lors de la progression = *) +(* = du type : (progress_save * float -> unit) option = *) +(* = caption = None ou Some(texte legende, couleur) = *) +(* ============================================================================= *) +let capture_complete window drawable width height filename format progress_func caption = + capture_part_with_caption window drawable 0 0 width height filename + format progress_func caption + +(* ============================================================================= *) +(* = Creation d'une image Rgb24 quel que soit le format d'origine = *) +(* ============================================================================= *) +let gtk_image_rgb24_of_image image = + match image with + Images.Index8 i -> Index8.to_rgb24 i + | Images.Rgb24 i -> i + | Images.Index16 i -> Index16.to_rgb24 i + | Images.Rgba32 i -> Rgb24.of_rgba32 i + | Images.Cmyk32 i -> Printf.printf "Pb : Image Cmyk32 !!!\n"; flush stdout ; exit 1 + +(* ============================================================================= *) +(* = Lecture d'une image et creation d'une pixmap = *) +(* ============================================================================= *) +let gtk_image_load filename win format = + (* Chargement de l'image *) + let load_func = get_load_func format in + let image = load_func filename [] in + + (* Recuperation de sa taille *) + let (w, h) = Images.size image in + + (* Creation d'une pixmap de meme taille *) + let create_pixmap window width height = + let depth = (window:GWindow.window)#misc#visual_depth and w = window#misc#window in + let pix = Gdk.Pixmap.create ~window:w ~width:width ~height:height ~depth:depth () in + let pixmap = new GDraw.pixmap pix in + (pix, pixmap) + in + + let (pix, pixmap) = create_pixmap win w h in + + (* Creation d'une image Rgb24 quel que soit le format d'origine *) + let rgb = gtk_image_rgb24_of_image image in + + (* Transfert de l'image Rgb24 dans la pixmap *) + for y = 0 to h-1 do + for x = 0 to w-1 do + let {Images.r=r; Images.g=g; Images.b=b} = Rgb24.get rgb x y in + pixmap#set_foreground (`RGB (r*256, g*256, b*256)) ; + pixmap#point ~x:x ~y:y + done ; + done ; + + (* On renvoie la pixmap qui contient a present l'image lue *) + pixmap + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/gtk_image.mli b/sw/lib/ocaml/gtk_image.mli new file mode 100644 index 0000000000..07274abaaa --- /dev/null +++ b/sw/lib/ocaml/gtk_image.mli @@ -0,0 +1,117 @@ +(* + * $Id$ + * + * Images utils + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(** Module de capture d'écran/gestion d'images + + {b Dépendences : Platform} + *) + +open GDraw + +(** Formats de sauvegarde pris en compte *) +type format_capture = PNG | GIF | JPEG | TIFF | BMP | PPM | POSTSCRIPT + +(** Formats disponibles suivant qu'on est sous Unix/Windows *) +val formats_capture_dispos : format_capture list + +(** [is_format_capture_dispo format] indique si le format est disponible *) +val is_format_capture_dispo : format_capture -> bool + +(** Etats lors de la sauvegarde *) +type progress_save = INIT | SAVING | FINISHED + +(** [extended_string_of_format_capture format] fournit une chaine correspondant + au format : [PNG] -> "PNG"; [POSTSCRIPT] -> "Postscript" *) +val extended_string_of_format_capture : format_capture -> string + +(** [string_of_format_capture format] fournit une chaine correspondant + au format : [PNG] -> "PNG"; [POSTSCRIPT] -> "PS" *) +val string_of_format_capture : format_capture -> string + +(** [format_capture_of_string chaine] renvoie le type {!Gtk_image.format_capture} + correspondant à la chaine *) +val format_capture_of_string : string -> format_capture + +(** [string_of_extension format] renvoie l'extension correspondant au format. + i.e [PNG] -> ".png" *) +val string_of_extension : format_capture -> string + +(** [set_filename_extension fichier format] ajoute l'extension correspondant au + format à la chaine [fichier] si nécessaire *) +val set_filename_extension : string -> format_capture -> string + +(** [update_extension_capture fichier ancien_format nouveau_format] modifie + l'extension du fichier pour qu'elle corresponde à [nouveau_format] *) +val update_extension_capture : + string -> format_capture -> format_capture -> string + +(** [capture_part pixmap_ou_fenetre x y largeur hauteur fichier format progress_func] + effectue la capture partielle à partir de [pixmap_ou_fenetre]. [x] et [y] + designent le coin en haut à gauche et [largeur] et [hauteur] la taille de + l'image à capturer *) +val capture_part : + [> `drawable ] Gobject.obj -> int -> int -> int -> int -> + string -> format_capture -> (progress_save -> float -> unit) option -> unit + +(** [capture_part_with_caption fenetre pixmap_ou_fenetre x y largeur hauteur + fichier format progress_func legende] + effectue la capture partielle à partir de [pixmap_ou_fenetre]. [x] et [y] + designent le coin en haut à gauche et [largeur] et [hauteur] la taille de + l'image à capturer. Une legende est ajoutée sur la capture si + [legende=Some (texte_legende, couleur_texte, couleur_contour, couleur_fond, + fonte)]. + [fenetre] designe ici la fenetre mère du drawable [pixmap_ou_fenetre] + *) +val capture_part_with_caption : + < misc : < visual_depth : int; window : Gdk.window; .. >; .. > -> + Gdk.pixmap -> + int -> + int -> + int -> + int -> + string -> + format_capture -> + (progress_save -> float -> unit) option -> + (string * GDraw.color * GDraw.color * GDraw.color * Gdk.font) option -> + unit + +(** [capture_complete fenetre pixmap_ou_fenetre largeur hauteur + fichier format progress_func legende_optionnelle] sauvegarde tout le contenu + de [pixmap_ou_fenetre] *) +val capture_complete : + < misc : < visual_depth : int; window : Gdk.window; .. >; .. > -> + Gdk.pixmap -> + int -> + int -> + string -> + format_capture -> + (progress_save -> float -> unit) option -> + (string * GDraw.color * GDraw.color * GDraw.color * Gdk.font) option -> + unit + +(** [gtk_image_load filename window format] charge une image au format [format] + et renvoie une pixmap contenant l'image lue *) +val gtk_image_load : string -> GWindow.window -> format_capture -> GDraw.pixmap diff --git a/sw/lib/ocaml/gtk_tools.ml b/sw/lib/ocaml/gtk_tools.ml new file mode 100644 index 0000000000..2b5edf626a --- /dev/null +++ b/sw/lib/ocaml/gtk_tools.ml @@ -0,0 +1,3199 @@ +(* + * $Id$ + * + * Lablgtk2 utils + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(* Numero de version *) +let version = "4.10" + +(* ================================================================================== *) +(* = Version : 4.10 = *) +(* = Derniere update : 26/08/2004 = *) +(* = = *) +(* = 26/08/2004 : Fonctions de creation de fenetres on top = *) +(* = v4.10 = *) +(* = 10/08/2004 : Nouvelles fonctions de modification des couleurs de widgets = *) +(* = v4.09 = *) +(* = 09/08/2004 : Ajout de rectangle_pixmap = *) +(* = v4.08 = *) +(* = 30/07/2004 : Ajout de set_columns_sizes = *) +(* = v4.07 = *) +(* = 16/12/2003 : Scroll des adjustments avec scroll_adjustement. = *) +(* = v4.06 Utilisation de cette fonction pour scroller la fenetre d'affichage= *) +(* = d'un fichier (gtk_tools_display_file), les listes et les text_edit= *) +(* = 03/11/2003 : Ajout de la molette souris = *) +(* = v4.05 = *) +(* = 01/10/2003 : Utilisation de lablgtk-20030326 = *) +(* = v4.04 Encapsulation de GToolbox.popup dans popup car les = *) +(* = coordonnees x et y d'avant ne servent en fait a rien = *) +(* = 25/08/2003 : text_entry_select_text entry = *) +(* = v4.03 = *) +(* = 04/08/2003 : Widget de selection de fonte : select_font_dlg = *) +(* = v4.02 = *) +(* = 29/07/2003 : Modif a create_option_menu qui renvoie une seconde fct = *) +(* = v4.01 permettant de mettre a jour l'option et d'activer le menu = *) +(* = correspondant = *) +(* = Alignement du texte dans les boutons et padding = *) +(* = Padding dans un label = *) +(* = but_set_width et but_set_height = *) +(* = 24/07/2003 : but_set_label, set_widget_front_color et = *) +(* = v4.0 set_button_front_color = *) +(* = 23/07/2003 : create_stipple_pixmap_from_data = *) +(* = v3.99 = *) +(* = 22/07/2003 : calendar et calendar_window = *) +(* = v3.98 set_widget_back_color = *) +(* = create_sized_label_align* et set_label_align* = *) +(* = 21/07/2003 : calendar = *) +(* = v3.97 = *) +(* = 18/06/2003 : create_text_edit, text_edit_clear = *) +(* = text_edit_get_text et text_edit_get_lines = *) +(* = create_spaced_vframe et create_spaced_hframe = *) +(* = 16/06/2003 : animated_msg_box = *) +(* = 12/06/2003 : Ajout des fonctions de creation de menus qui peuvent etre actives = *) +(* = v3.94 ou desactives = *) +(* = 27/05/2003 : Fonctions de creation de menus = *) +(* = v3.93 = *) +(* = 07/04/2003 : get_widget_size widget = *) +(* = v3.92 = *) +(* = 03/03/2003 : Eval_string modifiee pour les noms finissant par 'x' = *) +(* = v3.91 Le callback de selection des managed lists recoit (row, col, dbl) = *) +(* = au lieu du double click = *) +(* = 26/02/2003 : connect_win_focus_change = *) +(* = v3.9 = *) +(* = 10/02/2003 : Passage des rgb<->color dans gtk_draw = *) +(* = v3.8 = *) +(* = 05/02/2003 : int_spinner_connect/gtk_tools_float_spinner_connect = *) +(* = v3.7 = *) +(* = 30/01/2003 : check_dbl_click = *) +(* = 29/01/2003 : pixmap_from_file = *) +(* = 28/01/2003 : Correction du widget latlon : le menu N/S ou E/W n'etait pas a sa = *) +(* = bonne valeur au depart si lat<0 ou lon<0 = *) +(* = Les fonctions de mise en pointilles marchent mal par defaut. = *) +(* = il faut ajouter le parametre width pour que ca marche correctement= *) +(* = 22/01/2003 : color_to_rgb et rgb_to_color, = *) +(* = create_hom_hbox et create_spaced_hbox = *) +(* = 21/01/2003 : create_infos_win = *) +(* = 15/01/2003 : create_vslider_simple, slider_connect et = *) +(* = create_vslider = *) +(* = 21/11/2002 : ajout de create_togglebutton, = *) +(* = create_managed_list, connect_managed_list, = *) +(* = create_buttons et create_buttons_connect = *) +(* = create_hpaned, create_vpaned = *) +(* = 20/11/2002 : create_notebook et notebook_add_page = *) +(* = 19/11/2002 : ajout de create_ops_compare et = *) +(* = create_time_select = *) +(* = 31/10/2002 : ajout de check_overwrite a la boite de selection de fichier = *) +(* = et ajout de question_box = *) +(* = 21/10/2002 : set_cursor = *) +(* = 17/10/2002 : list_connect_up_down_keys = *) +(* = 15/10/2002 : create_hbox/vbox = *) +(* = 11/10/2002 : create_progress_bar = *) +(* = 26/09/2002 : list_connect_check_dbl_click = *) +(* = 18/09/2002 : connect_popup_menu, connect_func_popup_menu = *) +(* = 16/09/2002 : modifs a create_list_(with_hor_scroll) = *) +(* = 13/09/2002 : create_modal_window = *) +(* = 11/09/2002 : display_file = *) +(* = 11/09/2002 : create_window_with_menubar_help = *) +(* = 05/09/2002 : show_log,gtk_tools_hide_log,gtk_tools_add_log = *) +(* = 04/09/2002 : create_optionmenu = *) +(* = 03/09/2002 : disconnect = *) +(* = 18/07/2002 : create_bbox = *) +(* = 18/07/2002 : create_button, create_(sized_)label = *) +(* = 18/07/2002 : create_hframe, create_frame -> create_vframe = *) +(* = 17/07/2002 : get_screen_size = *) +(* = 17/07/2002 : get_registered_window = *) +(* = 17/07/2002 : get_window_geometry, window_modify_connect = *) +(* = et set_window_position = *) +(* = 16/07/2002 : create_list_with_hor_scroll = *) +(* = 11/07/2002 : create_latlon_selection, latlon_selection_get = *) +(* = update_latlon_selection et = *) +(* = latlon_selection_change = *) +(* = 10/07/2002 : create_text_entry_simple, text_entry_connect = *) +(* = et text_entry_connect_modify = *) +(* = 10/07/2002 : create_list/gtk_tools_list_connect = *) +(* = 10/07/2002 : register_window/gtk_tools_show_registered_window = *) +(* = 08/07/2002 : area_key_connect et modif de area_connect en = *) +(* = area_mouse_connect = *) +(* = 08/07/2002 : create_radiobuttons = *) +(* = 16/05/2002 : screenshot_box + screenshot_box_with_caption = *) +(* = 14/05/2002 : create_color_selection_button = *) +(* = 14/05/2002 : create_scrolled_box et change_scrolled_box = *) +(* = 20/04/2002 : create_text_entry = *) +(* = 19/04/2002 : create_int_spinner et create_float_spinner = *) +(* = 19/04/2002 : create_checkbutton_simple, create_checkbutton = *) +(* = 19/04/2002 : Modif dans insert_timer = *) +(* = 23/01/2002 : string_width_height = *) +(* = 23/01/2002 : legende dans screenshot_box = *) +(* = 17/01/2002 : create_pixbutton = *) +(* = 17/01/2002 : create_frame = *) +(* = 17/01/2002 : select_colors = *) +(* = 16/01/2002 : set_sensitive & set_sensitive_list = *) +(* = 16/01/2002 : error_box : boite de message d'erreur = *) +(* = 15/01/2002 : screenshot_box : boite de capture d'ecran = *) +(* = 14/01/2002 : create_popup_menu : ajout de separateurs = *) +(* = 14/01/2002 : set_widget_font = *) +(* = 10/01/2002 : create_draw_area_simple/gtk_tools_area_connect = *) +(* = 07/01/2002 : boite de selection de couleur = *) +(* = 04/01/2002 : passage a lablgtk version 1.2.3 (anciennement 1.2.1) = *) +(* = 02/01/2002 : window_set_front = *) +(* = = *) +(* ================================================================================== *) + +(* Modules Gtk/Gdk *) +open GMain +open GdkKeysyms +open Gdk +open GToolbox (* Acces a message_box *) + +(* Modules locaux *) +open Platform +open Ocaml_tools +open Gtk_tools_icons + +(* ============================================================================= *) +(* = Force la mise a jour de l'interface = *) +(* ============================================================================= *) +let force_update_interface () = + while Glib.Main.pending () do ignore(Glib.Main.iteration false) done + +(* ============================================================================= *) +(* = Initialisation des couleurs = *) +(* ============================================================================= *) +let init_colors () = + (* Mise en place des couleurs correctes *) + Gdk.Rgb.init () ; + GtkBase.Widget.set_default_visual (Gdk.Rgb.get_visual ()) ; + GtkBase.Widget.set_default_colormap (Gdk.Rgb.get_cmap ()) + +(* ============================================================================= *) +(* = Lancement de la mainloop pour l'interface = *) +(* ============================================================================= *) +let main_loop () = Main.main () + +(* ============================================================================= *) +(* = Initialisation des tooltips = *) +(* ============================================================================= *) +let init_tooltips () = GData.tooltips () + +(* ============================================================================= *) +(* = Ajout d'un tooltip a un widget = *) +(* = = *) +(* = widget = le bouton concerne = *) +(* = texte = le texte de l'aide contextuelle = *) +(* ============================================================================= *) +let add_tooltips tooltips widget texte = + (tooltips:GData.tooltips)#set_tip widget#coerce ~text:texte + +(* ============================================================================= *) +(* = Renvoie la taille de l'ecran = *) +(* ============================================================================= *) +let get_screen_size () = (Gdk.Screen.width (), Gdk.Screen.height ()) + +(* ============================================================================= *) +(* = Deconnexion d'un signal = *) +(* = = *) +(* = wid = l'objet concerne = *) +(* = id = identifiant recu lors de la connexion du signal = *) +(* ============================================================================= *) +let disconnect wid id = wid#misc#disconnect id + +(* ============================================================================= *) +(* = Activation/Desactivation d'un widget = *) +(* = = *) +(* = widget = le widget a modifier = *) +(* = sensitive = true si active, false sinon = *) +(* ============================================================================= *) +let set_sensitive widget sensitive = + ignore(widget#misc#set_sensitive sensitive) + +(* ============================================================================= *) +(* = Activation/Desactivation d'une liste de widgets = *) +(* = = *) +(* = widget_list = la liste des widgets a modifier = *) +(* = sensitive = true si active, false sinon = *) +(* ============================================================================= *) +let set_sensitive_list widget_list sensitive = + List.iter (fun widget -> set_sensitive widget sensitive) widget_list + +(* ============================================================================= *) +(* = Modification du curseur dans une fenetre = *) +(* = = *) +(* = window = la fenetre Gdk.window concernee = *) +(* = cursor = le curseur (cree avec Gdk.Cursor.create) = *) +(* ============================================================================= *) +let set_cursor window cursor = Gdk.Window.set_cursor window cursor + +(* ============================================================================= *) +(* = Taille d'un widget = *) +(* = widget = le widget dont on desire connaitre la taille = *) +(* ============================================================================= *) +let get_widget_size widget = + let rect = widget#misc#allocation in (rect.Gtk.width, rect.Gtk.height) + +(* Boutons evenements souris *) +type bouton_souris = B_GAUCHE | B_DROIT | B_MILIEU | B_NONE + +(* ============================================================================= *) +(* = Test d'un bouton souris apres un evenement de type click = *) +(* = = *) +(* = event = l'evenement souris = *) +(* ============================================================================= *) +let test_mouse_but event = + match GdkEvent.Button.button event with + 1 -> B_GAUCHE | 2 -> B_MILIEU | 3 -> B_DROIT |_ -> B_NONE + +(* ============================================================================= *) +(* = Renvoie la position de la souris apres un evenement de type click = *) +(* = = *) +(* = event = l'evenement souris = *) +(* ============================================================================= *) +let get_mouse_pos_click event = + (int_of_float (GdkEvent.Button.x event), int_of_float (GdkEvent.Button.y event)) + +(* ============================================================================= *) +(* = Renvoie la position de la souris apres un evenement de type deplacement = *) +(* = = *) +(* = event = l'evenement souris = *) +(* ============================================================================= *) +let get_mouse_pos_move event = + (int_of_float (GdkEvent.Motion.x event), int_of_float (GdkEvent.Motion.y event)) + +(* ============================================================================= *) +(* = Fonction de test d'un double click = *) +(* = = *) +(* = event = l'evenement concerne = *) +(* ============================================================================= *) +let check_dbl_click event = + match GdkEvent.get_type event with `TWO_BUTTON_PRESS -> true | _ -> false + +(* ============================================================================= *) +(* = Renvoie la fonte fixed = *) +(* ============================================================================= *) +let get_fixed_font () = + if platform_is_unix then Gdk.Font.load_fontset "fixed" + else Gdk.Font.load_fontset + "-misc-arial-medium-r-normal--13-120-75-75-c-80-iso8859-1" + +let get_fixed_font2 () = + Gdk.Font.load_fontset "-misc-fixed-medium-r-normal--13-120-75-75-c-80-iso8859-1" + +(* ============================================================================= *) +(* = Modification de la fonte d'un widget = *) +(* = = *) +(* = w = le widget = *) +(* = font = la fonte a appliquer = *) +(* ============================================================================= *) +let set_widget_font w font = + let style = w#misc#style#copy in + style#set_font font ; + ignore(w#misc#set_style style) + +(* ============================================================================= *) +(* = Modification de la couleur d'un widget = *) +(* = = *) +(* = w = le widget = *) +(* = color = la nouvelle couleur = *) +(* ============================================================================= *) +let set_widget_back_color w color = + let style = w#misc#style#copy in + style#set_bg [`NORMAL, color]; + ignore(w#misc#set_style style) +let set_widget_front_color w color = + let style = w#misc#style#copy in + style#set_fg [`NORMAL, color]; + ignore(w#misc#set_style style) + +let set_entry_back_color w color = + let style = w#misc#style#copy in + style#set_base [`NORMAL, color] ; + ignore(w#misc#set_style style) +let set_entry_front_color w color = + let style = w#misc#style#copy in + style#set_text [`NORMAL, color] ; + ignore(w#misc#set_style style) +let set_entry_outline_color w color = + set_widget_back_color w color + +let set_button_back_color w color = + let style = w#misc#style#copy in + style#set_bg [`NORMAL, color; `PRELIGHT, color] ; + ignore(w#misc#set_style style) +let set_button_front_color w color = + set_widget_front_color (List.hd w#children) color + +(* Pour le fond d'un label il faut une event box au dessus *) + +(* ============================================================================= *) +(* = Modification de l'alignement d'un widget de type label = *) +(* = = *) +(* = label = le widget = *) +(* = pos = la valeur de l'alignement (entre 0. et 1.) = *) +(* ============================================================================= *) +let set_label_align label pos = label#set_xalign pos + +let set_label_align_left label = set_label_align label 0. +let set_label_align_right label = set_label_align label 1. +let set_label_align_center label = set_label_align label 0.5 + +let set_label_padding label xpad = label#set_xpad xpad + +(* ============================================================================= *) +(* = Taille en pixel de la chaine de caractere dans la fonte indiquee = *) +(* = = *) +(* = font = la fonte = *) +(* = string = la chaine de caracteres = *) +(* ============================================================================= *) +let string_width_height font string = + (Gdk.Font.string_width font string, Gdk.Font.string_height font string) + +(* ============================================================================= *) +(* = Creation d'une boite a boutons = *) +(* = = *) +(* = pack_method = ou mettre la boite = *) +(* ============================================================================= *) +let create_bbox pack_method = + GPack.button_box `HORIZONTAL ~border_width:5 ~packing:pack_method + ~layout:`SPREAD ~spacing:5 () + +(* ============================================================================= *) +(* = Creation d'une boite horizontale = *) +(* = = *) +(* = pack_method = ou mettre la boite = *) +(* ============================================================================= *) +let create_hbox pack_method = GPack.hbox ~packing:pack_method () +let create_hom_hbox pack_method = + GPack.hbox ~homogeneous:true ~packing:pack_method () +let create_spaced_hbox pack_method = + GPack.hbox ~spacing:5 ~border_width:5~packing:pack_method () +let create_hom_spaced_hbox pack_method = + GPack.hbox ~homogeneous:true ~spacing:5 ~border_width:5~packing:pack_method () + +(* ============================================================================= *) +(* = Creation d'une boite verticale = *) +(* = = *) +(* = pack_method = ou mettre la boite = *) +(* ============================================================================= *) +let create_vbox pack_method = GPack.vbox ~packing:pack_method () +let create_hom_vbox pack_method = + GPack.vbox ~homogeneous:true ~packing:pack_method () +let create_spaced_vbox pack_method = + GPack.vbox ~spacing:5 ~border_width:5~packing:pack_method () +let create_hom_spaced_vbox pack_method = + GPack.vbox ~homogeneous:true ~spacing:5 ~border_width:5~packing:pack_method () + +(* ============================================================================= *) +(* = Creation d'une frame contenant une vbox = *) +(* = = *) +(* = title = titre de la frame = *) +(* = pack_method = ou mettre la frame = *) +(* = Renvoie la frame et la vbox = *) +(* ============================================================================= *) +let create_vframe title pack_method = + let fr = GBin.frame ~label:title ~packing:pack_method () in + (fr, create_vbox fr#add) + +let create_spaced_vframe title pack_method = + let fr = GBin.frame ~label:title ~packing:pack_method () in + (fr, create_spaced_vbox fr#add) + +(* ============================================================================= *) +(* = Creation d'une frame contenant une hbox = *) +(* = = *) +(* = title = titre de la frame = *) +(* = pack_method = ou mettre la frame = *) +(* = Renvoie la frame et la vbox = *) +(* ============================================================================= *) +let create_hframe title pack_method = + let fr = GBin.frame ~label:title ~packing:pack_method () in + (fr, create_hbox fr#add) + +let create_spaced_hframe title pack_method = + let fr = GBin.frame ~label:title ~packing:pack_method () in + (fr, create_spaced_hbox fr#add) + +(* ============================================================================= *) +(* = Creation d'une zone scrollable contenant une boite a widgets = *) +(* = = *) +(* = pack_method = ou mettre la zone scrollable = *) +(* ============================================================================= *) +let create_scrolled_box pack_method = + let scrolled_window = GBin.scrolled_window ~border_width: 10 + ~hpolicy: `AUTOMATIC ~packing:pack_method () in + (scrolled_window, create_vbox scrolled_window#add_with_viewport) + +(* ============================================================================= *) +(* = Destruction et recreation d'une boite verticale dans une zone scrollable = *) +(* = = *) +(* = scrolled_window = zone scrollable a modifier = *) +(* = old_box = ancienne boite contenue dans cette zone = *) +(* ============================================================================= *) +let change_scrolled_box scrolled_window old_box = + old_box#destroy () ; + create_vbox scrolled_window#add_with_viewport + +(* ============================================================================= *) +(* = Creation d'un widget de type notebook = *) +(* = = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_notebook pack_method = + GPack.notebook ~scrollable:true ~packing:pack_method () + +(* ============================================================================= *) +(* = Ajout d'une page a un notebook = *) +(* = = *) +(* = notebook = le notebook ou ajouter la page = *) +(* = page_label = nom de la page = *) +(* ============================================================================= *) +let notebook_add_page notebook page_label = + let lbl = GMisc.label ~text:page_label () in + let f = GBin.frame ~packing:((notebook:GPack.notebook)#append_page + ~tab_label:lbl#coerce) () in + (f, create_vbox f#add) + +(* ============================================================================= *) +(* = Creation d'un paned (division mobile entre deux zones d'une fenetre) = *) +(* = = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_hpaned pack_method = + GPack.paned ~packing:pack_method `HORIZONTAL () + +let create_vpaned pack_method = + GPack.paned ~packing:pack_method `VERTICAL () + +(* ============================================================================= *) +(* = Creation d'un bouton = *) +(* = = *) +(* = label = texte du bouton = *) +(* = pack_method = ou mettre le bouton = *) +(* ============================================================================= *) +let create_button label pack_method = + GButton.button ~label:label ~packing:pack_method () + +(* ============================================================================= *) +(* = Creation d'un bouton avec une taille fixee = *) +(* = = *) +(* = label = texte du bouton = *) +(* = width = taille du bouton = *) +(* = pack_method = ou mettre le bouton = *) +(* ============================================================================= *) +let create_sized_button label width pack_method = + let but = GButton.button ~label:label ~packing:pack_method () in + but#misc#set_size_request ~width:width () ; + but +(* GTK2 AAA GButton.button ~label:label ~width:width ~packing:pack_method () *) + +(* ============================================================================= *) +(* = Modification du texte dans un bouton = *) +(* = = *) +(* = button = le bouton = *) +(* = label = texte du bouton = *) +(* ============================================================================= *) +let but_set_label button label = + (GMisc.label_cast (List.hd button#children))#set_text label + +let set_button_align button pos = + set_label_align (GMisc.label_cast (List.hd button#children)) pos +let set_button_align_left button =set_button_align button 0. +let set_button_align_right button =set_button_align button 1. +let set_button_align_center button=set_button_align button 0.5 + +let set_button_padding button xpad = + set_label_padding (GMisc.label_cast (List.hd button#children)) xpad + +let but_set_width button width = + (button:GButton.button)#misc#set_size_request ~width () +(* GTK2 AAA GtkBase.Container.set (GtkButton.Button.cast button#as_widget) ~width:width*) +let but_set_height button height = + (button:GButton.button)#misc#set_size_request ~height () +(* GTK2 AAA GtkBase.Container.set (GtkButton.Button.cast button#as_widget) ~height:height*) + +(* ============================================================================= *) +(* = Connexion d'une fonction a un bouton = *) +(* = = *) +(* = but = le bouton = *) +(* = func = la fonction = *) +(* ============================================================================= *) +let but_connect but func = ignore(but#connect#clicked ~callback:func) + +(* ============================================================================= *) +(* = Creation d'une rangee de boutons = *) +(* = = *) +(* = lst_buts = liste des (noms, tooltips) = *) +(* = tooltips = aide contextuelle = *) +(* = pack_method = ou mettre les boutons = *) +(* ============================================================================= *) +let create_buttons lst_buts tooltips pack_method = + let bbox = create_bbox pack_method in + List.map (fun (text, tip) -> + let b = create_button text bbox#add in + add_tooltips tooltips b tip; + b) lst_buts + +(* ============================================================================= *) +(* = Association de callbacks a une liste de boutons = *) +(* = = *) +(* = lst_buts = liste des boutons creee avec la fonction precedente = *) +(* = lst_callbacks = callbacks associes = *) +(* ============================================================================= *) +let create_buttons_connect lst_buts lst_callbacks = + List.iter2 (fun b c -> but_connect b c) lst_buts lst_callbacks + +(* ============================================================================= *) +(* = Creation d'un check button sans callback = *) +(* = = *) +(* = active = valeur active/inactif au depart = *) +(* = label = texte du label = *) +(* = pack_method = ou mettre les widgets = *) +(* = tip = texte de l'aide contextuelle = *) +(* = tooltips = aide contextuelle = *) +(* ============================================================================= *) +let create_checkbutton_simple active label pack_method tip tooltips = + let check = GButton.check_button ~label:label ~active:active + ~packing:pack_method () in + if tip <> "" then add_tooltips tooltips check tip ; + check + +(* ============================================================================= *) +(* = Creation d'un check button avec callback = *) +(* = = *) +(* = active = valeur active/inactif au depart = *) +(* = label = texte du label = *) +(* = pack_method = ou mettre les widgets = *) +(* = tip = texte de l'aide contextuelle = *) +(* = tooltips = aide contextuelle = *) +(* = callback = callback appele lors de la modification du bouton = *) +(* ============================================================================= *) +let create_checkbutton active label pack_method tip tooltips callback = + let check = create_checkbutton_simple active label pack_method + tip tooltips in + but_connect check (fun () -> callback check#active) ; + check + +(* ============================================================================= *) +(* = Creation d'une suite de radio buttons = *) +(* = = *) +(* = lst_names = liste des noms des boutons avec leurs types (nom, type) = *) +(* = func_active = fonction indiquant quel est le bouton actif par defaut = *) +(* = pack_method = ou mettre les widgets = *) +(* = = *) +(* = Renvoie la liste des boutons crees (pour ajout de tooltips par exemple) = *) +(* ============================================================================= *) +let create_radiobuttons_simple lst_names func_active pack_method = + let lst_but = List.fold_left (fun lst (name, typ) -> + let but = + if lst = [] then GButton.radio_button ~label:name ~packing:pack_method () + else GButton.radio_button ~label:name ~packing:pack_method + ~group:(List.hd lst)#group () + in + but#set_active (func_active typ) ; + lst @ [but]) [] lst_names in + + lst_but + +(* ============================================================================= *) +(* = Connexion de callbacks a des radiobuttons = *) +(* = = *) +(* = lst_names = liste des noms des boutons avec leurs types (nom, type) = *) +(* = lst_but = liste des radiobuttons = *) +(* = func_select = fonction appelee lors de la modification du bouton actif = *) +(* ============================================================================= *) +let radiobuttons_connect lst_names lst_but func_select = + List.iter2 (fun but (_, typ) -> + ignore((but:GButton.radio_button)#connect#clicked ~callback:(fun () -> + func_select typ))) lst_but lst_names + +(* ============================================================================= *) +(* = Creation d'une suite de radio buttons = *) +(* = = *) +(* = lst_names = liste des noms des boutons avec leurs types (nom, type) = *) +(* = func_active = fonction indiquant quel est le bouton actif par defaut = *) +(* = func_select = fonction appelee lors de la modification du bouton actif = *) +(* = pack_method = ou mettre les widgets = *) +(* = = *) +(* = Renvoie la liste des boutons crees (pour ajout de tooltips par exemple) = *) +(* ============================================================================= *) +let create_radiobuttons lst_names func_active func_select pack_method = + let lst_but = create_radiobuttons_simple + lst_names func_active pack_method in + + (* Connexion de la fonction de selection *) + radiobuttons_connect lst_names lst_but func_select ; + lst_but + +(* ============================================================================= *) +(* = Creation d'un toggle button = *) +(* = = *) +(* = label = texte du bouton = *) +(* = active = indique l'etat initial du bouton = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_togglebutton label active pack_method = + GButton.toggle_button ~label:label ~active:active ~packing:pack_method () + +(* ============================================================================= *) +(* = Creation d'un bouton contenant une pixmap = *) +(* = = *) +(* = pm = la pixmap a mettre dans le bouton = *) +(* = pack_method = ou mettre le bouton = *) +(* ============================================================================= *) +let create_pixbutton pm pack_method = + let but = GButton.button ~packing:pack_method () in + ignore(GMisc.pixmap (pm:GDraw.pixmap) ~packing:but#add ()) ; + but + +(* ============================================================================= *) +(* = Creation d'un label = *) +(* = = *) +(* = label = texte du label = *) +(* = pack_method = ou mettre le label = *) +(* ============================================================================= *) +let create_label label pack_method = + GMisc.label ~text:label ~packing:pack_method () + +(* ============================================================================= *) +(* = Creation d'un label ayant une taille fixee = *) +(* = = *) +(* = label = texte du label = *) +(* = width = largeur du label en pixels = *) +(* = pack_method = ou mettre le label = *) +(* ============================================================================= *) +let create_sized_label label width pack_method = + GMisc.label ~text:label ~width:width ~packing:pack_method () + +let create_sized_label_align label width pos pack_method = + GMisc.label ~text:label ~width:width ~packing:pack_method ~xalign:pos () +let create_sized_label_align_left label width pack_method = + GMisc.label ~text:label ~width:width ~packing:pack_method ~xalign:0. () +let create_sized_label_align_right label width pack_method = + GMisc.label ~text:label ~width:width ~packing:pack_method ~xalign:1. () + +(* ============================================================================= *) +(* = Boite de question = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = question_msg = message a afficher = *) +(* = default_is_cancel = indique si c'est le bouton annuler qui est le defaut = *) +(* ============================================================================= *) +let question_box window title question_msg default_is_cancel = + let pm = GDraw.pixmap_from_xpm_d ~data:question_icon ~window:window () in + let icon = (GMisc.pixmap pm ())#coerce in + (* Fonction de remplacement d'une extension *) + let reponse = question_box ~title:title ~buttons:["Oui"; "Annuler"] + ~default:(if default_is_cancel then 2 else 1) + ~icon:icon question_msg in + (* Renvoie vrai si "Oui" est selectionne *) + reponse=1 + +(* ============================================================================= *) +(* = Boite de message d'erreur = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = error_msg = message d'erreur a afficher = *) +(* ============================================================================= *) +let error_box window title error_msg = + let pm = GDraw.pixmap_from_xpm_d ~data:warning_icon ~window:window () in + let icon = (GMisc.pixmap pm ())#coerce in + message_box ~title:title ~icon:icon error_msg + +(* ============================================================================= *) +(* = Boite de message avec icone anime = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = error_msg = message d'erreur a afficher = *) +(* ============================================================================= *) +let animated_msg_box title msg lst_pixmaps = + let window = GWindow.dialog ~modal:true ~title:title () in + + let max_pixmaps = List.length lst_pixmaps in + let l = List.map (fun f -> + GDraw.pixmap_from_xpm ~file:f ~window:window ()) lst_pixmaps in + let t = Array.of_list l in + + let hb = GPack.hbox ~border_width:10 ~packing:window#vbox#add () in + ignore (create_label msg (hb#pack ~from:`END)); + let hbox = ref (create_hbox hb#pack) in + + (* Fonction de mise a jour du pixmap dans la boite de dialogue *) + let create pm = + (!hbox)#destroy () ; + hbox := create_hbox hb#pack ; + (!hbox)#pack (GMisc.pixmap pm ())#coerce ~padding:4 ; + in + create t.(0) ; + + let b = create_button "OK" + (window#action_area#pack ~expand:true ~padding:4) in + but_connect b window#destroy ; + b#grab_default () ; + window#set_position `CENTER; + window#show (); + + (* Timeout de l'animation *) + let current_idx = ref 0 in + let timeout _ = + incr current_idx ; + if !current_idx>=max_pixmaps then current_idx:=0 ; + create t.(!current_idx) ; + true + in + let timeoutid = Timeout.add ~ms:100 ~callback:timeout in + + (* On stoppe le timeout quand la boite de dialogue est fermee *) + ignore (window#connect#destroy ~callback: (fun () -> + Timeout.remove timeoutid ; GMain.Main.quit ())) ; + + GMain.Main.main () + +(* ============================================================================= *) +(* = Ouverture d'une boite de dialogue de selection de fichier. On teste = *) +(* = l'existence du fichier si c'est demande = *) +(* = = *) +(* = title : titre de la fenetre de selection = *) +(* = read_func : fonction de lecture du fichier apres selection = *) +(* = update_func : None ou Some f, fonction appelee apres read_func = *) +(* = default_filename : nom selectionne par defaut si <> "" = *) +(* = check_overwrite : indique si on teste l'existence du fichier = *) +(* ============================================================================= *) +let open_file_dlg title read_func update_func default_filename + check_overwrite = + let do_select_file filename = + (* Appel a la fonction de lecture du fichier *) + read_func filename; + (* Appel a la fonction de mise a jour si besoin *) + match update_func with None -> () | Some f -> f () + in + + (* Creation de la boite de dialogue *) + let fs = GWindow.file_selection ~modal:true ~title:title () in + + let check_file_overwrite_ok filename = + try + let stat = Unix.stat filename in + if stat.Unix.st_kind = Unix.S_REG then begin + question_box fs + "Le fichier existe" "Le fichier existe, continuer ?" false + end else true + with + Unix.Unix_error (Unix.ENOENT, _, _) -> true + | _ -> false + in + + ignore(fs#ok_button#connect#clicked ~callback:(fun () -> + (* Recuperation du nom du fichier *) + let filename = fs#filename in + if not check_overwrite or (check_file_overwrite_ok filename) then begin + fs#destroy () ; + do_select_file filename + end)) ; + + ignore(fs#cancel_button#connect#clicked ~callback:fs#destroy) ; + (* Mise a jour du nom de fichier par defaut *) + if default_filename <> "" then fs#set_filename default_filename ; + fs#show () + +type timer_type = + TIMER_TIME (* Heure uniquement *) + | TIMER_DATE (* Date uniquement *) + | TIMER_TIME_AND_DATE (* Affichage de l'heure et de la date *) + +(* ============================================================================= *) +(* = Insertion d'un timer dans un label = *) +(* = = *) +(* = label = le widget label ou afficher le timer = *) +(* = timer_type = TIMER_TIME, TIMER_DATE ou TIMER_TIME_AND_DATE = *) +(* = force_beginning = true si active lance des le depart = *) +(* ============================================================================= *) +let insert_timer label timer_type force_beginning = + (* Fonction appelee *) + let timeout () = + let tm = timer_get_time () in + let val_time = + match timer_type with + TIMER_TIME -> timer_string_of_time tm + | TIMER_DATE -> timer_string_of_date tm + | TIMER_TIME_AND_DATE -> (timer_string_of_time tm) ^ " - " ^ + (timer_string_of_date tm) + in + + (label:GMisc.label)#set_text val_time; + true in + + (* Mise en place du timer toutes les 1000 millisecondes *) + ignore(Timeout.add ~ms:1000 ~callback:timeout) ; + + (* Si c'est demande, on force l'affichage des le debut *) + if force_beginning then ignore(timeout ()) + +(* ============================================================================= *) +(* = Encapsulation de GToolbox.popup = *) +(* ============================================================================= *) +let popup menu_entries = + GToolbox.popup_menu ~button:0 ~time:Int32.zero ~entries:menu_entries + +(* ============================================================================= *) +(* = Connexion d'un popup menu a un bouton = *) +(* = = *) +(* = wid = le wiget concerne = *) +(* = button = bouton a presser pour afficher le menu = *) +(* = test_cond_func = fonction testant si le menu doit etre affiche = *) +(* = menu_entries = entrees a afficher dans le menu = *) +(* ============================================================================= *) +let connect_popup_menu wid button test_cond_func menu_entries = + ignore(wid#event#connect#button_press ~callback:(fun ev -> + if test_cond_func () && + (test_mouse_but ev) = button && + GdkEvent.get_type ev = `BUTTON_PRESS then begin + popup menu_entries ; true + end else false)) + +(* ============================================================================= *) +(* = Connexion d'un popup menu a un bouton. Le menu est construit de maniere = *) +(* = dynamique, il provient de l'appel a une fonction = *) +(* = = *) +(* = wid = le wiget concerne = *) +(* = button = bouton a presser pour afficher le menu = *) +(* = test_cond_func = fonction testant si le menu doit etre affiche = *) +(* = get_menu_entries = fonction renvoyant les entrees a afficher dans le menu = *) +(* ============================================================================= *) +let connect_func_popup_menu wid button test_cond_func get_menu_entries = + ignore(wid#event#connect#button_press ~callback:(fun ev -> + if test_cond_func () && + (test_mouse_but ev) = button && + GdkEvent.get_type ev = `BUTTON_PRESS then begin + popup (get_menu_entries ()) ; true + end else false)) + +(* ============================================================================= *) +(* = Creation d'un menu la ou se trouve la souris = *) +(* = = *) +(* = title = titre du menu ("" si pas de titre) = *) +(* = event = evenement de type click souris = *) +(* = data = liste de parametres du type (texte, Some fonction, param) ou = *) +(* = texte designe le texte correspondant au sous-menu et fonction la fonction = *) +(* = a appeler apres selection avec le parametre param = *) +(* ============================================================================= *) +let create_popup_menu title event data = + let button = GdkEvent.Button.button event and + time = GdkEvent.Button.time event and + menu = GMenu.menu ~show:true () in + + (* Fonction d'ajout de sous-menus *) + let attache_menu (texte, fonction, param, sous_menu) = + (* Ajout d'un separateur ou d'un sous-menu ? *) + if texte = "" then ignore(GMenu.menu_item ~packing:menu#append ()) + else begin + let ssmenu = GMenu.menu_item ~label:texte ~packing:menu#append () in + begin + match fonction with + None -> () + | Some f -> ignore(ssmenu#connect#activate ~callback:(fun () -> f param)) + end ; + + (* Ajout d'un sous-menu *) + if sous_menu <> [] then begin + let m = GMenu.menu () in + List.iter (fun (t, func, param) -> + let ss_m = GMenu.menu_item ~label:t ~packing:m#append () in + match func with + None -> () + | Some f -> ignore(ss_m#connect#activate ~callback:(fun () -> f param)) + ) sous_menu ; + ssmenu#set_submenu m + end + end + in + + if title <> "" then begin + (* Ajout du titre *) + attache_menu (title, None, "", []) ; + (* Ajout d'un separateur apres le titre *) + attache_menu ("", None, "", []) ; + end ; + List.iter attache_menu data ; + + (* Affichage du popup-menu *) + menu#popup ~button:button ~time:time + +(* ============================================================================= *) +(* = Creation d'un menu a options = *) +(* = = *) +(* = lst_names = liste des noms des options avec leurs types (nom, type) = *) +(* = func_active = fonction indiquant quelle est l'option active par defaut = *) +(* = func_select = fonction appelee lors de la modification de l'option = *) +(* = pack_method = ou mettre le menu = *) +(* = = *) +(* = Renvoie le menu et la fonction de selection d'une option = *) +(* ============================================================================= *) +let create_optionmenu lst_names func_active func_select pack_method = + (* Creation des options *) + let menu = GMenu.menu () in + let lst_menus = + List.map (fun (nom, typ) -> + let m = GMenu.menu_item ~label:nom ~packing: menu#append () in + ignore (m#connect#activate ~callback:(fun () -> func_select typ)) ; + m) lst_names in + + (* Creation de l'option menu *) + let optionmenu = GMenu.option_menu ~packing:pack_method () in + optionmenu#set_menu menu ; + + (* Recherche du numero du menu de l'option par defaut *) + let n = ref 0 and idx = ref 0 in + List.iter (fun (_, typ) -> if func_active typ then n:=!idx; incr idx) lst_names ; + optionmenu#set_history !n ; + + let set_option typ = + let n = ref 0 and idx = ref 0 in + List.iter (fun (_, typ0) -> if typ0=typ then n:=!idx; incr idx) lst_names ; + optionmenu#set_history !n ; + in + + let set_option_and_activate typ = + let n = ref 0 and idx = ref 0 in + List.iter2 (fun (_, typ0) m -> + if typ0=typ then begin n:=!idx; m#activate () end ; + incr idx) lst_names lst_menus ; + optionmenu#set_history !n ; + in + + (optionmenu, set_option, set_option_and_activate) + +(* ============================================================================= *) +(* = Pixmap d'ouverture d'un fichier = *) +(* ============================================================================= *) +let open_file_pixmap = + [|(* width height num_colors chars_per_pixel *) + " 20 19 5 1"; + (* colors *) + ". c None";"# c #000000";"i c #ffffff";"s c #7f7f00";"y c #ffff00"; + (* pixels *) + "...................."; "...................."; "...................."; + "...........###......"; "..........#...#.#..."; "...............##..."; + "...###........###..."; "..#yiy#######......."; "..#iyiyiyiyi#......."; + "..#yiyiyiyiy#......."; "..#iyiy###########.."; "..#yiy#sssssssss#..."; + "..#iy#sssssssss#...."; "..#y#sssssssss#....."; "..##sssssssss#......"; + "..###########......."; "...................."; "...................."; + "...................." |] + +(* ============================================================================= *) +(* = Creation d'une GDraw.pixmap a partir d'un fichier xpm = *) +(* = = *) +(* = filename = nom du fichier xpm = *) +(* = window = fenetre = *) +(* ============================================================================= *) +let pixmap_from_file filename window = + GDraw.pixmap_from_xpm ~file:filename ~window:(window:GWindow.window) () + +(* ============================================================================= *) +(* = Creation d'une pixmap = *) +(* = = *) +(* = window = fenetre mere = *) +(* = width = largeur en pixels de la pixmap = *) +(* = height = hauteur en pixels de la pixmap = *) +(* = = *) +(* = En retour, pix sert a mettre la pixmap dans une zone de dessin par ex. = *) +(* = et pixmap sert au dessin. = *) +(* ============================================================================= *) +let create_pixmap window width height = + let depth = (window:GWindow.window)#misc#visual_depth and w = window#misc#window in + let pix = Pixmap.create ~window:w ~width:width ~height:height ~depth:depth () in + let pixmap = new GDraw.pixmap pix in + (pix, pixmap) + +let create_d_pixmap window width height = + let depth = (window:GWindow.window)#misc#visual_depth and w = window#misc#window in + let pix = Pixmap.create ~window:w ~width:width ~height:height ~depth:depth () in + let pixmap = new GDraw.drawable pix in + (pix, pixmap) + +(* ============================================================================= *) +(* = Fonction de creation d'une fenetre avec une boite verticale = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = width = largeur de la fenetre = *) +(* = height = hauteur de la fenetre = *) +(* ============================================================================= *) +let create_window title width height = + let window = + if width = 0 && height = 0 then + GWindow.window ~border_width: 1 ~title () + else + GWindow.window ~width ~height ~border_width: 1 ~title () + in + (window, create_vbox window#add) + +(* ============================================================================= *) +(* = Fonction de creation d'une fenetre modale avec une boite verticale = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = width = largeur de la fenetre = *) +(* = height = hauteur de la fenetre = *) +(* ============================================================================= *) +let create_modal_window title width height = + let window = + if width = 0 && height = 0 then + GWindow.window ~modal:true ~border_width: 1 ~title () + else + GWindow.window ~modal:true ~width ~height ~border_width: 1 ~title () + in + (window, create_vbox window#add) + +(* ============================================================================= *) +(* = Fonction de creation d'une fenetre avec une boite verticale. De plus, la = *) +(* = fenetre contient une barre de menu = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = width = largeur de la fenetre = *) +(* = height = hauteur de la fenetre = *) +(* = menubar_items = liste de chaines contenant les titres des menus = *) +(* ============================================================================= *) +let create_window_with_menubar title width height menubar_items = + let (window, vbox) = create_window title width height in + let menubar = GMenu.menu_bar ~packing:vbox#pack () in + let factory = new GMenu.factory menubar in + let menus = List.map (fun str -> factory#add_submenu str) menubar_items in + + (window, vbox, factory, factory#accel_group, Array.of_list menus) + +(* ============================================================================= *) +(* = Fonction de creation d'une fenetre avec une boite verticale. De plus, la = *) +(* = fenetre contient une barre de menu et un menu a gauche pour l'aide = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = width = largeur de la fenetre = *) +(* = height = hauteur de la fenetre = *) +(* = menubar_items = liste de chaines contenant les titres des menus = *) +(* ============================================================================= *) +let create_window_with_menubar_help title width height menubar_items = + let (window, vbox) = create_window title width height in + let hbox = create_hbox vbox#pack in + let menubar = GMenu.menu_bar ~packing:hbox#add () in + let factory = new GMenu.factory menubar in + let menus = List.map (fun str -> factory#add_submenu str) menubar_items in + + let menubar_hlp = GMenu.menu_bar ~packing:(hbox#pack ~from:`END) () in + let factory_hlp = new GMenu.factory menubar_hlp in + let menu_help = factory_hlp#add_submenu "Aide" in + + (window, vbox, factory, factory#accel_group, Array.of_list menus, menu_help) + +(* ============================================================================= *) +(* = Passage en avant-plan d'une fenetre = *) +(* = = *) +(* = window = la fenetre a passer en avant-plan = *) +(* ============================================================================= *) +let window_set_front window = + (* On est oblige de masquer la fenetre avant de la montrer pour etre *) + (* sur qu'elle passe en avant plan. Show tout seul ne suffit pas... *) + GtkBase.Widget.hide (window:GWindow.window)#as_window ; + ignore(window#show ()) + +(* ============================================================================= *) +(* = Renvoie la geometrie d'une fenetre = *) +(* = = *) +(* = window = la fenetre concernee = *) +(* ============================================================================= *) +let get_window_geometry window = + let pos = Window.get_position (window:GWindow.window)#misc#window and + size = Drawable.get_size window#misc#window in + (pos, size) + +(* ============================================================================= *) +(* = Fixe la position x, y d'une fenetre (la deplace si besoin) = *) +(* = = *) +(* = window = la fenetre concernee = *) +(* = (x, y) = la nouvelle position = *) +(* ============================================================================= *) +let set_window_position window (x, y) = + GtkBase.Widget.set_uposition (window:GWindow.window)#as_window ~x:x ~y:y + +(* ============================================================================= *) +(* = Connexion d'un callback lors du deplacement ou de la modification d'une = *) +(* = fenetre = *) +(* = = *) +(* = window = la fenetre concernee = *) +(* = callback = le callback a appeler = *) +(* ============================================================================= *) +let window_modify_connect window callback = + (window:GWindow.window)#event#connect#configure ~callback:(fun ev -> + callback (get_window_geometry window) ; + true) + +(* ============================================================================= *) +(* = Connecte les callbacks lies aux evenements focus_in et focus_out = *) +(* = = *) +(* = window = la fenetre concernee = *) +(* = focus_in = fonction appelee lorsque la souris entre dans le widget = *) +(* = focus_out = fonction appelee lorsque la souris sort du widget = *) +(* ============================================================================= *) +let connect_win_focus_change window focus_in focus_out = + let win = (window:GWindow.window) in + (match focus_in with + None -> () + | Some f -> ignore(win#event#connect#focus_in (fun _ -> f (); false))) ; + (match focus_out with + None -> () + | Some f -> ignore(win#event#connect#focus_out (fun _ -> f (); false))) + +(* ============================================================================= *) +(* = Creation d'une zone de dessin simple (i.e pas de callback associe) = *) +(* = = *) +(* = width = hauteur de la zone = *) +(* = height = hauteur de la zone = *) +(* = pack_method = maniere de placer la zone (ex. : hbox#pack) = *) +(* = pix_expose = pixmap a utiliser pour redessiner apres un event expose = *) +(* ============================================================================= *) +let create_draw_area_simple width height pack_method pix_expose = + (* Creation des widgets *) + let area = GMisc.drawing_area ~width:width ~height:height ~packing:pack_method () in + let drawing = area#misc#realize (); new GDraw.drawable (area#misc#window) in + + (* Creation du event expose : remise en place de la pixmap dans la zone de dessin *) + let area_expose () = drawing#put_pixmap ~x:0 ~y:0 pix_expose in + ignore(area#event#connect#expose ~callback:(fun _ -> area_expose () ; false)) ; + + (* Renvoie la fonction de reaffichage et la zone de dessin *) + (area, area_expose) + +(* ============================================================================= *) +(* = Connexion evenements souris a une zone de dessin = *) +(* = = *) +(* = area = la zone de dessin (create_draw_area_simple) = *) +(* = mouse_press = fonction appelee lors d'un click souris = *) +(* = mouse_move = fonction appelee lors d'un deplacement = *) +(* = mouse_release = fonction appelee lors du relachement d'un bouton = *) +(* ============================================================================= *) +let area_mouse_connect area mouse_press mouse_move mouse_release = + (area:GMisc.drawing_area)#event#add + [`POINTER_MOTION; `BUTTON_PRESS; `BUTTON_RELEASE] ; + area#event#set_extensions `ALL; + ignore(area#event#connect#button_press ~callback:mouse_press) ; + ignore(area#event#connect#motion_notify ~callback:mouse_move) ; + ignore(area#event#connect#button_release ~callback:mouse_release) + +(* ============================================================================= *) +(* = Connexion evenements clavier a une zone de dessin = *) +(* = = *) +(* = area = la zone de dessin (create_draw_area_simple) = *) +(* = key_press = fonction appelee lors de l'appui sur une touche = *) +(* = key_release = fonction appelee lors du relachement d'une touche = *) +(* ============================================================================= *) +let area_key_connect area key_press key_release = + (* Par defaut l'evenement key_release n'est pas associe au widget *) + (area:GMisc.drawing_area)#event#add [`KEY_RELEASE] ; + ignore(area#event#connect#key_press + ~callback:(fun ev -> key_press (GdkEvent.Key.keyval ev))) ; + ignore(area#event#connect#key_release + ~callback:(fun ev -> key_release (GdkEvent.Key.keyval ev))) ; + area#misc#set_can_focus true ; + area#misc#grab_focus () + +(* ============================================================================= *) +(* = Creation d'une zone de dessin avec callbacks = *) +(* = = *) +(* = width : largeur de la zone = *) +(* = height : hauteur de la zone = *) +(* = pack_method : ou mettre la zone (ex : hbox#pack) = *) +(* = pix_expose : pix servant au dessin dans la zone = *) +(* = mouse_press : callback click souris (fun event -> ... ; true) = *) +(* = mouse_move : callback deplacement = *) +(* = mouse_release : callback bouton relache = *) +(* = = *) +(* = En retour, la fonction area_expose permet de forcer l'affichage dans = *) +(* = la zone de dessin = *) +(* ============================================================================= *) +let create_draw_area width height pack_method pix_expose + mouse_press mouse_move mouse_release = + let (area, area_expose) = create_draw_area_simple width height + pack_method pix_expose in + (* Connexion des eventuels signaux generes par la souris *) + area_mouse_connect area mouse_press mouse_move mouse_release ; + + (* Renvoie la fonction de reaffichage *) + area_expose + +(* ============================================================================= *) +(* = Boite de selection d'une couleur = *) +(* = = *) +(* = update_func = la fonction appelee apres selection. La couleur selectionnee= *) +(* = lui est alors passee en parametre = *) +(* ============================================================================= *) +let select_color update_func = + let csd = GWindow.color_selection_dialog ~modal:true ~title:"Selection couleur" () in + ignore(csd#cancel_button#connect#clicked ~callback:csd#destroy); + ignore(csd#ok_button#connect#clicked ~callback:(fun () -> + let color = csd#colorsel#color in + (* Destruction dela fenetre apres selection *) + csd#destroy () ; + update_func (`RGB (Color.red color, Color.green color, Color.blue color)))) ; + csd#show () + +(* ============================================================================= *) +(* = Bouton de couleur permettant la selection d'une autre couleur = *) +(* = = *) +(* = window = fenetre mere = *) +(* = taille_x = largeur du rectangle indiquant la couleur = *) +(* = taille_y = hauteur = *) +(* = color = couleur initiale = *) +(* = pack_method = ou mettre le bouton = *) +(* = callback = fonction appelee apres modification de la couleur = *) +(* ============================================================================= *) +let create_color_selection_button window taille_x taille_y color + pack_method callback = + (* Le pixmap contenant la couleur courante *) + let pm = GDraw.pixmap ~window:window ~width:taille_x ~height:taille_y () in + (* Mise a jour de la couleur initiale *) + (pm:GDraw.pixmap)#set_foreground color ; + (* Dessin avec cette couleur dans le pixmap *) + pm#rectangle ~filled:true ~x:0 ~y:0 ~width:taille_x ~height:taille_y () ; + (* Creation d'un bouton contenant ce pixmap *) + let but = create_pixbutton pm pack_method in + (* Connexion du callback *) + but_connect but (fun () -> select_color callback) ; + + but + +let create_color_selection_button2 window taille_x taille_y color + pack_method callback = + (* Le pixmap contenant la couleur courante *) + let pm = GDraw.pixmap ~window:window ~width:taille_x ~height:taille_y () in + (* Mise a jour de la couleur initiale *) + (pm:GDraw.pixmap)#set_foreground color ; + (* Dessin avec cette couleur dans le pixmap *) + pm#rectangle ~filled:true ~x:0 ~y:0 ~width:taille_x ~height:taille_y () ; + (* Creation d'un bouton contenant ce pixmap *) + let but = create_pixbutton pm pack_method in + (* Connexion du callback *) + but_connect but (fun () -> select_color + (fun c -> pm#set_foreground c ; + pm#rectangle ~filled:true ~x:0 ~y:0 ~width:taille_x ~height:taille_y () ; + callback c)) ; + + but + +(* ============================================================================= *) +(* = Routine interne de creation d'une boite de selection de couleurs = *) +(* ============================================================================= *) +let scw window colors tooltips update_func vbox destroy_func = + (* Sauvegarde des valeurs initiales des couleurs en cas d'annulation *) + let save_colors = + let f c (_, lst) = c @ (List.map (fun (_, couleur, _) -> !couleur) lst) in + Array.of_list(List.fold_left f [] colors) + in + + let taille_x = 40 and taille_y = 10 + and clicked_apply = ref false and application_auto = ref true in + + (vbox:GPack.box)#set_spacing 10 ; + let (scrolled_window, v) = create_scrolled_box vbox#add in + let vb = ref v in + + (* Fonction de creation d'une boite contenant un label de titre *) + (* et un bouton de selection de couleur dont la couleur est color *) + let create_boite v title color callback = + let hbox = create_hbox (v:GPack.box)#pack in + let lab = create_label title hbox#pack and + but = create_color_selection_button (window:GWindow.window) + taille_x taille_y color (hbox#pack ~from:`END) callback in + but + in + + (* Creation des boites de selection de couleurs, pour toutes les frames *) + let rec creation_liste () = + (* Destruction de la liste precedente et recreation dans la scrolled_window *) + vb := change_scrolled_box scrolled_window !vb ; + + List.iter (fun (nom, lst) -> + let v = snd (create_vframe nom !vb#pack) in + let do_boites (title, couleur, _) = + let b = create_boite v title !couleur + (fun color -> couleur := color; + (* Recreation de toute la liste pour mise a jour de la couleur de la boite *) + creation_liste () ; + (* Application automatique ? *) + if !application_auto then begin + clicked_apply := true ; update_func () + end) in + () + in + List.iter do_boites lst) colors + in + + (* Application (i.e redessin par update_func) apres chaque modif ? *) + let check = GButton.check_button ~label:"Appliquer automatiquement" + ~active: !application_auto ~packing:vbox#pack () in + but_connect check (fun () -> application_auto := check#active) ; + + (* Boutons OK/Annulation, ... *) + let hbox = create_hom_hbox vbox#pack in + let but_ok = create_button "Ok" hbox#pack and + but_apply = create_button "Appliquer" hbox#pack and + but_default = create_button "Defaut" hbox#pack and + but_cancel = create_button "Annuler" hbox#pack in + + (* Association de l'aide contextuelle si possible *) + begin + match tooltips with + None -> () + | Some t -> + add_tooltips t but_ok "Valide les changements" ; + add_tooltips t but_apply "Applique les changements" ; + add_tooltips t but_default "Couleurs par defaut" ; + add_tooltips t but_cancel "Annulation des changements" ; + add_tooltips t check "Application des qu'une modification est effectuee" + end ; + + (* Connexion des callbacks des boutons *) + but_connect but_ok (fun () -> + (match destroy_func with None -> () | Some f -> f ()); + if not !application_auto then update_func ()) ; + but_connect but_apply (fun () -> clicked_apply := true; update_func()) ; + but_connect but_default (fun () -> + (* Valeurs par defaut *) + let f (_, lst) = List.iter + (fun (_, couleur, couleur_def) -> couleur := couleur_def) lst in + List.iter f colors ; + clicked_apply := true ; + (* Mise a jour de la liste *) + creation_liste (); + (* Si application automatique on redessine *) + if !application_auto then update_func ()) ; + + but_connect but_cancel (fun () -> + (* Restauration des couleurs initiales *) + let idx = ref 0 in + let f (_, lst) = List.iter (fun (_, couleur, _) -> + couleur := save_colors.(!idx); incr idx) lst in + List.iter f colors ; + (match destroy_func with None -> () | Some f -> f ()); + (* Application des parametres anterieurs si necessaire *) + if !clicked_apply then update_func ()) ; + + (* Premier affichage de la liste des couleurs *) + creation_liste () + +(* ============================================================================= *) +(* = Widget de selection de plusieurs couleurs = *) +(* = = *) +(* = colors = liste de (nom_frame, [(label, couleur ref, couleur_defaut)])= *) +(* = tooltips = None ou (Some tooltips) = *) +(* = update_func = la fonction d'application des nouvelles couleurs = *) +(* = vbox = ou mettre le widget = *) +(* ============================================================================= *) +let select_colors_widget window colors tooltips update_func vbox = + scw window colors tooltips update_func vbox None + +(* ============================================================================= *) +(* = Boite de selection de plusieurs couleurs = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = colors = liste de (nom_frame, [(label, couleur ref, couleur_defaut)])= *) +(* = tooltips = None ou (Some tooltips) = *) +(* = update_func = la fonction d'application des nouvelles couleurs = *) +(* ============================================================================= *) +let select_colors title width height colors tooltips update_func = + (* Creation de la fenetre *) + let (window, vbox) = create_window title width height in + scw window colors tooltips update_func vbox + (Some (fun () -> window#destroy ())); + window#show () + +(* ============================================================================= *) +(* = Fonction interne de creation d'une boite de capture d'ecran = *) +(* ============================================================================= *) +let creation_fen_capture default_filename default_format tooltips with_caption = + let filename = ref default_filename and format = ref default_format in + filename := Gtk_image.set_filename_extension !filename default_format ; + + let lst_formats = [Gtk_image.PNG; Gtk_image.JPEG; Gtk_image.POSTSCRIPT; + Gtk_image.TIFF; Gtk_image.BMP; Gtk_image.PPM] in + (* Correction du format par defaut suivant qu'il est disponible ou pas *) + (* sur l'architecture courante *) + let default_format = + if Gtk_image.is_format_capture_dispo default_format then default_format else begin + let rec f lst = + match lst with + typ::reste -> + if Gtk_image.is_format_capture_dispo typ then typ else f reste + | [] -> default_format + in + let typ = f lst_formats in + filename := Gtk_image.update_extension_capture !filename !format typ ; + typ + end + in + format := default_format ; + + let get_caption = ref (fun () -> None) in + + (* Creation de la boite de dialogue *) + let height = if with_caption then 250 else 210 in + let window = GWindow.dialog ~title:"Capture d'ecran" + ~border_width:10 ~width:350 ~height:height () in + let vbox = window#vbox in + + (* Legende *) + let taille_x = 20 and taille_y = 10 in + + if with_caption then begin + let color_caption = ref (`RGB(65535, 65535, 65535)) and + contour_color = ref (`RGB(65535, 65535, 65535)) and + back_color = ref (`RGB(0, 0, 0)) in + + let (fr, hbox) = create_hframe "Legende" vbox#pack in + let entry_caption = GEdit.entry ~text:"" ~packing:hbox#add () and + hb = ref (create_hbox (hbox#pack ~from:`END)) in + + (* Fonction de creation d'un bouton de selection d'une couleur *) + let create_but col help_text pack_method redraw_func = + let but = create_color_selection_button window taille_x taille_y !col + pack_method (fun color -> col := color; redraw_func ()) in + match tooltips with + None -> () + | Some t -> add_tooltips t but help_text + in + + (* Creation de tous les boutons de selection des couleurs de la legende *) + let rec create_boutons () = + !hb#destroy () ; + hb := create_hbox (hbox#pack ~from:`END) ; + create_but contour_color "Couleur du texte de la legende" + !hb#pack create_boutons; + create_but back_color "Couleur du fond de la boite de legende" + !hb#pack create_boutons; + create_but color_caption "Couleur du contour de la boite de legende" + !hb#pack create_boutons; + in + + create_boutons () ; + set_sensitive !hb false ; + + (* Masquage des boutons de selection de couleur si pas de legende *) + ignore(entry_caption#connect#changed ~callback:(fun () -> + set_sensitive !hb (entry_caption#text <> ""))) ; + (match tooltips with + None -> () | Some t -> add_tooltips t entry_caption + "Texte de la legende (rien=pas de legende)") ; + + (* Recuperation de la legende *) + get_caption := fun () -> + let caption = entry_caption#text in + if caption = "" then None + else Some (caption, !color_caption, !contour_color, !back_color, + get_fixed_font2 ()) + end ; + + (* Selection du format de sauvegarde *) + let (fr, hbox) = create_hframe "Format" vbox#pack in + let lst_but = List.fold_left (fun lst typ -> + let name = Gtk_image.string_of_format_capture typ in + let but = + if lst = [] then GButton.radio_button ~label:name ~packing:hbox#add () + else GButton.radio_button ~label:name ~packing:hbox#add ~group:(List.hd lst)#group () + in + but#set_active (typ = default_format) ; + set_sensitive but (Gtk_image.is_format_capture_dispo typ) ; + begin + match tooltips with + None -> () + | Some t -> + add_tooltips t but + ("Capture au format "^(Gtk_image.extended_string_of_format_capture typ)) + end ; + lst @ [but]) [] lst_formats in + + (* Nom du fichier *) + let pm = GDraw.pixmap_from_xpm_d ~data:open_file_pixmap ~window:window () and + (fr, hbox) = create_hframe "Nom du fichier" vbox#pack in + let entry = GEdit.entry ~text: !filename ~packing:hbox#add () and + but_fic = create_pixbutton pm hbox#pack in + + (* Progression de la sauvegarde *) + let (fr, vb) = create_vframe "Sauvegarde" vbox#pack in + let hbox = create_hbox vb#pack in + let l = create_label " Etat : " hbox#pack and + lab_save = create_label "" hbox#add in + + let hbox = create_hbox vb#pack in + let l = create_label " Progression : " hbox#pack and + pbar_save = GRange.progress_bar ~packing:hbox#add () in + pbar_save#set_fraction 0. ; + + let but_ok = create_button "Capture" window#action_area#add and + but_cancel = create_button "Fermer" window#action_area#add in + but_connect but_cancel (fun () -> window#destroy ()) ; + but_ok#grab_default (); + + (* Fonction appelee lors de la sauvegarde pour afficher la progression *) + let current_progress = ref "" in + let progression_save etape compteur = + begin + match etape with + Gtk_image.INIT -> lab_save#set_text "Sauvegarde en cours..." ; + | Gtk_image.SAVING -> lab_save#set_text "Sauvegarde en cours..." ; + | Gtk_image.FINISHED -> lab_save#set_text "OK" ; + end ; + let p = Printf.sprintf "%.0f%%" (100.*.compteur) in + (* Comme ca on n'affiche que tous les 1%. Sinon c'est tres lent avec GTK2 *) + if !current_progress<>p then begin + pbar_save#set_text p ; + pbar_save#set_fraction compteur ; force_update_interface () ; + current_progress:=p + end + in + + begin + match tooltips with + None -> () + | Some t -> + add_tooltips t but_ok "Effectue la capture d'ecran" ; + add_tooltips t but_cancel "Ferme la fenetre" ; + add_tooltips t entry "Nom du fichier de capture" ; + add_tooltips t but_fic "Selection d'un nom de fichier" ; + end ; + + (* Connexion des callbacks *) + List.iter2 (fun b typ -> + ignore(b#connect#clicked ~callback:(fun () -> + filename := Gtk_image.update_extension_capture !filename !format typ ; + format := typ ; + entry#set_text !filename))) lst_but lst_formats ; + + but_connect but_fic (fun () -> + open_file_dlg "Fichier image" (fun f -> + filename := Gtk_image.set_filename_extension f !format ; + entry#set_text !filename) + None !filename true) ; + + ignore(entry#connect#changed ~callback:(fun () -> filename:=entry#text)) ; + + (* On renvoie les divers parametres et widgets *) + (filename, format, Some progression_save, window, but_ok, entry, !get_caption) + +(* ============================================================================= *) +(* = Boite de capture d'ecran sans legende = *) +(* = = *) +(* = default_filename = nom du fichier par defaut = *) +(* = default_format = format selectionne par defaut (cf capture.ml) = *) +(* = drawable = zone (fenetre ou pixmap) contenant le dessin = *) +(* = tooltips = None ou Some t = *) +(* = x = coordonnee x du point de depart dans la zone = *) +(* = y = coordonnee y de ce point = *) +(* = width = largeur de la zone capturee = *) +(* = height = hauteur de la zone capturee = *) +(* ============================================================================= *) +let screenshot_box default_filename default_format drawable + tooltips x y width height = + + let (filename, format, progression_save, window, but_ok, entry, _) = + creation_fen_capture default_filename default_format tooltips false in + + let do_capture () = + if (String.length !filename)>0 then + Gtk_image.capture_part drawable x y width height !filename !format progression_save + in + + but_connect but_ok do_capture ; + + ignore(entry#connect#activate ~callback:(fun () -> + filename:=Gtk_image.set_filename_extension !filename !format ; + entry#set_text !filename ; + do_capture ())) ; + + (* Affichage de la fenetre *) + window#show () + +(* ============================================================================= *) +(* = Boite de capture d'ecran sans legende qui lance une fonction apres la = *) +(* = capture = *) +(* = = *) +(* = default_filename = nom du fichier par defaut = *) +(* = default_format = format selectionne par defaut (cf capture.ml) = *) +(* = drawable = zone (fenetre ou pixmap) contenant le dessin = *) +(* = tooltips = None ou Some t = *) +(* = x = coordonnee x du point de depart dans la zone = *) +(* = y = coordonnee y de ce point = *) +(* = width = largeur de la zone capturee = *) +(* = height = hauteur de la zone capturee = *) +(* = after_func = fonction a lancer apres la capture = *) +(* ============================================================================= *) +let screenshot_box_with_func default_filename default_format drawable + tooltips x y width height after_func = + + let (filename, format, progression_save, window, but_ok, entry, _) = + creation_fen_capture default_filename default_format tooltips false in + + let do_capture () = + if (String.length !filename)>0 then + Gtk_image.capture_part drawable x y width height !filename !format + progression_save ; + after_func () + in + + but_connect but_ok do_capture ; + + ignore(entry#connect#activate ~callback:(fun () -> + filename:=Gtk_image.set_filename_extension !filename !format ; + entry#set_text !filename ; + do_capture ())) ; + + (* Affichage de la fenetre *) + window#show () + +(* ============================================================================= *) +(* = Boite de capture d'ecran avec legende = *) +(* = Attention, ne fonctionne qu'avec des pixmaps a cause de la legende = *) +(* = = *) +(* = default_filename = nom du fichier par defaut = *) +(* = default_format = format selectionne par defaut (cf capture.ml) = *) +(* = window = la fenetre mere de la zone suivante = *) +(* = drawable = zone (pixmap uniquement) contenant le dessin = *) +(* = tooltips = None ou Some t = *) +(* = x = coordonnee x du point de depart dans la zone = *) +(* = y = coordonnee y de ce point = *) +(* = width = largeur de la zone capturee = *) +(* = height = hauteur de la zone capturee = *) +(* ============================================================================= *) +let screenshot_box_with_caption default_filename default_format + drawable tooltips x y width height = + + let (filename, format, progression_save, window, but_ok, entry, get_caption) = + creation_fen_capture default_filename default_format tooltips true in + + let do_capture () = + if (String.length !filename)>0 then + Gtk_image.capture_part_with_caption window drawable x y width height !filename !format + progression_save (get_caption()) + in + + but_connect but_ok do_capture ; + + ignore(entry#connect#activate ~callback:(fun () -> + filename:=Gtk_image.set_filename_extension !filename !format ; + entry#set_text !filename ; + do_capture ())) ; + + (* Affichage de la fenetre *) + window#show () + +(* ============================================================================= *) +(* = Creation d'une zone de selection de valeur simple (sans callback) = *) +(* = = *) +(* = label = texte du label = *) +(* = lab_width = taille du label = *) +(* = init_value = valeur initiale du texte dans la zone d'entree (chaine) = *) +(* = min_value = valeur min admissible = *) +(* = max_value = valeur max admissible = *) +(* = value_width = taille de la zone d'entree = *) +(* = step_incr = increment lie aux fleches haut/bas = *) +(* = page_incr = increment lie a page up/page down = *) +(* = tip = texte de l'aide contextuelle = *) +(* = tooltips = aide contextuelle = *) +(* = pack_method = ou mettre les widgets = *) +(* ============================================================================= *) +let create_int_spinner_simple label lab_width init_value min_value + max_value value_width step_incr page_incr tip tooltips pack_method = + let hbox = create_hbox pack_method in + let l = create_sized_label label lab_width hbox#pack and + spinner = GEdit.spin_button + ~adjustment:(GData.adjustment ~value:(float_of_int init_value) + ~lower:(float_of_int min_value) ~upper:(float_of_int max_value) + ~step_incr:(float_of_int step_incr) + ~page_incr:(float_of_int page_incr) ~page_size:0.0 ()) + ~rate:0. ~digits:0 ~width:value_width () in + hbox#pack spinner#coerce ; + if tip <> "" then add_tooltips tooltips spinner tip ; + spinner + +(* ============================================================================= *) +(* = Connexion d'un callback a un spinner = *) +(* = = *) +(* = sp = le spinner concerne = *) +(* = callback = le callback a associer = *) +(* ============================================================================= *) +let int_spinner_connect sp callback = + ignore((sp:GEdit.spin_button)#connect#value_changed ~callback:(fun () -> + try let new_value = sp#value_as_int in callback new_value + with Failure("int_of_string") -> ())) + (* Necessaire sous GTK2 pour mettre a jour la valeur quand on la *) + (* modifie en changeant directement le texte de l'entry... *) +(* ignore(sp#connect#changed ~callback:(fun () -> sp#update))*) + +(* ============================================================================= *) +(* = Creation d'une zone de selection de valeur avec callback = *) +(* = = *) +(* = label = texte du label = *) +(* = lab_width = taille du label = *) +(* = init_value = valeur initiale du texte dans la zone d'entree (chaine) = *) +(* = min_value = valeur min admissible = *) +(* = max_value = valeur max admissible = *) +(* = value_width = taille de la zone d'entree = *) +(* = step_incr = increment lie aux fleches haut/bas = *) +(* = page_incr = increment lie a page up/page down = *) +(* = tip = texte de l'aide contextuelle = *) +(* = tooltips = aide contextuelle = *) +(* = pack_method = ou mettre les widgets = *) +(* = callback = callback appele lors de la modification de la zone = *) +(* ============================================================================= *) +let create_int_spinner label lab_width init_value min_value max_value + value_width step_incr page_incr tip tooltips pack_method callback = + let spinner = create_int_spinner_simple label lab_width init_value + min_value max_value value_width step_incr page_incr tip tooltips + pack_method in + int_spinner_connect spinner callback ; + spinner + +(* ============================================================================= *) +(* = Creation d'un slider de selection d'une valeur entiere = *) +(* = = *) +(* = init_val = valeur initiale = *) +(* = min_val = valeur mini acceptee = *) +(* = max_val = valeur maxi acceptee = *) +(* = step = valeur du pas d'incrementation = *) +(* = page = valeur du pas d'incrementation d'une page = *) +(* = draw_val = affichage ou pas de la valeur sur le slider = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_vslider_simple init_val min_val max_val step page draw_val + pack_method = + let adj = GData.adjustment ~lower:(float_of_int min_val) + ~upper:(float_of_int (max_val+10)) + ~step_incr:(float_of_int step) ~page_incr:(float_of_int page) () in + let sc = GRange.scale `VERTICAL ~adjustment:adj ~draw_value:draw_val + ~digits:0 ~packing:pack_method () in + adj#set_value (float_of_int init_val) ; + (adj, sc) + +let create_hslider_simple init_val min_val max_val step page draw_val + pack_method = + let adj = GData.adjustment ~lower:(float_of_int min_val) + ~upper:(float_of_int (max_val+10)) + ~step_incr:(float_of_int step) ~page_incr:(float_of_int page) () in + let sc = GRange.scale `HORIZONTAL ~adjustment:adj ~draw_value:draw_val + ~digits:0 ~packing:pack_method () in + adj#set_value (float_of_int init_val) ; + (adj, sc) + +(* ============================================================================= *) +(* = Connexion d'un callback a un slider = *) +(* = = *) +(* = slider = le widget concerne = *) +(* = callback = le callback a associer = *) +(* ============================================================================= *) +let slider_connect slider callback = + (slider:GData.adjustment)#connect#value_changed ~callback:(fun () -> + callback (int_of_float slider#value)) + +(* ============================================================================= *) +(* = Creation d'un slider de selection d'une valeur entiere avec callback = *) +(* = = *) +(* = init_val = valeur initiale = *) +(* = min_val = valeur mini acceptee = *) +(* = max_val = valeur maxi acceptee = *) +(* = step = valeur du pas d'incrementation = *) +(* = page = valeur du pas d'incrementation d'une page = *) +(* = draw_val = affichage ou pas de la valeur sur le slider = *) +(* = pack_method = ou mettre le widget = *) +(* = callback = le callback a associer = *) +(* ============================================================================= *) +let create_vslider init_val min_val max_val step page draw_val + pack_method callback = + let slider = create_vslider_simple init_val min_val max_val + step page draw_val pack_method in + ignore(slider_connect (fst slider) callback) ; + slider + +let create_hslider init_val min_val max_val step page draw_val + pack_method callback = + let slider = create_hslider_simple init_val min_val max_val + step page draw_val pack_method in + ignore(slider_connect (fst slider) callback) ; + slider + +let create_float_spinner_simple label lab_width init_value min_value + max_value value_width nb_digits step_incr page_incr tip tooltips pack_method = + let hbox = create_hbox pack_method in + let l = create_sized_label label lab_width hbox#pack and + spinner = GEdit.spin_button + ~adjustment:(GData.adjustment ~value:init_value + ~lower:min_value ~upper:max_value + ~step_incr:step_incr ~page_incr:page_incr + ~page_size:0.0 ()) + ~rate:0. ~digits:nb_digits ~width:value_width () in + hbox#pack spinner#coerce ; + if tip <> "" then add_tooltips tooltips spinner tip ; + spinner + +let float_spinner_connect sp callback = + ignore((sp:GEdit.spin_button)#connect#value_changed ~callback:(fun () -> + try let new_value = sp#value in callback new_value ; + with Failure("float_of_string") -> ())) + (* Necessaire sous GTK2 pour mettre a jour la valeur quand on la *) + (* modifie en changeant directement le texte de l'entry... *) +(* ignore(sp#connect#changed ~callback:(fun () -> sp#update))*) + +let create_float_spinner label lab_width init_value min_value max_value + value_width nb_digits step_incr page_incr tip tooltips pack_method callback = + let spinner = create_float_spinner_simple label lab_width init_value + min_value max_value value_width nb_digits step_incr page_incr tip tooltips + pack_method in + float_spinner_connect spinner callback ; + spinner + +(* ============================================================================= *) +(* = Creation d'une zone de selection de texte = *) +(* = = *) +(* = label = texte du label = *) +(* = lab_width = taille du label = *) +(* = init_value = valeur initiale du texte dans la zone d'entree (chaine) = *) +(* = value_width = taille de la zone d'entree = *) +(* = tip = texte de l'aide contextuelle = *) +(* = tooltips = aide contextuelle = *) +(* = pack_method = ou mettre les widgets = *) +(* ============================================================================= *) +let create_text_entry_simple label lab_width init_value value_width + tip tooltips pack_method = + let hbox = create_hbox pack_method in + let lab = create_sized_label label lab_width hbox#pack and + entry = GEdit.entry ~text:init_value ~width:value_width ~packing:hbox#pack () in + if tip <> "" then add_tooltips tooltips entry tip ; + (lab, entry) + +(* ============================================================================= *) +(* = Attachement d'un callback lorsqu'on appuie sur entree dans l'entry = *) +(* = = *) +(* = entry = le widget concerne = *) +(* = callback = le callback a appeler = *) +(* ============================================================================= *) +let text_entry_connect entry callback = + (entry:GEdit.entry)#connect#activate + ~callback:(fun () -> callback (String.uppercase entry#text)) + +(* ============================================================================= *) +(* = Attachement d'un callback lorsqu'on modifie le texte dans l'entry = *) +(* = = *) +(* = entry = le widget concerne = *) +(* = callback = le callback a appeler = *) +(* ============================================================================= *) +let text_entry_connect_modify entry callback = + (entry:GEdit.entry)#connect#changed + ~callback:(fun () -> callback (String.uppercase entry#text)) + +(* ============================================================================= *) +(* = Creation d'une zone de selection de texte = *) +(* = = *) +(* = label = texte du label = *) +(* = lab_width = taille du label = *) +(* = init_value = valeur initiale du texte dans la zone d'entree (chaine) = *) +(* = value_width = taille de la zone d'entree = *) +(* = tip = texte de l'aide contextuelle = *) +(* = tooltips = aide contextuelle = *) +(* = pack_method = ou mettre les widgets = *) +(* = callback = callback appele lors de la modification de la zone = *) +(* ============================================================================= *) +let create_text_entry label lab_width init_value value_width + tip tooltips pack_method callback = + let (lab, entry) = create_text_entry_simple label lab_width + init_value value_width tip tooltips pack_method in + ignore(text_entry_connect_modify entry callback) ; + (lab, entry) + +(* Type pour les fenetres enregistrees *) +type registered_win = {reg_win_id : int ; + mutable reg_win_handle : GWindow.window option ; + reg_win_build : unit -> GWindow.window} + +(* Compteur pour les fenetres enregistrees *) +let register_count = ref 0 +(* Tableau contenant les fenetres enregistrees *) +let registered_windows = ref ([||] : registered_win array) + +(* Exception levee lors de l'appel a un numero de fenetre non enregistree *) +exception GTK_TOOLS_UNREGISTERED_WINDOW of int + +(* ============================================================================= *) +(* = Enregistrement d'une fenetre = *) +(* = = *) +(* = build_window_func = la fonction de creation de la fenetre. Cette fonction = *) +(* = doit renvoyer un objet du type GWindow.window = *) +(* = = *) +(* = Renvoie un identifiant (entier) pour cette fenetre = *) +(* ============================================================================= *) +let register_window build_window_func = + let this_win_num = !register_count in + incr register_count ; + + registered_windows := Array.append !registered_windows + [|{reg_win_id = this_win_num; reg_win_handle = None; + reg_win_build = build_window_func}|] ; + + this_win_num + +(* ============================================================================= *) +(* = Appel d'une fenetre enregistree = *) +(* = = *) +(* = id = identifiant de la fenetre (obtenu par register_window) = *) +(* ============================================================================= *) +let show_registered_window id = + if id < 0 or id >= !register_count then + raise (GTK_TOOLS_UNREGISTERED_WINDOW id) ; + + let w = !registered_windows.(id) in + match w.reg_win_handle with + None -> (* La fenetre n'avait pas encore ete creee, on le fait *) + (* Appel de la fonction de creation de la fenetre *) + let win = w.reg_win_build () in + ignore(win#connect#destroy ~callback:(fun _ -> w.reg_win_handle <- None)) ; + w.reg_win_handle <- Some win ; + win#show () + + | Some w -> (* La fenetre etait deja creee, on la met en avant-plan *) + window_set_front w + +(* ============================================================================= *) +(* = Cache (detruit) une fenetre enregistree = *) +(* = = *) +(* = id = identifiant de la fenetre (obtenu par register_window) = *) +(* ============================================================================= *) +let hide_registered_window id = + if id < 0 or id >= !register_count then + raise (GTK_TOOLS_UNREGISTERED_WINDOW id) ; + + let w = !registered_windows.(id) in + match w.reg_win_handle with + None -> () + | Some win -> win#destroy () ; w.reg_win_handle <- None + +(* ============================================================================= *) +(* = Renvoie une fenetre enregistree = *) +(* = = *) +(* = id = identifiant de la fenetre (obtenu par register_window) = *) +(* ============================================================================= *) +let get_registered_window id = + if id < 0 or id >= !register_count then + raise (GTK_TOOLS_UNREGISTERED_WINDOW id) ; + + (!registered_windows.(id)).reg_win_handle + +(* ============================================================================= *) +(* = Fonction effectuant le scrolling d'un adjustment (scrollbar) = *) +(* ============================================================================= *) +let scroll_adjustment adj scroll_up delta = + (* Valeur extremes que peut prendre l'adjustment *) + let val_min = adj#lower and val_max = adj#upper -. adj#page_size in + let current_value = adj#value in + let new_value = + if scroll_up then max val_min (current_value-.delta) + else min val_max (current_value+.delta) + in + if new_value<>current_value then adj#set_value new_value + +(* ============================================================================= *) +(* = Connexion d'un scroll aux mouvements de la molette de la souris = *) +(* ============================================================================= *) +let connect_mouse_wheel_scroll widget sb delta = + let check_wheel ev = + match GdkEvent.Scroll.direction ev with + `UP -> scroll_adjustment sb#adjustment true delta; true + | `DOWN -> scroll_adjustment sb#adjustment false delta; true + | _ -> false + in + + (* Connexion a present identique pour Unix et Windows... *) + ignore(widget#event#connect#scroll ~callback:check_wheel) + +(* ============================================================================= *) +(* = Creation d'un objet clist = *) +(* = = *) +(* = lst_titles = liste des titres de chaque colonne = *) +(* = sortable_titles = tri possible ou pas en cliquant sur les titres ? = *) +(* = first_sort = indique quelle est la colonne triee par defaut = *) +(* = pack_method = ou mettre la liste = *) +(* ============================================================================= *) +let create_list lst_titles (sortable_titles, first_sort) pack_method = + let hb = create_hbox pack_method in + let sb = GRange.scrollbar `VERTICAL ~packing:(hb#pack ~from:`END) () in + let clist = GList.clist ~titles:lst_titles ~shadow_type:`OUT + ~packing:hb#add ~vadjustment:sb#adjustment () in + + if sortable_titles then begin + (* Sens de tri de chaque colonne *) + let sens_tri = Array.mapi (fun i _ -> i <> first_sort) + (Array.of_list lst_titles) in + let select_column column = + let dir = if sens_tri.(column) then `ASCENDING else `DESCENDING in + clist#set_sort ~column:column ~dir:dir () ; + clist#sort () ; + (* Inversion du sens de tri pour la prochaine fois *) + sens_tri.(column) <- not sens_tri.(column) + in + ignore(clist#connect#click_column ~callback:(fun col -> select_column col)) ; + end ; + + (* Autorisation du defilement avec la molette de la souris *) + prerr_endline "TODO (Gtk_tools): connect_mouse_wheel_scroll clist sb 15. ;"; + clist + +(* ============================================================================= *) +(* = Creation d'un objet clist avec scroll horizontal = *) +(* = = *) +(* = lst_titles = liste des titres de chaque colonne = *) +(* = sortable_titles = tri possible ou pas en cliquant sur les titres ? = *) +(* = first_sort = indique quelle est la colonne triee par defaut = *) +(* = pack_method = ou mettre la liste = *) +(* ============================================================================= *) +let create_list_with_hor_scroll lst_titles + (sortable_titles, first_sort) pack_method = + let vb = create_vbox pack_method in + let hb = create_hbox vb#add in + let sb = GRange.scrollbar `VERTICAL ~packing:(hb#pack ~from:`END) () in + let sb2 = GRange.scrollbar `HORIZONTAL ~packing:(vb#pack ~from:`END) () in + let clist = GList.clist ~titles:lst_titles ~shadow_type:`OUT + ~packing:hb#add ~vadjustment:sb#adjustment ~hadjustment:sb2#adjustment () in + if sortable_titles then begin + (* Sens de tri de chaque colonne *) + let sens_tri = Array.mapi (fun i _ -> i <> first_sort) + (Array.of_list lst_titles) in + let select_column column = + let dir = if sens_tri.(column) then `ASCENDING else `DESCENDING in + clist#set_sort ~column:column ~dir:dir () ; + clist#sort () ; + (* Inversion du sens de tri pour la prochaine fois *) + sens_tri.(column) <- not sens_tri.(column) + in + ignore(clist#connect#click_column ~callback:(fun col -> select_column col)) ; + end ; + + (* Autorisation du defilement avec la molette de la souris *) + prerr_endline "TODO (Gtk_tools): connect_mouse_wheel_scroll clist sb 15.;"; + clist + +(* ============================================================================= *) +(* = Ajout de callbacks a un objet clist = *) +(* = = *) +(* = clist = la liste = *) +(* = callback_select = callback de selection d'une ligne = *) +(* = callback_deselect = callback de deselection d'une ligne = *) +(* = callback_select_column = callback de selection d'un titre de colonne = *) +(* ============================================================================= *) +let list_connect clist + callback_select callback_deselect callback_select_column = + (* Selection d'un element *) + (match callback_select with + None -> () + | Some callback -> + ignore((clist:string GList.clist)#connect#select_row ~callback: + (fun ~row ~column ~event -> callback row column))) ; + + (* Deselection d'un element *) + (match callback_deselect with + None -> () + | Some callback -> + ignore(clist#connect#unselect_row ~callback: + (fun ~row ~column ~event -> callback row column))) ; + + (* Click sur un titre *) + (match callback_select_column with + None -> () + | Some callback -> + ignore(clist#connect#click_column ~callback:(fun col -> callback col))) + +(* ============================================================================= *) +(* = Ajout de callbacks a un objet clist avec test du double click = *) +(* = = *) +(* = clist = la liste = *) +(* = callback_select = callback de selection d'une ligne = *) +(* = callback_deselect = callback de deselection d'une ligne = *) +(* = callback_select_column = callback de selection d'un titre de colonne = *) +(* ============================================================================= *) +let list_connect_check_dbl_click clist + callback_select callback_deselect callback_select_column = + let test_dbl_click event = + match event with None -> false | Some ev -> check_dbl_click ev + in + + (* Selection d'un element *) + (match callback_select with + None -> () + | Some callback -> + ignore((clist:string GList.clist)#connect#select_row ~callback: + (fun ~row ~column ~event -> callback row column + (test_dbl_click event)))) ; + + (* Deselection d'un element *) + (match callback_deselect with + None -> () + | Some callback -> + ignore(clist#connect#unselect_row ~callback: + (fun ~row ~column ~event -> callback row column + (test_dbl_click event)))) ; + + (* Click sur un titre *) + (match callback_select_column with + None -> () + | Some callback -> + ignore(clist#connect#click_column ~callback:(fun col -> callback col))) + +(* ============================================================================= *) +(* = Ajout de callbacks d'appui sur une touche dans une clist = *) +(* = = *) +(* = clist = la liste = *) +(* = callback_up = callback appui fleche haut = *) +(* = callback_down = callback appui fleche bas = *) +(* ============================================================================= *) +let list_connect_up_down_keys clist callback_up callback_down = + let key_press key = + if key = _Down then (callback_down (); true) + else if key = _Up then (callback_up (); true) + else false + in + ignore((clist:string GList.clist)#event#connect#key_press + ~callback:(fun ev -> key_press (GdkEvent.Key.keyval ev))) + +(* ============================================================================= *) +(* = Creation d'une liste avec titres+largeurs colonnes+label pour nb d'elts = *) +(* = = *) +(* = title_sizes = liste des (nom, taille) des colonnes = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_managed_list titles_sizes pack_method = + let vb = create_vbox pack_method in + let lab = create_label "" vb#pack in + + (* Creation de la liste *) + let titles = List.map fst titles_sizes in + let lst = create_list titles (true, 0) vb#add in + + let column_sizes = Array.of_list (List.map snd titles_sizes) in + (* Mise a jour de la taille des colonnes *) + Array.iteri (fun i size -> lst#set_column i ~width:size) column_sizes ; + + (lst, lab) + +(* ============================================================================= *) +(* = Mise a jour des tailles des colonnes d'une liste = *) +(* ============================================================================= *) +let set_columns_sizes (lst: string GList.clist) (sizes: int list) = + let i = ref 0 in + List.iter (fun width -> lst#set_column !i ~width ; incr i) sizes + +(* ============================================================================= *) +(* = Connection de callback a la liste precedente = *) +(* = = *) +(* = (lst, lab) = la liste et le label cree par la fct precedente = *) +(* = item_column = colonne ou se trouve l'item de reference = *) +(* = selection_callback = callback appele lors de la selection d'un element = *) +(* = (name, female, cap) = regle d'affichage du label du nombre d'elements = *) +(* ============================================================================= *) +let connect_managed_list (lst, lab) item_column selection_callback + (name, female, cap) = + let eval_string base_string n female first_char_upper = + let get_upper_and_female str = + let str = if female then str^"e" else str in + if first_char_upper then String.capitalize str + else str + in + match n with + 0 -> let s = get_upper_and_female "aucun" in s ^ " " ^ base_string + | 1 -> let s = get_upper_and_female "un" in s ^ " " ^ base_string + | _ -> if String.get base_string (String.length base_string -1) = 'x' then + Printf.sprintf "%d %s" n base_string + else Printf.sprintf "%d %ss" n base_string + in + + let last_row = ref (-1) and id_selected = ref "" in + let reset_selection () = last_row := (-1) ; id_selected := "" in + + (* Selections dans la liste *) + let select_func selection_type row column dbl_click = + if selection_type then begin + id_selected := lst#cell_text row item_column ; last_row := row + end else reset_selection () ; + selection_callback !id_selected (row, column, dbl_click) selection_type + in + + (* Connexion des callbacks de la liste *) + list_connect_check_dbl_click lst + (Some (select_func true)) (Some (select_func false)) None ; + + (* Ajout de cette fonction pour GTK2. En GTK1, ca scrolle tout seul *) + (* quand necessaire, pas en GTK2... *) + let scroll_if_needed () = + if !last_row<>(-1) then + match lst#row_is_visible !last_row with + `NONE | `PARTIAL -> lst#moveto !last_row item_column + | `FULL -> () + in + + let nb_elts = ref 0 in + (* Callback permettant de naviguer dans la liste avec les fleches haut/bas *) + let key_up () = + if !last_row<> -1 && !last_row <> 0 then begin + lst#select (!last_row-1) item_column ; + scroll_if_needed () + end + in + let key_down () = + if !last_row<> -1 && !last_row <> (!nb_elts-1) then begin + lst#select (!last_row+1) item_column ; + scroll_if_needed () + end + in + list_connect_up_down_keys lst key_up key_down ; + + let fill_list select_id lst_items = + lst#clear () ; lst#freeze () ; + reset_selection () ; + let row = ref 0 and selected_row = ref (-1) in + List.iter (fun l -> + ignore(lst#append l) ; + (* Est-ce la ligne de l'element a selectionner ? *) + if List.nth l item_column = select_id then selected_row := !row ; + incr row) lst_items ; + + nb_elts := !row ; + (lab:GMisc.label)#set_text (eval_string name !nb_elts female cap) ; + lst#thaw () ; + + (* Selection de l'element selectionne si c'est demande *) + if !selected_row <> -1 then lst#select !selected_row item_column + in + + fill_list + +(* Type Lat/Lon en degres ou degres/minutes/secondes *) +type t_val = + G_LAT (* Latitude en degres flottants *) + | G_LON (* Longitude en degres flottants *) + | G_LAT_D (* Latitude : degres *) + | G_LAT_M (* Latitude : minutes *) + | G_LAT_S (* Latitude : secondes *) + | G_LAT_NS (* Latitude : orientation Nord/Sud *) + | G_LON_D (* Longitude : degres *) + | G_LON_M (* Longitude : minutes *) + | G_LON_S (* Longitude : secondes *) + | G_LON_EW (* Longitude : orientation Est/Ouest *) + +(* Type contenant un widget de selection de coordonnees lat/lon *) +type latlon = {latlon_lat_val : float ref ; + latlon_lon_val : float ref ; + latlon_update : float -> float -> unit ; + latlon_change_callback : (unit -> unit) option ref} + +(* ============================================================================= *) +(* = Creation d'un widget de selection de coordonnees lat/lon = *) +(* = = *) +(* = (lat_in, lon_in) = position initiale en degres = *) +(* = pack_method = ou mettre le widget = *) +(* = tooltips = aide contextuelle = *) +(* = = *) +(* = Renvoie un widget de type latlon = *) +(* ============================================================================= *) +let create_latlon_selection (lat_in, lon_in) pack_method tooltips = + let degminsec_of_pos d = + let (d, dd, signe) = + if d < 0.0 then (-.d, int_of_float (-.d), (-1.0)) else (d, int_of_float d, 1.0) + in + let reste = d-.(float_of_int dd) in + let mm = int_of_float (reste*.60.0) in + let ss = (reste-.(float_of_int mm)/.60.0)*.3600.0 in + (* Evite d'avoir 49.3 -> 49 17 60.00 *) + let (mm, ss) = if ss > 59.9999 then (mm+1, 0.0) else (mm, ss) in + (dd, mm, ss, signe) in + + let pos_of_degminsec (d, m, s, signe) = + ((float_of_int d)+.(float_of_int m)/.60.0+.s/.3600.0) *. signe in + + (* Fonction de creation des menus de choix N/S et E/W *) + let menu_NS_EW options signe pack_method = + let menu = GMenu.menu () in + let l = List.map (fun op -> + (GMenu.menu_item ~label:op ~packing:menu#append (), op)) options in + let optionmenu = GMenu.option_menu ~packing:pack_method () in + optionmenu#set_menu menu ; + + (* Si signe est negatif alors S ou W. On selectionne par defaut le menu *) + (* correspondant *) + if signe<0.0 then optionmenu#set_history 1 ; + + (optionmenu, l) + in + + (* Initialisation des variables *) + let lat = ref lat_in and lon = ref lon_in in + let (a, b, c, s) = degminsec_of_pos lat_in in + let lat_d = ref a and lat_m = ref b and lat_s = ref c and lat_signe = ref s in + let (a, b, c, s) = degminsec_of_pos lon_in in + let lon_d = ref a and lon_m = ref b and lon_s = ref c and lon_signe = ref s in + + (* Creation des widgets *) + let box = create_spaced_hbox pack_method in + + (* Latitudes *) + let b = snd (create_vframe "Latitude" box#pack) in + let e_lat = create_float_spinner_simple "" 0 + !lat (-90.0) 90.0 210 4 0.1 1.0 + "Latitude en degres (flottant)" tooltips b#pack and + hb = GPack.hbox ~packing:b#pack ~spacing:5 ~border_width:5 () in + let e_lat_d = create_int_spinner_simple "" 0 !lat_d 0 90 40 1 5 + "Latitude : degres" tooltips hb#pack and + e_lat_m = create_int_spinner_simple "" 0 !lat_m 0 59 40 1 5 + "Latitude : minutes" tooltips hb#pack and + e_lat_s = create_float_spinner_simple "" 0 + !lat_s 0.0 59.99 55 2 0.1 1.0 + "Latitude : secondes" tooltips hb#pack and + (o_lat, m_lat) = menu_NS_EW ["N"; "S"] !lat_signe hb#pack in + + (* Longitudes *) + let b = snd (create_vframe "Longitude" box#pack) in + let e_lon = create_float_spinner_simple "" 0 + !lon (-180.0) 180.0 210 4 0.1 1.0 + "Longitude en degres (flottant)" tooltips b#pack and + hb = GPack.hbox ~packing:b#pack ~spacing:5 ~border_width:5 () in + let e_lon_d = create_int_spinner_simple "" 0 !lon_d 0 180 40 1 5 + "Longitude : degres" tooltips hb#pack and + e_lon_m = create_int_spinner_simple "" 0 !lon_m 0 59 40 1 5 + "Longitude : minutes" tooltips hb#pack and + e_lon_s = create_float_spinner_simple "" 0 + !lon_s 0.0 59.99 55 2 0.1 1.0 + "Longitude : secondes" tooltips hb#pack and + (o_lon, m_lon) = menu_NS_EW ["E"; "W"] !lon_signe hb#pack in + + (* Callbacks de modification *) + let updating = ref false in + + (* Fonctions de coherence des differents affichages *) + let update_from_latlon () = + let (a, b, c, signe) = degminsec_of_pos !lat in + lat_d := a; lat_m := b; lat_s := c ; lat_signe := signe ; + + e_lat_d#set_value (float_of_int !lat_d) ; + e_lat_m#set_value (float_of_int !lat_m) ; + e_lat_s#set_value !lat_s ; + + let m = if signe > 0.0 then 0 else 1 in + o_lat#set_history m ; + let (a, b, c, signe) = degminsec_of_pos !lon in + lon_d := a; lon_m := b; lon_s := c ; lon_signe := signe ; + + e_lon_d#set_value (float_of_int !lon_d) ; + e_lon_m#set_value (float_of_int !lon_m) ; + e_lon_s#set_value !lon_s ; + + let m = if signe > 0.0 then 0 else 1 in + o_lon#set_history m + in + let update_from_latlon_dms () = + lat := pos_of_degminsec (!lat_d, !lat_m, !lat_s, !lat_signe) ; + lon := pos_of_degminsec (!lon_d, !lon_m, !lon_s, !lon_signe) ; + e_lat#set_value !lat ; e_lon#set_value ! lon ; + in + + let change_callback = ref None in + + let update type_valeur t = + if not !updating then begin + updating := true ; + begin + match type_valeur with + G_LAT -> lat := t ; update_from_latlon () + | G_LON -> lon := t ; update_from_latlon () + | G_LAT_D -> lat_d := int_of_float t ; update_from_latlon_dms () + | G_LAT_M -> lat_m := int_of_float t ; update_from_latlon_dms () + | G_LAT_S -> lat_s := t ; update_from_latlon_dms () + | G_LON_D -> lon_d := int_of_float t ; update_from_latlon_dms () + | G_LON_M -> lon_m := int_of_float t ; update_from_latlon_dms () + | G_LON_S -> lon_s := t ; update_from_latlon_dms () + | G_LAT_NS -> lat_signe := t ; update_from_latlon_dms () + | G_LON_EW -> lon_signe := t ; update_from_latlon_dms () + end ; + updating := false ; + match !change_callback with None -> () | Some callback -> callback () + end + in + + let connect_sp spinner t = + spinner#connect#value_changed ~callback:(fun () -> + try let new_value = spinner#value in update t new_value + with Failure("float_of_string") -> ()) + in + + ignore(connect_sp e_lat G_LAT) ; ignore(connect_sp e_lon G_LON) ; + ignore(connect_sp e_lat_d G_LAT_D) ; ignore(connect_sp e_lat_m G_LAT_M) ; + ignore(connect_sp e_lat_s G_LAT_S) ; ignore(connect_sp e_lon_d G_LON_D) ; + ignore(connect_sp e_lon_m G_LON_M) ; ignore(connect_sp e_lon_s G_LON_S) ; + + let connect_menu_NS_EW l = + List.iter (fun (menuitem, op) -> + ignore(menuitem#connect#activate ~callback:(fun () -> + if op = "N" then update G_LAT_NS 1.0 ; + if op = "S" then update G_LAT_NS (-1.0) ; + if op = "E" then update G_LON_EW 1.0 ; + if op = "W" then update G_LON_EW (-1.0)))) l + in + + connect_menu_NS_EW m_lat ; + connect_menu_NS_EW m_lon ; + + {latlon_lat_val = lat; latlon_lon_val = lon; + latlon_update = (fun lat lon -> update G_LAT lat; update G_LON lon; + update G_LAT_D (float_of_int !lat_d); update G_LON_D (float_of_int !lon_d)) ; + latlon_change_callback = change_callback} + +(* ============================================================================= *) +(* = Mise a jour des valeurs dans un widget de selection de coordonnees lat/lon= *) +(* = = *) +(* = latlon_widget = le widget de selection de coordonnees lat/lon = *) +(* = new_lat = nouvelle latitude (en degres) = *) +(* = new_lon = nouvelle longitude (en degres) = *) +(* ============================================================================= *) +let update_latlon_selection latlon_widget new_lat new_lon = + latlon_widget.latlon_update new_lat new_lon + +(* ============================================================================= *) +(* = Renvoie les coordonnees choisies dans un widget lat/lon = *) +(* = = *) +(* = latlon_widget = le widget de selection de coordonnees lat/lon = *) +(* = = *) +(* = Renvoie une paire contenant la latitude et la longitude en degres = *) +(* ============================================================================= *) +let latlon_selection_get latlon_widget = + (!(latlon_widget.latlon_lat_val), !(latlon_widget.latlon_lon_val)) + +(* ============================================================================= *) +(* = Connecte un callback appele lors d'une modif dans le widget lat/lon = *) +(* = = *) +(* = latlon_widget = le widget de selection de coordonnees lat/lon = *) +(* = callback = le callback = *) +(* ============================================================================= *) +let latlon_selection_change latlon_widget callback = + latlon_widget.latlon_change_callback := Some callback + +(* Numero de la fenetre de log enregistree *) +let id_log_win = ref (-1) + +(* Niveau de verbose dans la fenetre de log *) +let log_verbose_level = ref 100 + +(* Zone de texte de la fenetre du log quand elle existe *) +let log_wid = ref None + +(* Exception levee lors de l'ajout de texte dans la fenetre de log alors que + celle-ci n'a pas encore ete creee *) +exception GTK_TOOLS_NO_LOG_WIN + +(* ============================================================================= *) +(* = Creation d'une zone de texte = *) +(* = = *) +(* = editable = zone editable par l'utilisateur = *) +(* = with_vert_scroll = presence d'une barre de defilement verticale = *) +(* = with_hor_scroll = presence d'une barre de defilement horizontale = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_text_edit editable with_vert_scroll with_hor_scroll + pack_method = + let hpolicy = if with_hor_scroll then `ALWAYS else `NEVER + and vpolicy = if with_vert_scroll then `ALWAYS else `NEVER in + let text = GText.view ~editable () in + let sw = GBin.scrolled_window ~packing:pack_method ~hpolicy ~vpolicy () in + sw#add text#coerce; + text + +(* ============================================================================= *) +(* = Efface une zone editable = *) +(* ============================================================================= *) +let text_edit_clear edit = + let (start,stop) = edit#buffer#bounds in edit#buffer#delete ~start ~stop + +(* ============================================================================= *) +(* = Renvoie le texte d'une zone editable = *) +(* ============================================================================= *) +let text_edit_get_text edit = (edit:GText.view)#buffer#get_text () + +(* ============================================================================= *) +(* = Renvoie le texte d'une zone editable sous la forme d'une liste de chaines = *) +(* = de caracteres correspondant aux differentes lignes = *) +(* ============================================================================= *) +let text_edit_get_lines edit = + let text = text_edit_get_text edit in + let l = split2 '\n' text in + (* Supprime la derniere ligne vide au besoin *) + if l<>[] && List.hd (List.rev l) = "" then List.rev (List.tl (List.rev l)) + else l + +(* ============================================================================= *) +(* = Met a jour le texte d'une zone editable = *) +(* ============================================================================= *) +let text_edit_set_text_list edit text_lst = + List.iter (fun s -> (edit:GText.view)#buffer#insert (s^"\n")) text_lst + +let text_edit_set_text edit text = (edit:GText.view)#buffer#insert text + +(* ============================================================================= *) +(* = Fonction interne : renvoie le widget text du log = *) +(* ============================================================================= *) +let get_log_wid () = + match !log_wid with None -> raise GTK_TOOLS_NO_LOG_WIN | Some w -> w + +(* ============================================================================= *) +(* = Ajout d'un texte dans le log = *) +(* = = *) +(* = text = texte du message a afficher = *) +(* = level = niveau de priorite du message = *) +(* ============================================================================= *) +let add_log text level = + if level < !log_verbose_level then begin + let zone = get_log_wid () and + time = timer_string_of_time (timer_get_time ()) in + (zone:GText.view)#buffer#insert (time ^ " : " ^ text) + end + +(* ============================================================================= *) +(* = Ajout d'un texte avec couleur dans le log = *) +(* = = *) +(* = text = texte du message a afficher = *) +(* = level = niveau de priorite du message = *) +(* = color = la couleur a utiliser = *) +(* ============================================================================= *) +let add_log_with_color text level color = + if level < !log_verbose_level then begin + let zone = get_log_wid () and + time = timer_string_of_time (timer_get_time ()) in + ignore ((zone:GText.view)#buffer#create_tag ~name:"color" + [`FOREGROUND_GDK (GDraw.color color)]); + zone#buffer#insert ~tag_names:["color"] (time ^ " : " ^ text) + end + +(* ============================================================================= *) +(* = Effacement du contenu du log = *) +(* ============================================================================= *) +let clear_log () = text_edit_clear (get_log_wid ()) + +(* ============================================================================= *) +(* = Creation de la fenetre de log = *) +(* = = *) +(* = tooltips = systeme d'aide contextuelle = *) +(* ============================================================================= *) +let create_log tooltips = + let rec in_build_log tooltips = + let (win, box) = create_window "Log" 400 300 in + + let log = create_text_edit false true true box#add in + log_wid := Some log ; + + let bbox = create_bbox box#pack in + let but_ok = create_button "OK" bbox#add and + but_clear = create_button "Effacer" bbox#add and + but_save = create_button "Sauver" bbox#add in + add_tooltips tooltips but_ok "Fermer la fenetre de log" ; + add_tooltips tooltips but_clear + "Effacer le contenu de la fenetre de log" ; + add_tooltips tooltips but_save + "Sauver le contenu de la fenetre de log" ; + but_connect but_clear (fun () -> clear_log ()) ; + but_connect but_ok (fun () -> + hide_registered_window !id_log_win) ; + but_connect but_save (fun () -> + open_file_dlg "Sauvegarde du log" + (fun filename -> + if log#buffer#char_count>0 then begin + let data = text_edit_get_text log in + let c = open_out filename in + Printf.fprintf c "%s" data ; + close_out c + end) None "log.txt" true) ; + + (* Pour que le bouton de destruction de la fenetre ne fasse *) + (* que la cacher et non la detruire reellement *) + ignore(win#connect#destroy ~callback:(fun () -> + id_log_win := register_window (fun () -> in_build_log tooltips))) ; + + win + in + id_log_win := register_window (fun () -> in_build_log tooltips) ; + + let win = in_build_log tooltips in + let w = !registered_windows.(!id_log_win) in + ignore(win#connect#destroy ~callback:(fun _ -> w.reg_win_handle <- None)) ; + w.reg_win_handle <- Some win + +(* ============================================================================= *) +(* = Affichage de la fenetre de log = *) +(* = = *) +(* = tooltips = systeme d'aide contextuelle = *) +(* ============================================================================= *) +let show_log () = + if !id_log_win <> -1 then show_registered_window !id_log_win + +(* ============================================================================= *) +(* = Cache la fenetre de log = *) +(* ============================================================================= *) +let hide_log () = hide_registered_window !id_log_win + +(* ============================================================================= *) +(* = Mise a jour du niveau d'affichage des messages dans le log = *) +(* = = *) +(* = level = le niveau de priorite = *) +(* ============================================================================= *) +let set_log_verbose_level level = log_verbose_level := level + +(* ============================================================================= *) +(* = Affichage du contenu d'un fichier dans une fenetre = *) +(* = = *) +(* = filename = le fichier a afficher = *) +(* = title = titre de la fenetre = *) +(* = tooltips = systeme d'aide contextuelle = *) +(* ============================================================================= *) +let display_file filename title width height tooltips font = + let (win, vbox) = create_window title width height in + let text = create_text_edit false true true vbox#add in + + (* Tag pour mettre le texte en rouge *) + ignore (text#buffer#create_tag ~name:"red_foreground" [`FOREGROUND "red"]); + + (* Mise a jour de la fonte si necessaire *) + (match font with + None -> () + | Some fontname -> + let font = Pango.Font.from_string fontname in + ignore(text#misc#modify_font font)) ; + + let bbox = create_bbox vbox#pack in + let but_ok = create_button "OK" bbox#add in + add_tooltips tooltips but_ok "Fermer la fenetre" ; + but_connect but_ok (fun () -> win#destroy ()) ; + + (try + let c = open_compress filename in + (try + while true do text#buffer#insert ((input_line c)^"\n") done ; + text#buffer#insert ~tag_names:["red_foreground"] + (Printf.sprintf "Erreur de lecture de %s\n" filename) + with End_of_file -> close_compress filename c) + with _ -> + text#buffer#insert ~tag_names:["red_foreground"] + (Printf.sprintf "Erreur d'ouverture de %s\n" filename)) ; + + win#show () + +(* ============================================================================= *) +(* = Creation d'une barre de progression dans une fenetre externe = *) +(* = = *) +(* = nb_blocks = nombre de subdivisions dans la barre = *) +(* = title = titre de la fenetre = *) +(* = = *) +(* = En sortie est renvoyee la fonction de mise a jour de la barre. Cette = *) +(* = fonction prend en parametre un flottant entre 0.0 et 1.0. = *) +(* = Lorsque ce flottant vaut 1.0, la fenetre est detruite = *) +(* ============================================================================= *) +let create_progress_bar_win nb_blocks title = + let window = GWindow.window ~title:title ~border_width:10 ~width:200 () in + let pbar = GRange.progress_bar ~packing:window#add () in +(* GTK2 AAA GRange.progress_bar ~bar_style:`DISCRETE ~discrete_blocks:nb_blocks () + ~packing:window#add in*) + let update_func pct = + pbar#set_fraction pct ; + (* Destruction de la fenetre si pct = 1.0 *) + if pct = 1.0 then window#destroy () ; + + (* Force la mise a jour pour voir la barre progresser *) + force_update_interface () in + window#show (); + + (* Renvoie la fonction de mise a jour *) + update_func + +(* ============================================================================= *) +(* = Creation d'une barre de progression = *) +(* = = *) +(* = pack_method = ou mettre la barre = *) +(* = = *) +(* = Renvoie la fonction de mise a jour (valeur entre 0.0 et 1.0) = *) +(* ============================================================================= *) +let create_progress_bar pack_method = + let pbar = GRange.progress_bar ~packing:pack_method () in + let current_progress = ref "" in + let update_progress p = + let pp = Printf.sprintf "%.0f%%" (100.*.p) in + (* Comme ca on n'affiche que tous les 1%. Sinon c'est tres lent avec GTK2 *) + if !current_progress<>pp then begin + pbar#set_text pp ; + pbar#set_fraction p ; force_update_interface () ; + current_progress:=pp + end + in + update_progress + +(* Operateurs de comparaison *) +type t_ops_compare = T_EQ | T_L | T_LEQ | T_G | T_GEQ + +(* ============================================================================= *) +(* = Creation d'un widget de selection d'un operateur de comparaison = *) +(* = = *) +(* = variable = variable contenant l'operateur selectionne (ref) = *) +(* = callback_modified = callback appele lorsque la valeur est modifiee (opt) = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_ops_compare variable callback_modified pack_method = + let string_of_t_op t = + match t with + T_EQ -> "=" + | T_L -> "<" + | T_LEQ -> "<=" + | T_G -> ">" + | T_GEQ -> ">=" + in + + let lst_ops = + List.map (fun t -> (string_of_t_op t, t)) [T_EQ; T_L; T_LEQ; T_G; T_GEQ] in + let func_active typ = typ = !variable in + let func_select typ = + variable := typ ; + match callback_modified with + None -> () + | Some f -> f !variable + in + ignore (create_optionmenu lst_ops func_active func_select pack_method) + +(* ============================================================================= *) +(* = Creation d'un widget de selection d'une heure = *) +(* = = *) +(* = variable = variable contenant l'heure selectionnee (ref) = *) +(* = callback_modified = callback appele lorsque la valeur est modifiee (opt) = *) +(* = pack_method = ou mettre le widget = *) +(* ============================================================================= *) +let create_time_select variable callback_modified pack_method = + let split c s = + let i = ref (String.length s - 1) in + let j = ref !i and r = ref [] in + if !i >= 0 then + while !i >= 0 do + while !i >= 0 & String.get s !i <> c do decr i; done; + if !i < !j then r := (String.sub s (!i+1) (!j - !i)) :: !r; + while !i >= 0 & String.get s !i = c do decr i; done; + j := !i; + done; + !r + in + let string_of_time s = + Printf.sprintf "%02d:%02d:%02d" (s/3600) (s/60 mod 60) (s mod 60) in + let time_of_string t = + match split ':' t with + [h;m;s]->(int_of_string h*60 + int_of_string m)*60 + int_of_string s + |_-> (-1) + in + + let b = create_hbox pack_method in + let bmoins = create_button "-" b#pack + and entry = GEdit.entry ~text:(string_of_time !variable) + ~width:65 ~packing:b#pack () + and bplus = create_button "+" b#pack in + + let set_time () = + entry#set_text (string_of_time !variable) ; + match callback_modified with None -> () | Some f -> f !variable + in + + but_connect bmoins (fun () -> + if !variable>=1 then decr variable; set_time ()) ; + but_connect bplus (fun () -> + if !variable<=86400-1 then incr variable; set_time ()) ; + + ignore(entry#connect#changed ~callback:(fun () -> + variable:=time_of_string entry#text ; + match callback_modified with None -> () | Some f -> f !variable)) ; + + match callback_modified with + None -> () + | Some f -> + ignore(entry#connect#activate ~callback:(fun () -> f !variable)) + +(* ============================================================================= *) +(* = Fenetre pour affichage d'infos sous forme de labels = *) +(* = = *) +(* = title = titre de la fenetre = *) +(* = width = largeur de la fenetre = *) +(* = height = hauteur de la fenetre = *) +(* ============================================================================= *) +let create_infos_win title width height = + let (win, vb) = create_modal_window title width height in + let update_win text = + ignore(create_sized_label text width vb#pack) ; + force_update_interface () + in + win#show () ; + force_update_interface () ; + (update_win, win#destroy) + +(* ============================================================================= *) +(* = Separateur dans un menu = *) +(* ============================================================================= *) +let menu_separator = ("", fun () -> ()) + +(* ============================================================================= *) +(* = Creation d'un menu simple dans la barre de menus d'une fenetre = *) +(* = = *) +(* = menu = le menu ou acrocher le menu cree = *) +(* = lst_items = liste des noms+actions du menu = *) +(* ============================================================================= *) +let create_simple_menu menu lst_items = + let factory = new GMenu.factory menu in + List.iter (fun (texte, action) -> + ignore(if texte<>"" then factory#add_item texte ~callback:action + else factory#add_separator ())) lst_items + +(* ============================================================================= *) +(* = Creation de menus dans la barre de menus d'une fenetre = *) +(* = = *) +(* = menus = liste des menus standards = *) +(* = nb_menus = variable (reference) indiquant le menu courant = *) +(* = lst_items = liste des noms+actions du menu = *) +(* ============================================================================= *) +let create_menu menus nb_menus lst_items = + create_simple_menu menus.(!nb_menus) lst_items ; incr nb_menus + +(* ============================================================================= *) +(* = Creation d'un menu simple dans la barre de menus d'une fenetre = *) +(* = = *) +(* = menu = le menu ou acrocher le menu cree = *) +(* = lst_items = liste des noms+actions du menu = *) +(* ============================================================================= *) +let create_simple_menu_sens menu lst_items = + let factory = new GMenu.factory menu in + List.fold_left (fun l data -> + match data with + `I (texte, action) -> + let m = factory#add_item texte ~callback:action in (texte, m)::l + | `S -> ignore (factory#add_separator ()); l + | `M (texte, entries) -> + let sub = GMenu.menu () in let f = new GMenu.factory sub in + List.iter (fun m -> + match m with + `I (texte, action) -> ignore(f#add_item texte ~callback:action) + | _ -> ()) entries ; + let m = factory#add_item ~submenu:sub texte in (texte, m)::l + | _ -> l) [] lst_items + +(* ============================================================================= *) +(* = Creation de menus dans la barre de menus d'une fenetre = *) +(* = = *) +(* = menus = liste des menus standards = *) +(* = store_menus = table de stockage des menus pour recherche ensuite = *) +(* = tab_menus_names = noms associes aux menus = *) +(* = nb_menus = variable (reference) indiquant le menu courant = *) +(* = lst_items = liste des noms+actions du menu = *) +(* ============================================================================= *) +let create_menu_sens menus tab_menus_names store_menus nb_menus lst_items = + let l = create_simple_menu_sens menus.(!nb_menus) lst_items in + Hashtbl.add store_menus tab_menus_names.(!nb_menus) l ; + incr nb_menus + +(* ============================================================================= *) +(* = Initialisation du stockage des menus = *) +(* ============================================================================= *) +let init_menus_sens () = Hashtbl.create 13 + +(* ============================================================================= *) +(* = Mise a jour de l'etat active/desactive d'un sous-menu = *) +(* = = *) +(* = store_menus = table de stockage des menus = *) +(* = menu_name = nom du menu = *) +(* = sub_menu_name = nom du sous-menu = *) +(* = sensitive = indique l'etat a donne au sous-menu = *) +(* ============================================================================= *) +let set_sub_menu_sensitive store_menus menu_name sub_menu_name sensitive = + try + let l = Hashtbl.find store_menus menu_name in + List.iter (fun (texte, sub_menu) -> + if texte = sub_menu_name then set_sensitive sub_menu sensitive) l + with Not_found -> () + +(* ============================================================================= *) +(* = Combo box = *) +(* ============================================================================= *) +let create_combo_simple lst_items pack_method = + let combo = GEdit.combo ~packing:pack_method () in + List.iter (fun item -> + let i = GList.list_item () in + ignore(create_label item i#add) ; + combo#set_item_string i item ; + combo#list#add i) lst_items ; + (* Zone ou se trouve le texte selectionne *) + let entry = combo#entry in + (* On la desactive pour que l'utilisateur ne puisse pas la modifier a la main *) + set_sensitive entry false ; + + entry + +let combo_connect entry lst_items callback = + ignore(text_entry_connect_modify entry + (fun s -> + try let (_, t) = List.find (fun (n, _) -> n=s) lst_items in callback t + with Not_found -> ())) + +(* ============================================================================= *) +(* = Widget calendrier pour la selection d'une date. Les dates disponibles sont= *) +(* = en couleur = *) +(* ============================================================================= *) +let calendar lst_dates callback_select only_available_dates_selectable + init_with_last_available_date tooltips win pack_method = + + (* Date de depart = la plus recente dans la liste ou date actuelle *) + let current_month = ref 0 and current_year = ref 0 in + if init_with_last_available_date && lst_dates<>[] then begin + let d = List.hd (List.fast_sort (fun d1 d2 -> cmp_int d2 d1) lst_dates) in + let (j, m, a) = decompose_date d in current_month := m; current_year := a + end else begin + let tm = timer_get_time () in + current_month := (tm.Unix.tm_mon+1); current_year:= (tm.Unix.tm_year+1900) + end ; + + (* Transformation des dates 20030721 -> (21, 07, 2003) *) + let lst_dates = List.map decompose_date lst_dates in + (* Fonction indiquant si une date est dans la listes des dates autorisees *) + let date_in_list i = + try ignore(List.find (fun date -> + date = (i+1, !current_month, !current_year)) lst_dates) ; true + with Not_found -> false + in + + let styles = + let default = (Obj.magic () : GObj.style) in [|default; default; default|] + in + + (* Boite pour mettre le widget *) + let vb = create_vbox pack_method in + + (* Boutons de changement de mois et label d'affichage mois+annee courants *) + let hb = create_hbox vb#pack in + let pm = pixmap_from_file "Pixmaps/left_arrow.xpm" win in + let b_mmois = create_pixbutton pm hb#pack in + let lab_mois = create_label "" hb#add in + let pm = pixmap_from_file "Pixmaps/right_arrow.xpm" win in + let b_pmois = create_pixbutton pm hb#pack in + + (* Table contenant les boutons pour les jours *) + let calendar = + GPack.table ~homogeneous: true ~rows:7 ~columns:7 + ~border_width:10 ~row_spacings:2 ~col_spacings:2 ~packing:vb#pack () in + (* Barre de titre avec les jours de la semaine *) + Array.iteri (fun i wday -> + ignore(create_button wday + (calendar#attach ~top:0 ~left:i ~expand:`BOTH))) + [|"Dim"; "Lun"; "Mar"; "Mer"; "Jeu"; "Ven"; "Sam"|] ; + + (* Creation des boutons correspondants aux jours d'un mois *) + let buttons = Array.init 31 (fun i -> + let b = GButton.button ~label:(string_of_int (i+1)) ~show:false () in + but_connect b (fun () -> + if not only_available_dates_selectable or (date_in_list i) then begin + b#misc#set_style styles.(2) ; + callback_select (compose_date (i+1, !current_month, !current_year)) + end) ; + b) in + let buttons_shown = Array.init 31 (fun i -> false) in + + (* Mise a jour des boutons dans le calendrier *) + let update_calendar () = + let mois = get_month_of_num !current_month in + lab_mois#set_text (Printf.sprintf "%s %d" mois !current_year) ; + + (* Numero, dans la semaine, du premier jour du mois indique *) + let d = + match get_day_of_date (1, !current_month, !current_year) with + "Dimanche" -> 0 | "Lundi" -> 1 | "Mardi" -> 2 + | "Mercredi" -> 3 | "Jeudi" -> 4 | "Vendredi" -> 5 + | _ -> 6 + in + + (* Suppression des boutons precedemment affiches *) + Array.iteri (fun i button -> + if buttons_shown.(i) then begin + button#misc#hide (); + calendar#remove button#coerce ; + buttons_shown.(i) <- false + end) buttons ; + + (* Affichage du bon nombre de boutons *) + let ndays = get_nb_days_in_month !current_month !current_year in + for i = 0 to ndays - 1 do + let top = (i+d) / 7 + 1 and left = (i+d) mod 7 in + calendar#attach ~left ~top ~expand:`BOTH buttons.(i)#coerce ; + buttons.(i)#misc#show () ; + buttons_shown.(i) <- true ; + add_tooltips tooltips buttons.(i) + (Printf.sprintf "%s %d %s %d" + (get_day_of_date (i+1, !current_month, !current_year)) + (i+1) mois !current_year) ; + + if date_in_list i then buttons.(i)#misc#set_style styles.(1) + else buttons.(i)#misc#set_style styles.(0) + done + in + + (* Boutons de modification du mois courant *) + but_connect b_mmois (fun () -> + if !current_month = 1 then begin decr current_year; current_month:=12 + end else decr current_month ; + update_calendar ()) ; + but_connect b_pmois (fun () -> + if !current_month = 12 then begin incr current_year; current_month:=1 + end else incr current_month ; + update_calendar ()) ; + + (* Valeurs des styles pour le changement de couleurs des boutons *) + let style = win#misc#style#copy in styles.(0) <- style; + let style = style#copy in + style#set_bg [`NORMAL, `NAME "light green"; + `PRELIGHT, `NAME "light green"]; + styles.(1) <- style; + let style = style#copy in + style#set_bg [`ACTIVE, `NAME "blue"]; + + styles.(2) <- style; + + (* Mise a jour initiale des boutons dans le calendrier *) + update_calendar () + +(* ============================================================================= *) +(* = Fenetre calendrier pour la selection d'une date. Les dates disponibles = *) +(* = sont en couleur = *) +(* ============================================================================= *) +let calendar_window lst_dates callback_select + only_available_dates_selectable init_with_last_available_date is_modal tooltips = + + (* Creation de la fenetre modale ou pas *) + let (win, vb) = + if is_modal then create_modal_window "Choix de date" 0 0 + else create_window "Choix de date" 0 0 + in + + let new_callback_select date = callback_select date; win#destroy () in + calendar lst_dates new_callback_select only_available_dates_selectable + init_with_last_available_date tooltips win vb#add ; + + (* Bouton de fermeture de la fenetre *) + let bbox = create_bbox vb#pack in + let but_cancel = create_button "Annuler" bbox#add in + but_connect but_cancel win#destroy ; + + win#show () + +(* ============================================================================= *) +(* = Creation d'une pixmap pour faire ensuite un stipple = *) +(* ============================================================================= *) +let create_stipple_pixmap_from_data data width height = + try + let s = String.create (width*height) and i = ref 0 in + List.iter (fun v -> s.[!i] <- Char.chr v; incr i) data ; + + let c1 = GDraw.color `WHITE and c2 = GDraw.color `BLACK in + (new GDraw.pixmap (Gdk.Pixmap.create_from_data ~width:width ~height:height + ~depth:1 ~fg:c1 ~bg:c2 s))#pixmap + with _ -> Printf.printf "Erreur dans create_stipple_pixmap_from_data\n"; + flush stdout; exit 1 + +(* ============================================================================= *) +(* = Fenetre de selection d'une fonte = *) +(* ============================================================================= *) +let select_font_dlg tooltips fonte_init selection_func = + let (win, vb)= create_modal_window "Selection de fonte" 0 0 in + let fn_dlg = GMisc.font_selection ~packing:vb#add ~show:true () in + let lst_buts = [("OK", "Selection de la fonte"); + ("Annuler", "Ferme la fenetre")] + in + + if fonte_init <> "" then fn_dlg#set_font_name fonte_init ; + + let get_font () = + if fn_dlg#font_name<>"" then selection_func fn_dlg#font_name ; + win#destroy () + in + + let buts = create_buttons lst_buts tooltips (vb#pack ~from:`END) in + create_buttons_connect buts + [get_font; win#destroy] ; + + win#show () ; + fn_dlg + +(* ============================================================================= *) +(* = Selection du texte contenu dans un widget = *) +(* ============================================================================= *) +let text_entry_select_text entry = + entry#select_region ~start:0 ~stop:entry#text_length + +(* ============================================================================= *) +(* = Creation d'une pixmap rectangulaire coloree = *) +(* ============================================================================= *) +let rectangle_pixmap window color width height = + (* ================================================================== *) + (* La ligne suivante marche jusqu'a lablgtk2-20040304, a partir des *) + (* versions suivantes on obtient un bug a l'execution si on n'utilise *) + (* pas GDraw.pixmap_from_xpm_d a la place de GDraw.pixmap *) + (* ================================================================== *) +(* let pm = GDraw.pixmap ~window ~width ~height () in*) + let size = Printf.sprintf "%d %d 2 1" width height in + let data = [size ; ". c None"; "# c #000000"] in + let one_line = ref "" in + for i = 0 to width-1 do one_line:=!one_line^"#" done ; + let lines = ref [] in + for i = 0 to height-1 do lines:=!one_line::!lines done ; + + let data = Array.of_list (data @ !lines) in + let pm = GDraw.pixmap_from_xpm_d ~data:data + ~window:(window:GWindow.window) () in + (pm:GDraw.pixmap)#set_foreground color ; + pm#rectangle ~filled:true ~x:0 ~y:0 ~width ~height () ; + pm + +(* ============================================================================= *) +(* = Creation d'une fenetre on top = *) +(* ============================================================================= *) +let create_window_on_top title width height window = + let x = create_window title width height in + (fst x)#set_transient_for window#as_window; x + +let create_window_on_top2 title width height window = + let x = create_window title width height in + (match window with + None -> () | Some window -> (fst x)#set_transient_for window#as_window) ; + x + +(* ============================================================================= *) +(* = Creation d'une fenetre modale on top = *) +(* ============================================================================= *) +let create_modal_window_on_top title width height window = + let x = create_modal_window title width height in + (fst x)#set_transient_for window#as_window; x + +let create_modal_window_on_top2 title width height window = + let x = create_modal_window title width height in + (match window with + None -> () | Some window -> (fst x)#set_transient_for window#as_window) ; + x + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/gtk_tools.mli b/sw/lib/ocaml/gtk_tools.mli new file mode 100644 index 0000000000..2eb3f888ad --- /dev/null +++ b/sw/lib/ocaml/gtk_tools.mli @@ -0,0 +1,1354 @@ +(* + * $Id$ + * + * Lablgtk2 utils + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) +(** Module outils pour lablgtk-2.4.0 + + Nouveaux widgets et encapsulation de fonctions GTK pour faciliter + l'utilisation de la bibliotheque lablgtk + + {b Dépendences : Platform, Ocaml_Tools} + + {e Yann Le Fablec, version 4.10, 26/08/2004} + *) + +(** Chaine indiquant la version de la librairie *) +val version : string + + +(** {6 Fonctions principales} *) + + +(** force la mise à jour de l'interface *) +val force_update_interface : unit -> unit + +(** initialise les couleurs *) +val init_colors : unit -> unit + +(** lance la mainloop de l'interface *) +val main_loop : unit -> unit + +(** initialise le systeme d'aide contextuelle et renvoie l'objet correspondant *) +val init_tooltips : unit -> GData.tooltips + +(** [gtk_tools_add_tooltips tooltips widget texte] ajoute une aide contextuelle + à un widget *) +val add_tooltips : + GData.tooltips -> < coerce : GObj.widget; .. > -> string -> unit + +(** renvoie la largeur et la hauteur de l'écran en pixels *) +val get_screen_size : unit -> int * int + +(** [gtk_tools_disconnect wid id] déconnecte le signal [id] du widget [wid] *) +val disconnect : + < misc : < disconnect : 'a -> 'b; .. >; .. > -> 'a -> 'b + +(** [gtk_tools_set_sensitive widget sensitive] active/désactive un widget *) +val set_sensitive : + < misc : < set_sensitive : 'a -> 'b; .. >; .. > -> 'a -> unit + +(** [gtk_tools_set_sensitive_list widgets sensitive] active/désactive une liste + de widgets *) +val set_sensitive_list : + < misc : < set_sensitive : 'a -> 'b; .. >; .. > list -> 'a -> unit + +(** [gtk_tools_set_cursor window cursor] met à jour la forme du curseur dans la + fenetre indiquée *) +val set_cursor : Gdk.window -> Gdk.cursor -> unit + +(** [gtk_tools_get_widget_size widget] renvoie un couple donnant la largeur et + la hauteur du widget indiqué *) +val get_widget_size : + < misc : < allocation : Gtk.rectangle; .. >; .. > -> int * int + +(** [gtk_tools_set_widget_back_color widget couleur] modifie la couleur de fond + d'un widget. Marche avec les widgets créant une fenetre (les boutons + par exemple), ça ne fonctionne donc pas avec les labels pour lesquels il faut + une event_box...*) +val set_widget_back_color : + < misc : < set_style : (< set_bg : ([> `NORMAL] * GDraw.color) list -> + 'c; + .. > as 'a) -> + 'd; + style : < copy : 'a; .. >; .. >; + .. > -> GDraw.color -> unit + +(** [gtk_tools_set_widget_front_color w color] modifie la couleur du texte d'un widget + (du type label) *) +val set_widget_front_color : + < misc : < set_style : (< set_fg : ([> `NORMAL] * GDraw.color) list -> + 'b; .. > as 'a) -> 'c; style : < copy : 'a; .. >; .. >; .. > -> + GDraw.color -> unit + +(** [gtk_tools_set_button_front_color bouton color] modifie la couleur du texte d'un + bouton (la fonction précédente ne marche pas avec un bouton car le texte d'un bouton + ne se trouve pas directement dans le bouton mais dans un label fils de ce bouton *) +val set_button_front_color : GButton.button -> GDraw.color -> unit + +(** [gtk_tools_set_button_back_color bouton color] modifie la couleur de fond d'un bouton. *) +val set_button_back_color : GButton.button -> GDraw.color -> unit + +(** [gtk_tools_set_entry_front_color entry color] modifie la couleur du texte dans une entry *) +val set_entry_front_color : GEdit.entry -> GDraw.color -> unit + +(** [gtk_tools_set_entry_back_color entry color] modifie la couleur de fond d'une entry *) +val set_entry_back_color : GEdit.entry -> GDraw.color -> unit + +(** [gtk_tools_set_entry_outline_color entry color] modifie la couleur du contour d'une entry *) +val set_entry_outline_color : GEdit.entry -> GDraw.color -> unit + + +(** [gtk_tools_scroll_adjustment adj scroll_up delta] effectue le scrolling d'un + adjustment [adj] (d'une scrollbar par exemple) vers le haut si [scroll_up] est vrai + et vers le bas sinon. [delta] désigne la valeur absolue du deplacement *) +val scroll_adjustment : GData.adjustment -> bool -> float -> unit + +(** [gtk_tools_connect_mouse_wheel_scroll widget scrollbar delta] connecte + un scroll de valeur absolue [delta] de la scrollbar [scrollbar] + lors de l'utilisation de la molette souris dans le widget [widget] *) +val connect_mouse_wheel_scroll : + < event : < connect : < button_press : callback:(GdkEvent.Button.t -> + bool) -> 'a; scroll : callback:(GdkEvent.Scroll.t -> bool) -> 'a; .. >; .. >; .. > + -> GRange.range -> float -> unit + +(** {6 Evénements souris} *) + + +(** Boutons souris *) +type bouton_souris = B_GAUCHE | B_DROIT | B_MILIEU | B_NONE + +(** [gtk_tools_test_mouse_but event] renvoie un element de type + {!Gtk_tools.gtk_tools_bouton_souris} indiquant le bouton de souris pressé lors + d'un click souris *) +val test_mouse_but : GdkEvent.Button.t -> bouton_souris + +(** [gtk_tools_get_mouse_pos_click event] renvoie un couple d'entiers donnant + la position de la souris lors d'un click *) +val get_mouse_pos_click : GdkEvent.Button.t -> int * int + +(** [gtk_tools_get_mouse_pos_move event] renvoie un element de type + {!Gtk_tools.gtk_tools_bouton_souris} indiquant le bouton de souris pressé + pendant un deplacement de la souris *) +val get_mouse_pos_move : GdkEvent.Motion.t -> int * int + +(** [gtk_tools_check_dbl_click event] teste le double click souris *) +val check_dbl_click : [> `TWO_BUTTON_PRESS] Gdk.event -> bool + + +(** {6 Fontes/Texte} *) + + +(** renvoie la fonte 'fixed' 8 points *) +val get_fixed_font : unit -> Gdk.font + +(** renvoie la fonte 'fixed' 13 points *) +val get_fixed_font2 : unit -> Gdk.font + +(** [gtk_tools_set_widget_font widget font] modifie la fonte d'un widget *) +val set_widget_font : + < misc : < set_style : (< set_font : 'b -> 'c; .. > as 'a) -> 'd; + style : < copy : 'a; .. >; .. >; + .. > -> + 'b -> unit + +(** [gtk_tools_string_width_height font string] indique la largeur et la hauteur, + en pixels, du texte donné par [string] affiché dans la fonte [font] *) +val string_width_height : Gdk.font -> string -> int * int + + +(** {6 Boites} *) + + +(** {0 Boites de widgets} *) + +(** [gtk_tools_create_bbox pack_method] crée une boite de boutons *) +val create_bbox : (GObj.widget -> unit) -> GPack.button_box + +(** [gtk_tools_create_hbox pack_method] création d'une boite horizontale *) +val create_hbox : (GObj.widget -> unit) -> GPack.box + +(** [gtk_tools_create_hom_hbox pack_method] création d'une boite horizontale où + la taille des widgets est homogène *) +val create_hom_hbox : (GObj.widget -> unit) -> GPack.box + +(** [gtk_tools_create_spaced_hbox pack_method] création d'une boite horizontale + avec espacement des widgets de 5 pixels *) +val create_spaced_hbox : (GObj.widget -> unit) -> GPack.box + +(** [gtk_tools_create_hom_spaced_hbox pack_method] création d'une boite horizontale + avec espacement des widgets de 5 pixels. Les widgets ont en plus une taille + homogène *) +val create_hom_spaced_hbox : (GObj.widget -> unit) -> GPack.box + +(** [gtk_tools_create_vbox pack_method] création d'une boite verticale *) +val create_vbox : (GObj.widget -> unit) -> GPack.box + +(** [gtk_tools_create_hom_vbox pack_method] création d'une boite verticale où + la taille des widgets est homogène *) +val create_hom_vbox : (GObj.widget -> unit) -> GPack.box + +(** [gtk_tools_create_spaced_vbox pack_method] création d'une boite verticale + avec espacement des widgets de 5 pixels *) +val create_spaced_vbox : (GObj.widget -> unit) -> GPack.box + +(** [gtk_tools_create_hom_spaced_vbox pack_method] création d'une boite verticale + avec espacement des widgets de 5 pixels. Les widgets ont en plus une taille + homogène *) +val create_hom_spaced_vbox : (GObj.widget -> unit) -> GPack.box + + +(** {0 Frames} *) + + +(** [gtk_tools_create_vframe title pack_method] crée une frame contenant une vbox*) +val create_vframe : + string -> (GObj.widget -> unit) -> GBin.frame * GPack.box + +(** [gtk_tools_create_spaced_vframe title pack_method] crée une frame contenant une vbox + avec espaces *) +val create_spaced_vframe : + string -> (GObj.widget -> unit) -> GBin.frame * GPack.box + +(** [gtk_tools_create_hframe title pack_method] crée une frame contenant une hbox*) +val create_hframe : + string -> (GObj.widget -> unit) -> GBin.frame * GPack.box + +(** [gtk_tools_create_spaced_hframe title pack_method] crée une frame contenant une hbox + avec espaces *) +val create_spaced_hframe : + string -> (GObj.widget -> unit) -> GBin.frame * GPack.box + + +(** {0 Boites scrollables} *) + + +(** [gtk_tools_create_scrolled_box pack_method] crée une zone scrollable contenant + une vbox. Renvoie la zone et la vbox *) +val create_scrolled_box : + (GObj.widget -> unit) -> GBin.scrolled_window * GPack.box + +(** [gtk_tools_change_scrolled_box scrolled_window old_box] détruit puis recrée + la vbox dans une zone scrollable. Renvoie la nouvelle vbox *) +val change_scrolled_box : + < add_with_viewport : GObj.widget -> unit; .. > -> + < destroy : unit -> 'a; .. > -> GPack.box + + +(** {0 Notebooks} *) + + +(** [gtk_tools_create_notebook pack_method] crée un notebook (widget contenant + différentes pages *) +val create_notebook : (GObj.widget -> unit) -> GPack.notebook + +(** [gtk_tools_notebook_add_page notebook page_label] ajoute une page nommée + [page_label] au notebook indiqué. En retour un couple contenant une frame + et une boite verticale dans cette frame est retourné *) +val notebook_add_page : + GPack.notebook -> string -> GBin.frame * GPack.box + + +(** {0 Paned windows} *) + + +(** [gtk_tools_create_hpaned pack_method] création d'une zone avec division + mobile horizontale *) +val create_hpaned : (GObj.widget -> unit) -> GPack.paned + +(** [gtk_tools_create_vpaned pack_method] création d'une zone avec division + mobile verticale *) +val create_vpaned : (GObj.widget -> unit) -> GPack.paned + + +(** {6 Boutons} *) + + +(** [gtk_tools_create_button label pack_method] création d'un bouton *) +val create_button : + string -> (GObj.widget -> unit) -> GButton.button + +(** [gtk_tools_create_sized_button label width pack_method] création d'un bouton + ayant une taille fixée *) +val create_sized_button : + string -> int -> (GObj.widget -> unit) -> GButton.button + +(** [gtk_tools_but_connect but func] connecte le callback [func] au bouton [but] *) +val but_connect : + < connect : < clicked : callback:'a -> 'b; .. >; .. > -> 'a -> unit + +(** [gtk_tools_but_set_label but label] change le texte dans le bouton pour le + remplacer par [label] *) +val but_set_label : + < children : < as_widget : 'a Gtk.obj; .. > list; .. > -> string -> unit + +(** [gtk_tools_set_button_align but pos] change l'alignement du texte + d'un bouton *) +val set_button_align : + < children : < as_widget : 'a Gtk.obj; .. > list; .. > -> float -> unit + +(** [gtk_tools_set_button_align_left but] alignement du texte + du bouton à gauche*) +val set_button_align_left : + < children : < as_widget : 'a Gtk.obj; .. > list; .. > -> unit + +(** [gtk_tools_set_button_align_right but] idem à droite *) +val set_button_align_right : + < children : < as_widget : 'a Gtk.obj; .. > list; .. > -> unit + +(** [gtk_tools_set_button_align_center but] idem au milieu *) +val set_button_align_center : + < children : < as_widget : 'a Gtk.obj; .. > list; .. > -> unit + +(** [gtk_tools_set_button_padding but xpad] met a jour la position du texte dans le bouton *) +val set_button_padding : + < children : < as_widget : 'a Gtk.obj; .. > list; .. > -> int -> unit + +(** [gtk_tools_but_set_width but width] met a jour la largeur d'un bouton *) +val but_set_width : GButton.button -> int -> unit + +(** [gtk_tools_but_set_height but height] met a jour la hauteur d'un bouton *) +val but_set_height : GButton.button -> int -> unit + +(** [gtk_tools_create_buttons lst_buts tooltips pack_method] crée une rangée de + boutons placés dans une {!Gtk_tools.gtk_tools_create_bbox}. Le paramètre + [lst_buts] est une liste de couples du type [(nom du bouton, aide)] *) +val create_buttons : + (string * string) list -> + GData.tooltips -> (GObj.widget -> unit) -> GButton.button list + +(** [gtk_tools_create_buttons_connect lst_buts lst_callbacks] connecte une liste + de callbacks à une liste de boutons *) +val create_buttons_connect : + < connect : < clicked : callback:'a -> 'b; .. >; .. > list -> + 'a list -> unit + +(** [gtk_tools_create_checkbutton_simple active label pack_method tip tooltips] + crée un check button sans callback mais avec une aide contextuelle si [tip]<>""*) +val create_checkbutton_simple : + bool -> + string -> + (GObj.widget -> unit) -> string -> GData.tooltips -> GButton.toggle_button + +(** [gtk_tools_create_checkbutton active label pack_method tip tooltips callback] + crée un check button avec callback et une aide contextuelle (si [tip]<>"") *) +val create_checkbutton : + bool -> + string -> + (GObj.widget -> unit) -> + string -> GData.tooltips -> (bool -> unit) -> GButton.toggle_button + +(** [gtk_tools_create_radiobuttons_simple lst_names func_active pack_method] crée + une liste de radio boutons : + - [lst_names] est une liste contenant des couples [(nom_bouton, type_associe)] + - [func_active] indique quel est le bouton actif au depart + - [pack_method] indique où mettre les boutons + *) +val create_radiobuttons_simple : + (string * 'a) list -> + ('a -> bool) -> (GObj.widget -> unit) -> GButton.radio_button list + +(** [gtk_tools_radiobuttons_connect lst_names lst_but func_select] connecte + un callback lié à la modification du radiobutton selectionné. Le callback + reçoit en paramètre le type correspondant à ce bouton *) +val radiobuttons_connect : + ('a * 'b) list -> GButton.radio_button list -> ('b -> unit) -> unit + +(** [gtk_tools_create_radiobuttons lst_names func_active func_select pack_method] + crée des radiobuttons avec callback *) +val create_radiobuttons : + (string * 'a) list -> + ('a -> bool) -> + ('a -> unit) -> (GObj.widget -> unit) -> GButton.radio_button list + +(** [gtk_tools_create_togglebutton label active pack_method] crée un togglebutton*) +val create_togglebutton : + string -> bool -> (GObj.widget -> unit) -> GButton.toggle_button + +(** [gtk_tools_create_pixbutton pixmap pack_method] création d'un bouton contenant + une pixmap *) +val create_pixbutton : + GDraw.pixmap -> (GObj.widget -> unit) -> GButton.button + + +(** {6 Labels} *) + + +(** [gtk_tools_create_label label pack_method] création d'un label *) +val create_label : string -> (GObj.widget -> unit) -> GMisc.label + +(** [gtk_tools_create_sized_label label width pack_method] création d'un label + avec une taille fixée *) +val create_sized_label : + string -> int -> (GObj.widget -> unit) -> GMisc.label + +(** {0 Alignement des labels} *) + +(** [gtk_tools_create_sized_label_align label width pos pack_method] création d'un label + avec une taille fixée. Le texte est positionné suivant [pos] qui est compris entre + 0.0 et 1.0 (0.0 = à gauche et 1.0 = à droite) *) +val create_sized_label_align : + string -> int -> float -> (GObj.widget -> unit) -> GMisc.label + +(** [gtk_tools_create_sized_label_align_left label width pack_method] création d'un label + avec une taille fixée. Le texte est positionné à gauche dans le label *) +val create_sized_label_align_left : + string -> int -> (GObj.widget -> unit) -> GMisc.label + +(** [gtk_tools_create_sized_label_align_right label width pack_method] création d'un label + avec une taille fixée. Le texte est positionné à droite dans le label *) +val create_sized_label_align_right : + string -> int -> (GObj.widget -> unit) -> GMisc.label + +(** [gtk_tools_set_label_align label pos] fixe l'alignement du texte dans un label. + [pos] indique où se fait l'alignement : 0.0 = à gauche et 1.0 = à droite *) +val set_label_align : GMisc.label -> float -> unit + +(** [gtk_tools_set_label_align_left label] fixe l'alignement du texte dans un label + à gauche *) +val set_label_align_left : GMisc.label -> unit + +(** [gtk_tools_set_label_align_right label] fixe l'alignement du texte dans un label + à droite *) +val set_label_align_right : GMisc.label -> unit + +(** [gtk_tools_set_label_align_center label] fixe l'alignement du texte dans un label + au centre de ce dernier *) +val set_label_align_center : GMisc.label -> unit + +(** [gtk_tools_set_label_padding label xpad] fixe la position gauche du texte + dans un label *) +val set_label_padding : GMisc.label -> int -> unit + +(** {6 Boites de dialogue} *) + + +(** [gtk_tools_question_box window title question_msg default_is_cancel] crée une + fenetre posant une question à l'utilisateur : + - [window] désigne la fenetre mère + - [title] le titre à donner à la fenetre + - [question_msg] le message à afficher + - [defaut_is_cancel] indique si par defaut c'est le bouton Annuler qui est + selectionné + + Renvoie vrai si OK a été choisi, faux sinon + *) +val question_box : + < misc : #GDraw.misc_ops; .. > -> string -> string -> bool -> bool + +(** [gtk_tools_error_box window title error_msg] affiche une boite contenant un + message d'erreur *) +val error_box : + < misc : #GDraw.misc_ops; .. > -> string -> string -> unit + +(** [gtk_tools_animated_msg_box title msg lst_pixmaps] crée une boite de message + contenant un icone animé dont [lst_pixmaps] désigne les noms des différentes + images (pixmaps) de l'animation *) +val animated_msg_box : string -> string -> string list -> unit + +(** [gtk_tools_open_file_dlg title read_func update_func default_filename + check_overwrite] crée une boite de selection de fichier : + - [title] indique le titre à donner à la fenetre + - [read_func] fonction appelée pour traiter le fichier selectionné + - [update_func] : [None] ou [Some f] fonction optionnelle appelée après + le traitement du fichier par la fonction précédente + - [default_filename] nom de fichier/répertoire par defaut + - [check_overwrite] indique si on doit tester l'existence du fichier (pour + eviter un écrasement + *) +val open_file_dlg : + string -> (string -> 'a) -> (unit -> unit) option -> string -> bool -> unit + +(** [gtk_tools_select_font_dlg tooltips fonte_init selection_func] ouvre une fenetre + de selection de fonte. Si une fonte est choisie, [selection_func] est + appelée avec son nom. [fonte_init] désigne le nom de la fonte sélectionnée par + défaut ("" pour rien). *) +val select_font_dlg : GData.tooltips -> string -> (string -> unit) + -> GMisc.font_selection + +(** {6 Timers} *) + + +(** Type de timer *) +type timer_type = + TIMER_TIME (** Heure uniquement *) + | TIMER_DATE (** Date uniquement *) + | TIMER_TIME_AND_DATE (** Affichage de l'heure et de la date *) + +(** [gtk_tools_insert_timer label timer_type force_beginning] place un timer + de type [timer_type] dans le widget [label]. Si [force_beginning] est vrai + alors l'affichage commence dès la creation du timer *) +val insert_timer : + GMisc.label -> timer_type -> bool -> unit + + +(** {6 Menus} *) + +(** [gtk_tools_popup menus_entries] affiche un popup menu *) +val popup : GToolbox.menu_entry list -> unit + +(** [gtk_tools_connect_popup_menu wid button test_cond_func menu_entries] connecte + un popup menu à un widget : + - [wid] désigne le widget concerné + - [button] le bouton à presser pour afficher le popup menu + - [test_cond_func] fonction de test indiquant si le menu doit etre affiché + - [menu_entries] contient la liste des éléments du menu +*) +val connect_popup_menu : + < event : < connect : < button_press : callback:(GdkEvent.Button.t -> bool) -> + 'a; .. >; .. >; .. > -> + bouton_souris -> + (unit -> bool) -> GToolbox.menu_entry list -> unit + +(** [gtk_tools_connect_func_popup_menu wid button test_cond_func get_menu_entries] + connecte un popup menu construit de manière dynamique à un widget : + - [wid] désigne le widget concerné + - [button] le bouton à presser pour afficher le popup menu + - [test_cond_func] fonction de test indiquant si le menu doit etre affiché + - [get_menu_entries] désigne la fonction appelée au moment de l'appui sur + le bouton et qui renvoie la liste des éléments du menu *) +val connect_func_popup_menu : + < event : < connect : < button_press : callback:(GdkEvent.Button.t -> bool) -> + 'a; .. >; .. >; .. > -> + bouton_souris -> + (unit -> bool) -> (unit -> GToolbox.menu_entry list) -> unit + +(** [gtk_tools_create_popup_menu title event data] crée un popup menu la où se + trouve la souris. [data] est une liste d'éléments indiquant le contenu du menu + sous la forme [(texte, Some fonction, parametre fonction, sous menu)]. + [titre] désigne le titre du menu, peut etre egal à "" pour ne pas mettre + de titre *) +val create_popup_menu : + string -> + GdkEvent.Button.t -> + (string * ('a -> unit) option * 'a * + (string * ('b -> unit) option * 'b) list) + list -> unit + +(** [gtk_tools_create_optionmenu lst_names func_active func_select pack_method] + creation d'un menu à options : + - [lst_names] contient la liste des options avec leur type sous la forme + [(nom, type)] + - [func_active] indique quelle est l'option active par defaut + - [func_select] appelée lors de la modification de l'option + - [pack_method] désigne l'endroit où mettre le menu + + Renvoie le menu ainsi qu'une fonction permettant de mettre à jour l'option + courante et une seconde fonction mettant à jour l'option courante et qui + appelle la fonction [func_select] en plus : + [(option_menu, set_option, set_option_and_activate)] + *) +val create_optionmenu : + (string * 'a) list -> + ('a -> bool) -> + ('a -> unit) -> (GObj.widget -> unit) -> + GMenu.option_menu * ('a -> unit) * ('a -> unit) + +(** [separateur dans un menu] *) +val menu_separator : string * (unit -> unit) + +(** [gtk_tools_create_simple_menu menu lst_items] crée un menu attaché à [menu] + et défini par [lst_items]. [lst_items] est une liste d'éléments du type + [(texte_menu, action)] définissant les éléments du menu à créer *) +val create_simple_menu : + #GMenu.menu_shell -> (string * (unit -> unit)) list -> unit + +(** [gtk_tools_create_menu menus nb_menus lst_items] : identique à la fonction + précédente sauf que [menus] désigne un tableau de menus et [nb_menus] la référence + sur une variable indiquant le menu courant dans ce tableau *) +val create_menu : + #GMenu.menu_shell array -> + int ref -> (string * (unit -> unit)) list -> unit + +(** Initialise la variable de stockage des menus (permet de les activer ou de les + desactiver) *) +val init_menus_sens : unit -> (string, (string*GMenu.menu_item) list) Hashtbl.t + +(** [gtk_tools_create_simple_menu_sens menu lst_items] crée un menu dont les sous-menus + peuvent etre activés ou désactivés *) +val create_simple_menu_sens : #GMenu.menu_shell -> GToolbox.menu_entry list -> + (string * GMenu.menu_item) list + +(** [gtk_tools_create_menu_sens menus tab_menus_names store_menus nb_menus lst_items] créé un menu + dont les sous-menus peuvent etre rendus actifs ou inactifs. [menus] désigne le tableau des menus, + [tab_menus_names] est un tableau contenant les noms des ces menus, [store_menus] contient la + variable de stockage créée avec {!Gtk_tools.gtk_tools_init_menus_sens}, nb_menus est une référence + sur la variable contenant le menu en cours de création. *) +val create_menu_sens : #GMenu.menu_shell array -> + string array -> (string, (string*GMenu.menu_item) list) Hashtbl.t -> + int ref -> GToolbox.menu_entry list -> unit + +(** [gtk_tools_set_sub_menu_sensitive store_menus menu_name sub_menu_name sensitive] active ou + désactive (suivant la valeur de [sensitive]) le sous-menu de nom [sub_menu_name] dans le + menu de nom [menu_name] *) +val set_sub_menu_sensitive : + (string, (string*GMenu.menu_item) list) Hashtbl.t -> string -> string -> bool -> unit + + +(** {6 Pixmaps} *) + + +(** pixmap d'ouverture d'un fichier *) +val open_file_pixmap : string array + +(** [gtk_tools_pixmap_from_file filename window] création d'un [GDraw.pixmap] à + partir d'un fichier xpm *) +val pixmap_from_file : string -> GWindow.window -> GDraw.pixmap + +(** [gtk_tools_create_pixmap window width height] crée une pixmap de taille + [width]*[height] où [window] désigne la fenetre mère de la pixmap. + En retour, un couple [(pix, pixmap)] où [pix] sert à mettre la pixmap dans + une zone de dessin (par ex.) et [pixmap] sert au dessin *) +val create_pixmap : + GWindow.window -> int -> int -> Gdk.pixmap * GDraw.pixmap + +(** [gtk_tools_create_d_pixmap window width height] crée un drawable de taille + [width]*[height] où [window] désigne la fenetre mère du drawable. + En retour, un couple [(pix, pixmap)] où [pix] sert à mettre le drawable dans + une zone de dessin (par ex.) et [pixmap] sert au dessin *) +val create_d_pixmap : + GWindow.window -> int -> int -> Gdk.pixmap * GDraw.drawable + +(** [gtk_tools_create_stipple_pixmap_from_data data width height] crée une pixmap + utilisable pour faire un stipple a partir de [data] qui est une liste d'entiers *) +val create_stipple_pixmap_from_data : int list -> int -> int -> Gdk.pixmap + +(** [gtk_tools_rectangle_pixmap window color width height] crée une pixmap + rectangulaire de taille [width]*[height] et de couleur [color] *) +val rectangle_pixmap : GWindow.window -> GDraw.color -> int -> int -> + GDraw.pixmap + + +(** {6 Fenetres} *) + + +(** [gtk_tools_create_window title width height] crée une fenetre contenant une + boite verticale. Si [width] et [height] sont nuls, la fenetre n'a pas de + taille par défaut *) +val create_window : + string -> int -> int -> GWindow.window * GPack.box + +(** [gtk_tools_create_window_on_top title width height window] *) +val create_window_on_top : + string -> int -> int -> GWindow.window -> GWindow.window * GPack.box + +(** [gtk_tools_create_window_on_top2 title width height window] *) +val create_window_on_top2 : + string -> int -> int -> GWindow.window option -> GWindow.window * GPack.box + +(** [gtk_tools_create_modal_window title width height] est identique à la fonction + précédente sauf que la fenetre est modale *) +val create_modal_window : + string -> int -> int -> GWindow.window * GPack.box + +(** [gtk_tools_create_modal_window_on_top title width height window] *) +val create_modal_window_on_top : + string -> int -> int -> GWindow.window -> GWindow.window * GPack.box + +(** [gtk_tools_create_modal_window_on_top2 title width height window] *) +val create_modal_window_on_top2 : + string -> int -> int -> GWindow.window option -> GWindow.window * GPack.box + +(** [gtk_tools_create_window_with_menubar title width height menubar_items] crée + une fenetre contenant une barre de menus *) +val create_window_with_menubar : + string -> + int -> + int -> + string list -> + GWindow.window * GPack.box * GMenu.menu_shell GMenu.factory * + Gtk.accel_group * GMenu.menu array + +(** [gtk_tools_create_window_with_menubar_help title width height menubar_items] + crée une fenetre avec une barre de menus et un menu d'aide à gauche de + cette barre *) +val create_window_with_menubar_help : + string -> + int -> + int -> + string list -> + GWindow.window * GPack.box * GMenu.menu_shell GMenu.factory * + Gtk.accel_group * GMenu.menu array * GMenu.menu + +(** [gtk_tools_window_set_front window] met la fenetre [window] en avant plan *) +val window_set_front : GWindow.window -> unit + +(** [gtk_tools_get_window_geometry window] renvoie la position et la taille de + la fenetre sous la forme [((x, y), (largeur, hauteur))] *) +val get_window_geometry : + GWindow.window -> (int * int) * (int * int) + +(** set_window_position window (x, y) déplace la fenetre pour que son + coin superieur à gauche soit à la position [(x, y)] *) +val set_window_position : GWindow.window -> int * int -> unit + +(** [gtk_tools_window_modify_connect window callback] connection d'un callback + appelé lors du deplacement ou du changement de taille de la fenetre [window]. + Le callback reçoit en paramètres la position et la taille de la fenetre *) +val window_modify_connect : + GWindow.window -> ((int * int) * (int * int) -> 'a) -> GtkSignal.id + +(** [gtk_tools_connect_win_focus_change window focus_in focus_out] connecte les + callbacks correspondants aux événements focus_in et focus_out à une fenetre *) +val connect_win_focus_change : + GWindow.window -> (unit -> 'a) option -> (unit -> 'b) option -> unit + + + +(** {6 Zones de dessin (Drawing Areas)} *) + + +(** [gtk_tools_create_draw_area_simple width height pack_method pix_expose] crée + une zone de dessin simple où [pix_expose] désigne la pixmap à utiliser pour + redessiner la zone après un event expose. En retour, un couple contenant + la zone créée et la fonction de mise à jour du dessin est renvoyé *) +val create_draw_area_simple : + int -> + int -> + (GObj.widget -> unit) -> Gdk.pixmap -> GMisc.drawing_area * (unit -> unit) + +(** [gtk_tools_area_mouse_connect area mouse_press mouse_move mouse_release] + connecte les événements souris à la zone de dessin [area] *) +val area_mouse_connect : + GMisc.drawing_area -> + (GdkEvent.Button.t -> bool) -> + (GdkEvent.Motion.t -> bool) -> (GdkEvent.Button.t -> bool) -> unit + +(** [gtk_tools_area_key_connect area key_press key_release] connecte les + événements claviers à la zone de dessin [area] *) +val area_key_connect : + GMisc.drawing_area -> (Gdk.keysym -> bool) -> (Gdk.keysym -> bool) -> unit + +(** [gtk_tools_create_draw_area width height pack_method pix_expose + mouse_press mouse_move mouse_release] crée une zone de dessin et connecte + les événements souris *) +val create_draw_area : + int -> + int -> + (GObj.widget -> unit) -> + Gdk.pixmap -> + (GdkEvent.Button.t -> bool) -> + (GdkEvent.Motion.t -> bool) -> (GdkEvent.Button.t -> bool) -> unit -> unit + + +(** {6 Sélection de couleurs} *) + + +(** [gtk_tools_select_color update_func] crée une boite de selection de couleur. + La fonction [update_func] est appelée après sélection avec en paramètre la + couleur choisie *) +val select_color : ([> `RGB of int * int * int] -> unit) -> unit + +(** [gtk_tools_create_color_selection_button window taille_x taille_y color + pack_method callback] crée un bouton coloré permettant le choix d'une couleur. + - [window] désigne la fenetre mère + - [taille_x] largeur du bouton + - [taille_y] hauteur du bouton + - [color] couleur initiale du bouton + - [pack_method] indique où mettre le bouton + - [callback] fonction appelée lorsqu'une nouvelle couleur est selectionnée. + Cette fonction reçoit en paramètre la nouvelle couleur + + Lors du choix d'une nouvelle couleur, la couleur du bouton n'est pas mise à jour + *) +val create_color_selection_button : + < misc : #GDraw.misc_ops; .. > -> + int -> + int -> + GDraw.color -> + (GObj.widget -> unit) -> + ([> `RGB of int * int * int] -> unit) -> GButton.button + +(** [gtk_tools_create_color_selection_button2 window taille_x taille_y color + pack_method callback] identique à la fonction précédente sauf que la couleur + selectionnee est mise à jour dans le bouton *) +val create_color_selection_button2 : + < misc : #GDraw.misc_ops; .. > -> + int -> + int -> + GDraw.color -> + (GObj.widget -> unit) -> (GDraw.color -> unit) -> GButton.button + +(** [gtk_tools_select_colors_widget window colors tooltips update_func vbox] crée + un widget de selection de plusieurs couleurs. [colors] est une liste d'éléments + au format [(nom_frame, (label, couleur ref, couleur_defaut) list)] *) +val select_colors_widget : + GWindow.window -> + (string * (string * GDraw.color ref * GDraw.color) list) list -> + GData.tooltips option -> (unit -> unit) -> GPack.box -> unit + +(** [gtk_tools_select_colors title width height colors tooltips update_func] crée + une fenetre de sélection de plusieurs couleurs *) +val select_colors : + string -> + int -> + int -> + (string * (string * GDraw.color ref * GDraw.color) list) list -> + GData.tooltips option -> (unit -> unit) -> unit + + +(** {6 Captures d'écran} *) + + +(** [creation_fen_capture default_filename default_format tooltips with_caption] + fonction interne de création d'une boite de capture d'écran *) +val creation_fen_capture : + string -> + Gtk_image.format_capture -> + GData.tooltips option -> + bool -> + string ref * Gtk_image.format_capture ref * + (Gtk_image.progress_save -> float -> unit) option * [> `DELETE_EVENT ] GWindow.dialog * + GButton.button * GEdit.entry * + (unit -> + (string * GDraw.color * GDraw.color * GDraw.color * Gdk.font) option) + +(** [gtk_tools_screenshot_box default_filename default_format drawable + tooltips x y width height] crée une boite de capture d'écran sans légende *) +val screenshot_box : + string -> + Gtk_image.format_capture -> + [> `drawable ] Gobject.obj -> + GData.tooltips option -> int -> int -> int -> int -> unit + +(** [gtk_tools_screenshot_box_with_func default_filename default_format drawable + tooltips x y width height after_func] identique à la fonction précédente sauf + que la fonction [after_func] est appelée après la capture *) +val screenshot_box_with_func : + string -> + Gtk_image.format_capture -> + [> `drawable ] Gobject.obj -> + GData.tooltips option -> int -> int -> int -> int -> (unit -> unit) -> unit + +(** [gtk_tools_screenshot_box_with_caption default_filename default_format + drawable tooltips x y width height] boite de capture écran avec légende. Ne + fonctionne qu'avec des pixmaps à cause de la légende *) +val screenshot_box_with_caption : + string -> + Gtk_image.format_capture -> + Gdk.pixmap -> GData.tooltips option -> int -> int -> int -> int -> unit + + +(** {6 Sélection de valeurs entières} *) + + +(** [gtk_tools_create_int_spinner_simple label lab_width init_value min_value + max_value value_width step_incr page_incr tip tooltips pack_method] crée un + widget de sélection d'une valeur entière *) +val create_int_spinner_simple : + string -> + int -> + int -> + int -> + int -> + int -> + int -> + int -> + string -> GData.tooltips -> (GObj.widget -> unit) -> GEdit.spin_button + +(** [gtk_tools_int_spinner_connect sp callback] connecte le callback à un widget de + sélection de valeur entière. Le callback est appelé avec la valeur selectionnée + à chaque fois que celle-ci change *) +val int_spinner_connect : GEdit.spin_button -> (int -> unit) -> unit + +(** [gtk_tools_create_int_spinner label lab_width init_value min_value max_value + value_width step_incr page_incr tip tooltips pack_method callback] crée un + widget de sélection de valeur entière avec callback *) +val create_int_spinner : + string -> + int -> + int -> + int -> + int -> + int -> + int -> + int -> + string -> + GData.tooltips -> + (GObj.widget -> unit) -> (int -> unit) -> GEdit.spin_button + +(** [gtk_tools_create_vslider_simple init_val min_val max_val step page draw_val + pack_method] création d'un slider vertical de sélection d'une valeur entière *) +val create_vslider_simple : + int -> + int -> + int -> + int -> + int -> bool -> (GObj.widget -> unit) -> GData.adjustment * GRange.scale + +(** [gtk_tools_create_hslider_simple init_val min_val max_val step page draw_val + pack_method] création d'un slider horizontal de sélection d'une valeur entière *) +val create_hslider_simple : + int -> + int -> + int -> + int -> + int -> bool -> (GObj.widget -> unit) -> GData.adjustment * GRange.scale + +(** [gtk_tools_slider_connect slider callback] connection d'un callback de + modification d'un slider de valeur entière *) +val slider_connect : + GData.adjustment -> (int -> unit) -> GtkSignal.id + +(** [gtk_tools_create_vslider init_val min_val max_val step page draw_val + pack_method callback] création d'un slider vertical de sélection d'une valeur + entière avec callback *) +val create_vslider : + int -> + int -> + int -> + int -> + int -> + bool -> + (GObj.widget -> unit) -> (int -> unit) -> GData.adjustment * GRange.scale + +(** [gtk_tools_create_hslider init_val min_val max_val step page draw_val + pack_method callback] création d'un slider horizontal de sélection d'une valeur + entière avec callback *) +val create_hslider : + int -> + int -> + int -> + int -> + int -> + bool -> + (GObj.widget -> unit) -> (int -> unit) -> GData.adjustment * GRange.scale + + +(** {6 Sélection de valeurs flottantes} *) + + +(** [gtk_tools_create_float_spinner_simple label lab_width init_value min_value + max_value value_width nb_digits step_incr page_incr tip tooltips pack_method] + crée un widget de sélection d'une valeur flottante *) +val create_float_spinner_simple : + string -> + int -> + float -> + float -> + float -> + int -> + int -> + float -> + float -> + string -> GData.tooltips -> (GObj.widget -> unit) -> GEdit.spin_button + +(** [gtk_tools_float_spinner_connect sp callback] connecte le callback à un widget de + sélection de valeur flottante. Le callback est appelé avec la valeur selectionnée + à chaque fois que celle-ci change *) +val float_spinner_connect : GEdit.spin_button -> (float -> unit) -> unit + +(** [gtk_tools_create_float_spinner label lab_width init_value min_value max_value + value_width nb_digits step_incr page_incr tip tooltips pack_method callback] + crée un widget de sélection de valeur flottante avec callback *) +val create_float_spinner : + string -> + int -> + float -> + float -> + float -> + int -> + int -> + float -> + float -> + string -> + GData.tooltips -> + (GObj.widget -> unit) -> (float -> unit) -> GEdit.spin_button + + +(** {6 Zones de texte} *) + + +(** [gtk_tools_create_text_entry_simple label lab_width init_value value_width + tip tooltips pack_method] : zone de sélection de texte. Renvoie le label et + la zone de sélection de texte *) +val create_text_entry_simple : + string -> + int -> + string -> + int -> + string -> + GData.tooltips -> (GObj.widget -> unit) -> GMisc.label * GEdit.entry + +(** [gtk_tools_text_entry_connect entry callback] connecte un callback lié à + l'appui sur la touche entrée dans la zone [entry] *) +val text_entry_connect : + GEdit.entry -> (string -> unit) -> GtkSignal.id + +(** [gtk_tools_text_entry_connect_modify entry callback] connecte un callback + appelé à chaque fois que le texte de la zone [entry] est modifié. Le callback + reçoit la chaine contenue dans la zone en majuscules *) +val text_entry_connect_modify : + GEdit.entry -> (string -> unit) -> GtkSignal.id + +(** [gtk_tools_create_text_entry label lab_width init_value value_width + tip tooltips pack_method callback] zone de sélection de texte avec callback + lié à la modification du texte dans la zone *) +val create_text_entry : + string -> + int -> + string -> + int -> + string -> + GData.tooltips -> + (GObj.widget -> unit) -> (string -> unit) -> GMisc.label * GEdit.entry + +(** [gtk_tools_text_entry_select_text entry] selectionne le texte présent dans + la zone de texte *) +val text_entry_select_text : GEdit.entry -> unit + +(** [gtk_tools_create_text_edit editable with_vert_scroll with_hor_scroll pack_method] + crée une zone de texte multiligne (editeur) avec éventuellement des barres de + défilement ([with_vert_scroll] et [with_hor_scroll]). [editable] indique si + la zone créée est éditable par l'utilisateur *) +val create_text_edit : bool -> bool -> bool -> (GObj.widget -> unit) + -> GText.view + +(** [gtk_tools_text_edit_clear edit] efface le contenu d'une zone de texte *) +val text_edit_clear : GText.view -> unit + +(** [gtk_tools_text_edit_get_text edit] renvoie l'intégralité du texte contenu + dans [edit] *) +val text_edit_get_text : GText.view -> string + +(** [gtk_tools_text_edit_get_lines edit] identique à la fonction précédente sauf + que le texte contenu dans le widget est renvoyé sous la forme d'une liste de + chaines de caractères, chacune d'elles correspondant à une ligne dans la + zone de texte *) +val text_edit_get_lines : GText.view -> string list + +(** [gtk_tools_text_edit_set_text_list edit text_lst] insère la liste de chaines + de caractères dans la zone de texte *) +val text_edit_set_text_list : GText.view -> string list -> unit + +(** [gtk_tools_text_edit_set_text edit text] insère le texte indiqué dans la + zone de texte *) +val text_edit_set_text : GText.view -> string -> unit + + +(** {6 Fenetres enregistrées} *) + + +(** Type pour les fenetres enregistrées *) +type registered_win = { + reg_win_id : int; + mutable reg_win_handle : GWindow.window option; + reg_win_build : unit -> GWindow.window; +} + +(** Exception levée lors de l'appel à un numéro de fenetre non enregistrée *) +exception GTK_TOOLS_UNREGISTERED_WINDOW of int + +(** [gtk_tools_register_window build_window_func] enregistre une fenetre. + [build_window_func] est la fonction de création de la fenetre en question *) +val register_window : (unit -> GWindow.window) -> int + +(** [gtk_tools_show_registered_window id] crée la fenetre enregistrée designée par + [id]. Si la fenetre existe déjà elle est mise en avant plan *) +val show_registered_window : int -> unit + +(** [gtk_tools_hide_registered_window id] détruit la fenetre enregistrée si elle + existe *) +val hide_registered_window : int -> unit + +(** [gtk_tools_get_registered_window id] renvoie la fenetre enregistrée + correspondant à l'identifiant [id] *) +val get_registered_window : int -> GWindow.window option + + +(** {6 Listes} *) + + +(** [gtk_tools_create_list lst_titles (sortable_titles, first_sort) pack_method] + crée une liste : + - [lst_titles] liste des titres des colonnes + - [sortable_titles] les titres sont-ils sélectionnables pour trier la liste ? + - [first_sort] indique quelle est la colonne qui sert de tri initialement + - [pack_method] où mettre la liste *) +val create_list : + string list -> bool * int -> (GObj.widget -> unit) -> string GList.clist + +(** [gtk_tools_create_list_with_hor_scroll lst_titles + (sortable_titles, first_sort) pack_method] crée une liste avec une barre de + scroll horizontale *) +val create_list_with_hor_scroll : + string list -> bool * int -> (GObj.widget -> unit) -> string GList.clist + +(** [gtk_tools_set_columns_sizes list sizes] met à jour la taille des colonnes + de la liste [list] *) +val set_columns_sizes : string GList.clist -> int list -> unit + +(** [gtk_tools_list_connect clist + callback_select callback_deselect callback_select_column] connecte les + événements sélection/désélection et sélection d'un titre de colonne. Les + callbacks [callback_select] et [callback_deselect] reçoivent en paramètres + la ligne et la colonne (dé)sélectionnées. [callback_select_column] reçoit le + numéro de la colonne *) +val list_connect : + string GList.clist -> + (int -> int -> unit) option -> + (int -> int -> unit) option -> (int -> unit) option -> unit + +(** [gtk_tools_list_connect_check_dbl_click clist + callback_select callback_deselect callback_select_column] meme chose que la + fonction précédente sauf que l'on teste s'il y a double click. Ici, + [callback_select] et [callback_deselect] reçoivent la ligne, la colonne et + un booleen indiquant s'il y a eu double click *) +val list_connect_check_dbl_click : + string GList.clist -> + (int -> int -> bool -> unit) option -> + (int -> int -> bool -> unit) option -> (int -> unit) option -> unit + +(** [gtk_tools_list_connect_up_down_keys clist callback_up callback_down] + connecte les callbacks liés à l'appui sur les touches flechées haut et bas *) +val list_connect_up_down_keys : + string GList.clist -> (unit -> 'a) -> (unit -> 'b) -> unit + +(** [gtk_tools_create_managed_list titles_sizes pack_method] crée une liste managée. + [titles_sizes] est une liste du type [(nom, taille)] indiquant le titre + ainsi que la largeur des différentes colonnes. Cet objet contient en plus + un label indiquant le nombre d'éléments contenus dans la liste. + + En retour, la liste et le label sont renvoyes *) +val create_managed_list : + (string * int) list -> + (GObj.widget -> unit) -> string GList.clist * GMisc.label + +(** [gtk_tools_connect_managed_list (lst, lab) item_column selection_callback + (name, female, cap)] connecte le callback de (dé)sélection à une liste + managée, les callbacks de selection avec les touches flechées et indique le + format du label de titre : + - [(lst, lab)] = la liste et le label renvoyés par la fonction précédente + - [item_column] = numéro de la colonne contenant l'item de référence + - [selection_callback] = la fonction appelée lors d'une (dé)sélection. + Elle appelée avec en paramètres : l'élément sélectionné (i.e élément de la ligne + sélectionnée et dans la colonne [item_column]), un triplet [(ligne, colonne, + double_ckick)] et un booleen valant vrai si sélection et faux si désélection + - [(name, female, cap)] indique le format à utiliser pour afficher le nombre + d'éléments de la liste (voir [eval_string]) + + En retour est renvoyée la fonction permettant de mettre à jour le contenu de + la liste. Cett fonction prend en paramètres : + - l'identifiant de la ligne à sélectionner par defaut ("" si pas de + sélection initiale) + - une liste contenant des listes correspondant aux différentes lignes *) +val connect_managed_list : + string GList.clist * GMisc.label -> + int -> + (string -> int * int * bool -> bool -> unit) -> + string * bool * bool -> string -> string list list -> unit + + +(** {6 Widget Lat/Lon} *) + + +(** Type contenant un widget de sélection de coordonnées lat/lon *) +type latlon = { + latlon_lat_val : float ref; + latlon_lon_val : float ref; + latlon_update : float -> float -> unit; + latlon_change_callback : (unit -> unit) option ref; +} + +(** [gtk_tools_create_latlon_selection (lat, lon) pack_method tooltips] crée un + widget de sélection de coordonnées géographiques lat/lon *) +val create_latlon_selection : + float * float -> + (GObj.widget -> unit) -> GData.tooltips -> latlon + +(** [gtk_tools_update_latlon_selection latlon_widget new_lat new_lon] met à jour + les coordonnées dans un widget de sélection lat/lon *) +val update_latlon_selection : + latlon -> float -> float -> unit + +(** [gtk_tools_latlon_selection_get latlon_widget] recupère les coordonnées + selectionnées dans le widget *) +val latlon_selection_get : latlon -> float * float + +(** [gtk_tools_latlon_selection_change latlon_widget callback] connecte un + callback appelé lors de la modification des coordonnées. Il ne prend pas de + paramètre : pour avoir connaitre la valeur des coordonnées, il faut utiliser + {!Gtk_tools.gtk_tools_latlon_selection_get} *) +val latlon_selection_change : + latlon -> (unit -> unit) -> unit + + +(** {6 Fenetre de Log} *) + +(** Exception levée lors de l'ajout de texte dans la fenetre de log alors que + celle-ci n'a pas encore ete créée *) +exception GTK_TOOLS_NO_LOG_WIN + +(** renvoie la zone de texte de la fenetre de log si cette derniere a ete créée. + Si ce n'est pas le cas alors l'exception {!Gtk_tools.GTK_TOOLS_NO_LOG_WIN} + est levée *) +val get_log_wid : unit -> GText.view + +(** [gtk_tools_add_log text level] ajoute un texte dans la fenetre de log + si [level]<[log_verbose_level]*) +val add_log : string -> int -> unit + +(** [gtk_tools_add_log_with_color text level color] meme chose que la + fonction précédente sauf que [color] indique la couleur du texte à ajouter *) +val add_log_with_color : string -> int -> GDraw.color -> unit + +(** efface le contenu de la fenetre de log *) +val clear_log : unit -> unit + +(** [gtk_tools_create_log tooltips] crée la fenetre de log *) +val create_log : GData.tooltips -> unit + +(** force l'affichage de la fenetre de log *) +val show_log : unit -> unit + +(** détruit la fenetre de log *) +val hide_log : unit -> unit + +(** [gtk_tools_set_log_verbose_level level] met à jour le niveau de verbose + dans la fenetre de log *) +val set_log_verbose_level : int -> unit + + +(** {6 Barres de progression} *) + + +(** [gtk_tools_create_progress_bar_win nb_blocks title] crée une barre de progression + dans une fenetre externe. [nb_blocks] désigne le nombre de subdivisions de + la barre. En sortie, la fonction de mise à jour de la barre de progression + est renvoyée. Cette fonction prend en paramètre un flottant compris entre + 0.0 et 1.0, lorsqu'on lui passe 1.0, la fenetre est fermée *) +val create_progress_bar_win : int -> string -> float -> unit + +(** [gtk_tools_create_progress_bar pack_method] creation d'une barre de + progression continue et sans fenetre (donc différente de + {!Gtk_tools.gtk_tools_create_progress_bar_win}. + La fonction de mise à jour de la progression est renvoyée et prend en + paramètre un flottant compris entre 0.0 et 1.0 *) +val create_progress_bar : (GObj.widget -> unit) -> float -> unit + + +(** {6 Widget de selection d'opérateur de comparaison} *) + + +(** Opérateurs de comparaison *) +type t_ops_compare = T_EQ | T_L | T_LEQ | T_G | T_GEQ + +(** [gtk_tools_create_ops_compare variable callback_modified pack_method] crée un + widget de sélection d'opérateur de comparaison. + - [variable] est une référence sur l'opérateur en cours + - [callback_modified] est appelé lorsque l'opérateur est modifié. Cette fonction + est optionnelle. Si elle est utilisée alors elle prend en paramètre la + valeur courante de l'opérateur de comparaison + - [pack_method] indique où mettre le widget de sélection + *) +val create_ops_compare : + t_ops_compare ref -> + (t_ops_compare -> unit) option -> (GObj.widget -> unit) -> unit + + +(** {6 Widget de sélection d'une heure} *) + + +(** [gtk_tools_create_time_select variable callback_modified pack_method] crée un + widget de sélection d'heure. [variable] est une référence sur un entier + contenant l'heure en secondes *) +val create_time_select : + int ref -> (int -> unit) option -> (GObj.widget -> unit) -> unit + + +(** {6 Fenetre d'affichage d'infos} *) + + +(** [gtk_tools_create_infos_win title width height] crée une fenetre d'affichage + d'informations sous forme de labels. + + En retour est renvoyé un couple contenant la fonction d'ajout d'un nouveau texte + ainsi que la fonction de fermeture de la fenetre *) +val create_infos_win : + string -> int -> int -> (string -> unit) * (unit -> unit) + + +(** {6 Widget affichant le contenu d'un fichier texte} *) + + +(** [gtk_tools_display_file filename title width height tooltips font] affiche + le contenu d'un fichier dans une fenetre *) +val display_file : + string -> string -> int -> int -> GData.tooltips -> string option -> unit + + +(** {6 Combo box} *) + + +(** [gtk_tools_create_combo lst_items pack_method] crée une combo box *) +val create_combo_simple : + string list -> (GObj.widget -> unit) -> GEdit.entry + +(** [gtk_tools_combo_connect entry lst_items callback] connecte [callback] qui est + appelé lors de la modification de la valeur dans la combo box *) +val combo_connect : GEdit.entry -> (string*'a) list -> ('a->unit) -> unit + + +(** {6 Widget calendrier} *) + +(** [gtk_tools_calendar lst_dates callback_select only_available_dates_selectable + init_with_last_available_date tooltips win pack_method] crée un widget + calendrier permettant de choisir une date. Les paramètres sont : + + - [lst_dates] contient (éventuellement) la liste des dates autorisées sous + la forme d'un entier (ex. : 20030721) + - [callback_select] désigne la fonction appelée après sélection. La date choisie + est passée en paramètre + - [only_available_dates_selectable] indique si celles les dates autorisées sont + selectionnables (ainsi, seules ces dernières peuvent appeler [callback_select]) + - [init_with_last_available_date] sert à l'initialisation du calendrier. + Si cette valeur vaut vrai, le calendrier affiche le mois et l'année correspondant + à la date la plus tardive parmi les dates autorisées. Sinon, le mois courant + est affiché + - [tooltips] désigne le système d'aide contextuelle + - [win] correspond à la fenetre mère + - [pack_method] indique où mettre le widget + *) +val calendar : int list -> (int -> unit) -> bool -> bool -> + GData.tooltips -> GWindow.window -> (GObj.widget -> unit) -> unit + +(** [gtk_tools_calendar_window lst_dates callback_select only_available_dates_selectable + init_with_last_available_date is_modal tooltips] crée une boite de dialogue + affichant un calendrier et permettant de choisir une date. + + Les paramètres utilisés sont les suivants : + + - [lst_dates] contient (éventuellement) la liste des dates autorisées sous + la forme d'un entier (ex. : 20030721) + - [callback_select] désigne la fonction appelée après sélection. La date choisie + est passée en paramètre + - [only_available_dates_selectable] indique si celles les dates autorisées sont + selectionnables (ainsi, seules ces dernières peuvent appeler [callback_select]) + - [init_with_last_available_date] sert à l'initialisation du calendrier. + Si cette valeur vaut vrai, le calendrier affiche le mois et l'année correspondant + à la date la plus tardive parmi les dates autorisées. Sinon, le mois courant + est affiché + - [is_modal] indique si la fenetre doit etre modale + *) +val calendar_window : int list -> (int -> unit) -> bool -> bool -> bool -> + GData.tooltips -> unit diff --git a/sw/lib/ocaml/gtk_tools_GL.ml b/sw/lib/ocaml/gtk_tools_GL.ml new file mode 100644 index 0000000000..e7e4080e15 --- /dev/null +++ b/sw/lib/ocaml/gtk_tools_GL.ml @@ -0,0 +1,149 @@ +(* + * $Id$ + * + * OpenGL utils + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) +(* = YLF 28/10/2001 = *) +(* = = *) +(* = Derniere update : 28/10/2002 = *) +(* = = *) +(* = = *) +(* = 28/10/2002 : create_draw_glarea_base et = *) +(* = et connect_draw_glarea_simple = *) +(* = 15/05/2002 : gl_to_gtk_color et gtk_to_gl_color = *) +(* = 12/04/2002 : create_draw_glarea_simple = *) +(* = = *) +(* ================================================================================== *) + +open Gtk_tools + +(* ============================================================================= *) +(* = Creation d'une drawing area OpenGL = *) +(* = = *) +(* = width = hauteur de la zone = *) +(* = height = hauteur de la zone = *) +(* = pack_method = maniere de placer la zone (ex. : hbox#pack) = *) +(* ============================================================================= *) +let create_draw_glarea_base width height pack_method = + (* Creation des widgets *) + GlGtk.area [`DEPTH_SIZE 1; `RGBA; `DOUBLEBUFFER] + ~width:width ~height:height ~packing:pack_method () + +(* ============================================================================= *) +(* = Connection des fonctions de base a une drawing area OpenGL = *) +(* = = *) +(* = area = la zone = *) +(* = init_func = fonction d'initialisation (realize) = *) +(* = display_func = fonction de dessin dans la zone = *) +(* = reshape_func = appelee lors d'un changement de taille = *) +(* ============================================================================= *) +let connect_draw_glarea_simple area + init_func display_func reshape_func = + (* La nouvelle fonction de dessin appelle celle qui est passee en parametre quand *) + (* necessaire et fait le flush ensuite *) + let draw = (fun () -> if (area:GlGtk.area)#misc#visible then begin + display_func (); Gl.flush (); area#swap_buffers () + end) in + + (* Connection des fonctions *) + ignore(area#connect#realize ~callback:init_func) ; + ignore(area#connect#display ~callback:draw) ; + ignore(area#connect#reshape ~callback:reshape_func) ; + + (* Renvoie la nouvelle fonction de dessin *) + draw + +(* ============================================================================= *) +(* = Creation d'une drawing area OpenGL = *) +(* = = *) +(* = width = hauteur de la zone = *) +(* = height = hauteur de la zone = *) +(* = pack_method = maniere de placer la zone (ex. : hbox#pack) = *) +(* = init_func = fonction d'initialisation (realize) = *) +(* = display_func = fonction de dessin dans la zone = *) +(* = reshape_func = appelee lors d'un changement de taille = *) +(* ============================================================================= *) +let create_draw_glarea_simple width height pack_method + init_func display_func reshape_func = + (* Creation des widgets *) + let area = create_draw_glarea_base width height pack_method in + + let draw = connect_draw_glarea_simple area + init_func display_func reshape_func in + + (* Renvoie la zone de dessin et la nouvelle fonction de dessin *) + (area, draw) + +(* ============================================================================= *) +(* = Connexion evenements souris a une zone de dessin OpenGL = *) +(* = = *) +(* = area = la zone de dessin = *) +(* = mouse_press = fonction appelee lors d'un click souris = *) +(* = mouse_move = fonction appelee lors d'un deplacement = *) +(* = mouse_release = fonction appelee lors du relachement d'un bouton = *) +(* ============================================================================= *) +let glarea_mouse_connect area mouse_press mouse_move mouse_release = + (area:GlGtk.area)#event#add [`POINTER_MOTION; `BUTTON_PRESS; `BUTTON_RELEASE] ; + area#event#set_extensions `ALL; + ignore(area#event#connect#button_press ~callback:mouse_press) ; + ignore(area#event#connect#motion_notify ~callback:mouse_move) ; + ignore(area#event#connect#button_release ~callback:mouse_release) + +(* ============================================================================= *) +(* = Connexion evenements clavier a une zone de dessin = *) +(* = = *) +(* = area = la zone de dessin = *) +(* = key_press = fonction appelee lors de l'appui sur une touche = *) +(* = key_release = fonction appelee lors du relachement d'une touche = *) +(* ============================================================================= *) +let glarea_key_connect area key_press key_release = + (* Par defaut l'evenement key_release n'est pas associe au widget *) + (area:GlGtk.area)#event#add [`KEY_RELEASE] ; + ignore(area#event#connect#key_press + ~callback:(fun ev -> key_press (GdkEvent.Key.keyval ev))) ; + ignore(area#event#connect#key_release + ~callback:(fun ev -> key_release (GdkEvent.Key.keyval ev))) ; + area#misc#set_can_focus true ; + area#misc#grab_focus () + +(* ============================================================================= *) +(* = Passage de couleur GTK vers GL = *) +(* = = *) +(* = color = couleur GTK (`NAME ou `RGB) a transformer = *) +(* ============================================================================= *) +let gtk_to_gl_color color = + let t = GDraw.color color in + ((float_of_int (Gdk.Color.red t))/.65535.0, + (float_of_int (Gdk.Color.green t))/.65535.0, + (float_of_int (Gdk.Color.blue t))/.65535.0) + +(* ============================================================================= *) +(* = Passage de couleur GL vers GTK = *) +(* = = *) +(* = (r, g, b) = couleur GL a transformer en equivalent GTK = *) +(* ============================================================================= *) +let gl_to_gtk_color (r, g, b) = + `RGB(int_of_float (r*.65535.0), int_of_float (g*.65535.0), + int_of_float (b*.65535.0)) + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/gtk_tools_GL.mli b/sw/lib/ocaml/gtk_tools_GL.mli new file mode 100644 index 0000000000..b7448efc22 --- /dev/null +++ b/sw/lib/ocaml/gtk_tools_GL.mli @@ -0,0 +1,90 @@ +(* + * $Id$ + * + * OpenGL utils + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(** Module de gestion des zones de dessin OpenGL + + {b Dépendences : Platform} + + *) + + +(** {6 Drawing Areas OpenGL} *) + + +(** [create_draw_glarea_base width height pack_method] crée une zone de + dessin OpenGL de largeur [width] et de hauteur [height]. Cette zone est + placée comme indiqué dans [pack_method]. La zone créée est renvoyée *) +val create_draw_glarea_base : + int -> int -> (GObj.widget -> unit) -> GlGtk.area + +(** [connect_draw_glarea_simple area init_func display_func reshape_func] + connecte les signaux de base à une zone de dessin créée avec + {!Gtk_tools_GL.create_draw_glarea_base}. La fonction de redessin + est renvoyée *) +val connect_draw_glarea_simple : + GlGtk.area -> + (unit -> unit) -> + (unit -> 'a) -> (width:int -> height:int -> unit) -> unit -> unit + +(** [create_draw_glarea_simple width height pack_method + init_func display_func reshape_func] crée une zone de dessin OpenGL et y + connecte les signaux. Un couple contenant la zone et la fonction de + redessin est renvoyé *) +val create_draw_glarea_simple : + int -> + int -> + (GObj.widget -> unit) -> + (unit -> unit) -> + (unit -> 'a) -> + (width:int -> height:int -> unit) -> GlGtk.area * (unit -> unit) + + +(** {6 Signaux des Drawing Areas OpenGL} *) + + +(** [glarea_mouse_connect area mouse_press mouse_move mouse_release] + connecte les événements souris à la zone de dessin [area] *) +val glarea_mouse_connect : + GlGtk.area -> + (GdkEvent.Button.t -> bool) -> + (GdkEvent.Motion.t -> bool) -> (GdkEvent.Button.t -> bool) -> unit + +(** [glarea_key_connect area key_press key_release] connecte les + événements claviers à la zone de dessin [area] *) +val glarea_key_connect : + GlGtk.area -> (Gdk.keysym -> bool) -> (Gdk.keysym -> bool) -> unit + + +(** {6 Couleurs Gtk <-> OpenGL} *) + + +(** [gtk_to_gl_color color] crée une couleur OpenGL (r, g, b) à partir + de [color]. Les composantes RGB sont dans l'intervalle [\[0.0, 1.0\]] *) +val gtk_to_gl_color : GDraw.color -> float * float * float + +(** [gl_to_gtk_color (r, g, b)] fonction inverse de la precedente *) +val gl_to_gtk_color : + float * float * float -> [> `RGB of int * int * int] diff --git a/sw/lib/ocaml/gtk_tools_icons.ml b/sw/lib/ocaml/gtk_tools_icons.ml new file mode 100644 index 0000000000..794ee1c41d --- /dev/null +++ b/sw/lib/ocaml/gtk_tools_icons.ml @@ -0,0 +1,238 @@ +(* + * $Id$ + * + * Icons library + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +let question_icon = + [|"48 48 69 1"; + " c #000000";". c #060708";"X c #06080a";"o c #0a0a0a";"O c #19150d"; + "+ c gray8";"@ c gray10";"# c #221c12";"$ c #393939";"% c #2b3d61"; + "& c #354564";"* c #654f24";"= c #6c5526";"- c #705627";"; c #715929"; + ": c #735f3b";"> c #7c622d";"; c #7f6941";"< c gray39";"1 c #727272"; + "2 c #737579";"3 c gray50";"4 c #81642e";"5 c #876a31";"6 c #8c6d31"; + "7 c #937435";"8 c #9b7b3a";"9 c #a27f3b";"0 c #927c52";"q c #a6823c"; + "w c #a9853d";"e c #a68748";"r c #a38e55";"t c #a48b5a";"y c #b38d40"; + "u c #bc9443";"i c #be9b53";"p c #bea363";"a c #bfa16a";"s c #bea272"; + "d c #c09745";"f c #c39f56";"g c #c9a45b";"h c #d2a64c";"j c #d8ab4e"; + "k c #d8ac5a";"l c #d8b15f";"z c #c3a466";"x c #c6a76a";"c c #c9ac73"; + "v c #d2b06c";"b c #d9b263";"n c #d8b56e";"m c #dbba73";"M c #d8ba7b"; + "N c #f7c35a";"B c #f7c96d";"V c #f7cf7e";"C c #aaaaaa";"Z c #d8be86"; + "A c #dcc494";"S c #f7d48c";"D c #f7d899";"F c #f7dca5";"G c #f7dfaf"; + "H c #f7e2b8";"J c #f7e5c0";"K c white";"L c None"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLLLo LLLLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLLL 9uj8q> LLLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLLL jDDHDMMMiq LLLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLL NHHDSNNNVMlq= LLLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLLo NGFNe77wNNVnlq LLLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLL pDHN7 7NNBll4 +LLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLo BJV7 % fNVjlq LLLLLLLLLLLLLL"; + "LLLLLLLLLLLLL rSGj &33 tNBljq KLLLLLLLLLLLLL"; + "LLLLLLLLLLLLL hVNq &3KK tBBnj4 .CKLLLLLLLLLLLL"; + "LLLLLLLLLLLLL NSNq 2KKL tNnnj* CKLLLLLLLLLLLL"; + "LLLLLLLLLLLLL vlu4 3KL zBMjq 3CKLLLLLLLLLLLL"; + "LLLLLLLLLLLLL XX tKL 6vZlj; 3CKLLLLLLLLLLLL"; + "LLLLLLLLLLLLL X X3K uSZju +3KLLLLLLLLLLLLL"; + "LLLLLLLLLLLLLL 0 c #361210";"; c #242424";"< c #2c2c2c";"1 c #323232"; + "2 c #3b3b3b";"3 c #41130e";"4 c #401411";"5 c #4d1712";"6 c #4e1814"; + "7 c #571b15";"8 c #591710";"9 c #581b15";"0 c #5c1e1a";"q c #6e190f"; + "w c #671c16";"e c #6b1d15";"r c #791e15";"t c #66221d";"y c #6c231e"; + "u c #7d231c";"i c #742620";"p c #772822";"a c #7f2821";"s c #484848"; + "d c #545454";"f c #656565";"g c #7e7e7e";"h c #951f11";"j c #981f11"; + "k c #83241b";"l c #87281e";"z c #892116";"x c #8b261c";"c c #89281f"; + "v c #952214";"b c #932419";"n c #92291f";"m c #9c2213";"M c #9a261a"; + "N c #9b291d";"B c #832b24";"V c #8c2a23";"C c #8e2e28";"Z c #8f3832"; + "A c #942c24";"S c #9b2d23";"D c #943029";"F c #9e3026";"G c #9d3229"; + "H c #9e3b32";"J c #a52415";"K c #a42618";"L c #a3291c";"P c #a82211"; + "I c #aa2718";"U c #ab291b";"Y c #b22413";"T c #b12718";"R c #b5291a"; + "E c #bb2513";"W c #bc2b1a";"Q c #a12d22";"! c #a92f20";"~ c #a03026"; + "^ c #a03229";"/ c #a5392e";"( c #ab3224";") c #a9352a";"_ c #a83a2e"; + "` c #a43c33";"' c #af3f33";"] c #c32613";"[ c #c72816";"{ c #c62d1b"; + "} c #ce2e1c";"| c #d22813";" . c #d02e1b";".. c #db2a15";"X. c #df301c"; + "o. c #e22b15";"O. c #ec2d15";"+. c #e4311b";"@. c #ea311b";"#. c #ea3a1b"; + "$. c #f4321a";"%. c #f6381c";"&. c #fd3216";"*. c #fb3318";"=. c #fc3b1a"; + "-. c #c1782e";";. c #ff401e";":. c #d48d1d";">. c #cd8622";";. c #c18138"; + "<. c #c79d37";"1. c #c49c39";"2. c #ca9d31";"3. c #cda137";"4. c #cfa43c"; + "5. c #d5a82e";"6. c #dcac2b";"7. c #d4a534";"8. c #d1a53a";"9. c #d8a733"; + "0. c #d9aa32";"q. c #daad3c";"w. c #dfb035";"e. c #ddb039";"r. c #e1ae23"; + "t. c #e1b233";"y. c #cfa540";"u. c #d4aa43";"i. c #d2aa48";"p. c #d8ac40"; + "a. c #dfb445";"s. c #fdfdfd";"d. c None"; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.=.&.*.*.$.O.@.@.@.@.@. d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.=.=.&.O.| ] E Y Y Y P Y W { } } W d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.&.&...] Y P P P P j v J J J I U R W R d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.=.&.O.] Y P P P P P m m P I J J K K L U U U z d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.=.&...E P P P P P P v J J J J J I K K L K L L L r d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.=.&.| Y P P P P P P J v J J K I K ) ( K K L K L L N 8 d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.=.&.| P P P P P P P P J J m m J K K K ( ) L L N L L N k # d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.=.o.Y P P P P J P ! ! J I J m m L K K K ) ) ) Q L L Q N w o d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.=.&.E P P P J P ! ! P ( ( J K K K b m L L K L _ ) Q L L S x ; o d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.*.| P P P P P J h S ! K ( ( K K K K b b L L L L ) _ Q Q L S 7 d.d.d.d.d.d."; + "d.d.d.d.d.d.d.#.*.E P P J h I P m P K ( ( ( ) J K K L b b N Q L L _ _ ( S Q u # $ d.d.d.d.d.d."; + "d.d.d.d.d.d.d.%...P P P P j J K I I K K ( ( ' ) L L L L M b N Q Q S / _ S S c = o d.d.d.d.d.d."; + "d.d.d.d.d.d. %.[ I :.r.r.r.r.r.r.6.6.6.6.6.e.a.e.0.0.0.0.7.2.2.7.8.8.-.Q S A 3 o d.d.d.d.d."; + "d.d.d.d.d.d. $.E P r.r.r.r.r.r.6.r.6.6.6.6.6.q.e.q.0.9.9.9.9.7.7.8.7.8.Q S A 5 . o d.d.d.d.d."; + "d.d.d.d.d.d. $.E P r.r.r.t.r.r.6.r.6.6.6.0.9.5.3.q.p.9.9.9.7.4.8.8.8.y.~ Q A 7 . , d.d.d.d.d."; + "d.d.d.d.d.d. #.Y P r.r.r.r.t.t.6.6.6.6.6.6.6.9.5.2.7.p.p.7.4.7.<.1.1.1.~ F D 9 . % d.d.d.d.d."; + "d.d.d.d.d.d. +.T J r.r.6.r.6.0.w.6.6.6.0.0.0.9.9.9.2.7.u.u.u.8.8.8.y.y.^ S C 6 % d.d.d.d.d."; + "d.d.d.d.d.d. X.T J r.6.6.6.6.6.e.e.0.0.0.6.2.9.7.7.7.2.3.8.u.u.y.8.4.y.^ G V : % d.d.d.d.d."; + "d.d.d.d.d.d. } R J >.6.6.6.6.6.6.e.e.9.0.9.2.2.9.7.7.3.3.4.8.i.i.4.y.,.^ G a & % s.d.d.d.d."; + "d.d.d.d.d.d. R W K K K I K L N M S ) _ L L Q S S S S S Q S S ^ ^ ` ^ ^ F D w X . % s.d.d.d.d."; + "d.d.d.d.d.d. q { U I K L K L U M b L / Q Q L Q n n Q A Q Q G Q ^ ^ ` G G C 6 o , s.d.d.d.d."; + "d.d.d.d.d.d. . U U K K L L L Q L N N N ^ S Q Q S c V A A ^ S G G ^ ^ ` D i # . . o 1 d.d.d.d.d."; + "d.d.d.d.d.d. o e R U L L L L L Q L N Q ) / S Q Q S F ~ S ^ ^ G ^ ^ G H Z 6 % d d.d.d.d.d."; + "d.d.d.d.d.d.d. . z U L L L L Q L S N N S ` Q S Q ~ Q ^ A A ^ G D / G C t + o < s.d.d.d.d.d."; + "d.d.d.d.d.d.d. + M K L S L Q Q ! Q S S S / Q Q ^ ^ ^ G D ^ ^ ^ G D p * $ s s.d.d.d.d.d."; + "d.d.d.d.d.d.d.d. . - M Q L L Q Q Q Q Q Q Q ` ` V A Q ^ ^ D D ^ G D p > o , g d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d. = x S S Q Q Q Q ~ ~ ~ A ` Z G ^ ^ G G G G C y * % d s.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.1 . @ e N N S S Q Q ~ ~ ~ S H ` G ^ G G D B 0 & % s s.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d. 3 k n S S S F F F G F ` G D D B y 4 . . O 1 s.s.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d. . X ; t k V V A A A C V B p 0 > X O < g s.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d. . # * 4 7 0 9 6 : * # . $ 2 g s.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d. . . . o , f s.s.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d. , % . . . . $ % d g s.s.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d. $ . . . o o . O O , s g s.s.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d. 1 , , % % % , , 1 d s.s.s.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.s.s.s.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."; + "d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d.d."|] + +let warning_icon = + [|"48 48 97 2";" c #000000";". c #0b0501";"X c #0b0900";"o c #0a0a0a"; + "O c #170402";"+ c #120e00";"@ c #1c0705";"# c #151001";"$ c #1c1501"; + "% c #131313";"& c #220704";"* c #260806";"= c #2c0805";"- c #2e0a08"; + "; c #251d02";": c #3b0b07";"> c #3d0c08";"; c #2a2103";"< c #312604"; + "1 c #3a2d05";"2 c gray17";"3 c #410c06";"4 c #440d09";"5 c #4b0d07"; + "6 c #490e09";"7 c #4d100c";"8 c #520e07";"9 c #571109";"0 c #5c110b"; + "q c #413205";"w c #4c3b06";"e c #513f07";"r c #62120a";"t c #6d140b"; + "y c #73150d";"u c #78150b";"i c #731711";"p c #7a1710";"a c #791812"; + "s c #524007";"d c #675009";"f c #69520a";"g c #71570a";"h c #745a0c"; + "j c #7b610a";"k c #85170c";"l c #86180d";"z c #8a180d";"x c #851a12"; + "c c #8a1a11";"v c #901a0e";"b c #9a1b0e";"n c #931d12";"m c #9c1e14"; + "M c #a41c0e";"N c #a91d0f";"B c #a01d11";"V c #b31f0f";"C c #b21f10"; + "Z c #b9200f";"A c #b62010";"S c #bc2111";"D c #ab4216";"F c #b14010"; + "G c #ba5b12";"H c #bf6f16";"J c #924f48";"K c #c62210";"L c #ca2311"; + "P c #d32412";"I c #da2613";"U c #e32712";"Y c #e42e1a";"T c #eb2713"; + "R c #ec2813";"E c #f42913";"W c #c26c13";"Q c #c67e15";"! c #c87e14"; + "~ c #b9930e";"^ c #bd9312";"/ c #cb8614";"( c #cd8912";") c #c39b0f"; + "_ c #c59913";"` c #cb9e13";"' c #d18913";"] c #d7a816";"[ c #d7a81a"; + "{ c #d8a51c";"} c #dcaa14";"| c #d8a919";" . c #dfb112";".. c #e0ad12"; + "X. c #e0b213";"o. c #808080";"O. c None"; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.R E L c o.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.T E P N l X O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.N E P N M b 0 O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.N E U V M M B z - % O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.R T Z M M A B b r O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.N E L M M G W M B v : O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.M E I M M M . .M B B u O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.R T Z M M .... .' B B v 5 O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.N E L M M W . . . .B B B y O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.N E I N M M ....~ ..} ( M b z 5 O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.U E Z M M ' ) X q } ..M B B y O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.M E P M M M .j . + ..} ( B m z 6 O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.T U V M M X. ., $ } } } B M m y O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.N E L M M ( X. .; ; } } } ( M m v 6 O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.N R I M M M ..X. .; ; q } } } } m B b y O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.t R S M M ( X. .} w q q } } } } ( B m c 4 O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.N E P N M M . . ...$ ; q } } } } ] m m m r O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.U R A M M . . ...} X X ; } } } } } ' m m x = O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.N R P N M ' . .} } } . + X < } } | } | | m m m 0 % O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.Y R A B M } ........} + s } } } | } | ( m m p @ O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.N R P B N ' .} ....} } 1 e + h } } | } | | | m m c 6 O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.Y T Z M B .} ....} } } ^ f < _ ] | } | } | | / m n i . O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.N R P M B ( .....} } } } } } } | } } | } | | [ | m m c > O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.Y T A B M .} ..} } } } } ` ; ; _ } | } | | [ | { H m n 0 O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.M R P M B ( ..} } ..} } } } w f | } ] | | [ [ { / m m a = O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.J R A B M } } } } } } } } } 1 X h | | | | | | | [ [ m m x 7 o o O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.M T I M M / ..} } } } } } } } ^ < j ] } | | | | | | [ [ H m m 0 o o O.O.O.O.O."; + "O.O.O.O.O.O.O.O.t R K M M m M M B B B G W / / } } } | | | | | | [ [ | { / D m a @ 2 O.O.O.O.O."; + "O.O.O.O.O.O.O.O.3 L B m M M M m B B B B B B m m m m B m m m m m m m m m m m m x > O.O.O.O."; + "O.O.O.O.O.O.O.O.O u 5 t t u l z n m m B B B B m m m m m m m m m m m m m m m m m 0 O.O.O.O."; + "O.O.O.O.O.O.O.O. o . & = 3 > 0 0 r t i y i x c x c c c x c x x x x x x a i 7 o O.O.O.O."; + "O.O.O.O.O.O.O.O. o . & @ & & 4 > 4 > 4 > > > : : > > > > - @ O.O.O.O."; + "O.O.O.O.O.O.O.O. . o . o o o o O.O.O.O."; + "O.O.O.O.O.O.O.O.O. o o . . o o o O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O. o o o O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O. O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."; + "O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O.O."|] diff --git a/sw/lib/ocaml/gtkgl_Hack.ml b/sw/lib/ocaml/gtkgl_Hack.ml new file mode 100644 index 0000000000..6cff47cea8 --- /dev/null +++ b/sw/lib/ocaml/gtkgl_Hack.ml @@ -0,0 +1,30 @@ +(* + * $Id$ + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +external load_bitmap_font : string -> GlList.base = + "_gtkgl_hack_load_bitmap_font" +external unload_bitmap_font : GlList.base -> unit = + "_gtkgl_hack_unload_bitmap_font" + +let gl_print_string font_base s = GlList.call_lists ~base:font_base(`byte s) diff --git a/sw/lib/ocaml/gtkgl_Hack.mli b/sw/lib/ocaml/gtkgl_Hack.mli new file mode 100644 index 0000000000..9bbd618f52 --- /dev/null +++ b/sw/lib/ocaml/gtkgl_Hack.mli @@ -0,0 +1,29 @@ +(* + * $Id$ + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +external load_bitmap_font : string -> GlList.base + = "_gtkgl_hack_load_bitmap_font" +external unload_bitmap_font : GlList.base -> unit + = "_gtkgl_hack_unload_bitmap_font" +val gl_print_string : GlList.base -> string -> unit diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml new file mode 100644 index 0000000000..571a6d7873 --- /dev/null +++ b/sw/lib/ocaml/latlong.ml @@ -0,0 +1,334 @@ +(* + * $Id$ + * + * Geographic conversion utilities + * + * Copyright (C) 2004 CENA/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. + * + *) + +module C = struct + include Complex + let make x y = {re = x; im = y} + let im c = c.im + let re c = c.re + let scal a {re = x; im = y} = {re = x*.a; im = y*.a} + let i {re = x; im = y} = {re = -.y; im = x} + let sin z = + let iz = i z in + i (scal (-. 0.5) (sub (exp iz) (exp (scal (-1.) iz)))) +end + +type degree = float +type radian = float +type semi = float +type dms = int * int * float + +type semicircle = { lat : semi; long : semi } +type geographic = { posn_lat : radian ; posn_long : radian } + +type angle_unit = Semi | Rad | Deg | Grd + +type cartesian = {x : float; y : float; z: float } + +let pi = 3.14159265358979323846;; + +let piradian = function + Semi -> 2. ** 31. | Rad -> pi | Deg -> 180. | Grd -> 200. +let (>>) u1 u2 x = (x *. piradian u2) /. piradian u1;; + +let sprint_degree_of_radian x = + Printf.sprintf "%.4f" ((Rad>>Deg) x) + +let string_degrees_of_geographic sm = + Printf.sprintf "%s\t%s" + (sprint_degree_of_radian sm.posn_lat) (sprint_degree_of_radian sm.posn_long) + +let of_semicircle x = + { posn_lat = (Semi>>Rad) x.lat ; posn_long = (Semi>>Rad) x.long } + +let semicircle_of x = + { lat = (Rad>>Semi) x.posn_lat ; long = (Rad>>Semi) x.posn_long } + +let decimal d m s = float d +. float m /. 60. +. s /. 3600.;; +let dms x = + let d = truncate x in + let m = truncate ((x -. float d) *. 60.) in + let s = 3600. *. (x -. float d -. float m /. 60.) in + (d, m, s);; + + + +type ellipsoid = { dx : float; dy : float; dz : float; a : float; df : float; e : float } +let ntf = { dx = -168.; dy = -60.; dz = 320. ; a = 6378249.2; df = 0.0034075495234250643; e = 0.08248325676} +let wgs84 = { dx = 0.; dy = 0.; dz = 0. ; a = 6378137.0; df = 0.0033528106647474805 ; e = 0.08181919106} +let ed50 = { dx = -87.0; dy = -98.0; dz = -121.0 ; a = 6378388.0; df = 0.003367003367003367 ; e = 0.08199188998} +let nad27 = { dx = 0.0; dy = 125.0; dz = 194.0 ; a = wgs84.a-. -.69.4; df = wgs84.df-. -0.37264639 /. 1e4 ; e = 0.08181919106(*** ??? ***)} + +type geodesic = NTF | ED50 | WGS84 | NAD27 +type ntf = geographic +let ellipsoid_of = function + NTF -> ntf | ED50 -> ed50 | WGS84 -> wgs84 | NAD27 -> nad27 + + +let latitude_isometrique phi e = + log (tan (pi/.4. +. phi /. 2.0)) -. e /. 2.0 *. log ((1.0 +. e *. sin phi) /. (1.0 -. e *. sin phi)) + +let inverse_latitude_isometrique lat e epsilon = + let exp_l = exp lat in + let pi_2 = pi /. 2. in + let phi0 = 2. *. atan exp_l -. pi_2 in + let rec loop phi = + let sin_phi = e *. sin phi in + let phi' = 2. *. atan (((1. +. sin_phi) /. (1. -. sin_phi))**(e/.2.) *. exp_l) -. pi_2 in + if abs_float (phi' -. phi) < epsilon then phi' else loop phi' in + loop phi0;; + +type lambert_zone = { + ellipsoid : ellipsoid; + phi0 : radian; + lphi0 : float; + r0 : float; + lambda0 : radian; + y0 : int; + x0 : int; + ys : int; + k0 : float (* facteur d'échelle *) + } + +type meter = int +type fmeter = float +type lambert = { lbt_x : meter; lbt_y : meter } +type utm = { utm_x : fmeter; utm_y : fmeter ; utm_zone : int } + +module Ellipse = struct + let e_square d = 2.0 *. d -. d ** 2.0;; + let e_prime_square d = 1.0 /. (1.0 -. d) ** 2.0 -. 1.0;; +end + +(* From http://www.tandt.be/wis/WiS/eqntf.html et http://www.ign.fr/MP/GEOD/geodesie/coordonnees.html *) +let lambertI = { + ellipsoid = ntf; + lambda0 = (Deg>>Rad) (decimal 2 20 14.025); + phi0 = (Deg>>Rad) (decimal 49 30 0.); + x0 = 600000; + y0 = 200000; + ys = 5657617; + lphi0 = 0.991996665; + r0 = 5457616.674; + k0 = 0.99987734 +};; + +let lambertII = { + ellipsoid = ntf; + lambda0 = (Deg>>Rad) (decimal 2 20 14.025); + phi0 = (Deg>>Rad) (decimal 46 48 0.); + x0 = 600000; + y0 = 2200000; + ys = 6199696; + lphi0 = 0.921557361; + r0 = 5999695.77; + k0 = 0.99987742};; + +let lambertIIe = { lambertII with ys = 8199696 };; + +let lambertIII = { + ellipsoid = ntf; + lambda0 = (Deg>>Rad) (decimal 2 20 14.025); + phi0 = (Deg>>Rad) (decimal 44 6 0.); + x0 = 600000; + y0 = 3200000; + ys = 6791905; + lphi0 = 0.854591098; + r0 = 6591905.08; + k0 = 0.99987750};; + +let lambertIV = { + ellipsoid = ntf; + lambda0 = (Deg>>Rad) (decimal 2 20 14.025); + phi0 = (Deg>>Rad) (decimal 42 09 54.); + x0 = 234; + y0 = 4185861; + ys = 7239162; + lphi0 = 0.808475773; + r0 = 7053300.18; + k0 = 0.99994471 +};; + +let lambert_n l = sin l.phi0 + + +let lambert_c l = + let n = lambert_n l in + l.r0 *. exp (l.lphi0 *. n) + +let lambert = function + 1 -> lambertI | 2 -> lambertII | 3 -> lambertIII | 4 -> lambertIV | _ -> failwith "lambert";; + + +let of_lambert l { lbt_x = x; lbt_y = y } = + let c = lambert_c l and n = lambert_n l in + let dx = float (x - l.x0) and dy = float (y - l.ys) in + let r = sqrt (dx**2. +. dy**2.) in + let gamma = atan2 dx (-. dy) in + let lambda = l.lambda0 +. gamma /. n + and ll = -. 1. /. n *. log (abs_float (r/.c)) in + let phi = inverse_latitude_isometrique ll l.ellipsoid.e 1e-11 in + {posn_long = lambda; posn_lat = phi};; + + +let lambert_of l {posn_long = lambda; posn_lat = phi} = + let n = lambert_n l in + let e = l.ellipsoid.e in + let ll = latitude_isometrique phi e in + let r = lambert_c l *. exp (-. ll *. n) in + let gamma = (lambda -. l.lambda0) *. n in + + let x = l.x0 + truncate (r *. sin gamma) + and y = l.ys - truncate (r *. cos gamma) in + { lbt_x = x; lbt_y = y };; + + +let serie5 cc e = + let ee = Array.init (Array.length cc.(0)) (fun i -> e ** (float (2*i))) in + Array.init (Array.length cc) + (fun i -> + let cci = cc.(i) in + let x = ref 0. in + for j = 0 to Array.length cci - 1 do + x := !x +. cci.(j) *. ee.(j) + done; + !x);; + +let coeff_proj_mercator = + [|[|1.; -. 1./.4.; -. 3./.64.; -.5./.256.; -.175./.16384.|]; + [|0.;1./.8.; -.1./.96.; -.9./.1024.; -.901./.184320.|]; + [|0.;0.;13./.768.;17./.5120.;-.311./.737280.|]; + [|0.;0.;0.; 61./.15360.;899./.430080.|]; + [|0.;0.;0.;0.;49561./.41287680.|]|];; + +let coeff_proj_mercator_inverse = + [|coeff_proj_mercator.(0); + [|0.;1./.8.; 1./.48.; 7./.2048.; 1./.61440.|]; + [|0.;0.;1./.768.;3./.1280.;559./.368640.|]; + [|0.;0.;0.; 17./.30720.;283./.430080.|]; + [|0.;0.;0.;0.;4397./.41287680.|]|];; + +let utm_of geo {posn_long = lambda; posn_lat = phi} = + let ellipsoid = ellipsoid_of geo in + let k0 = 0.9996 + and xs = 500000. + and ys = if phi > 0. then 0. else 10000000. in + let lambda_deg = truncate (floor ((Rad>>Deg)lambda)) in + let zone = (lambda_deg + 180) / 6 + 1 in + let lambda_c = (Deg>>Rad) (float (lambda_deg - lambda_deg mod 6 + 3)) in + let e = ellipsoid.e + and n = k0 *. ellipsoid.a in + let ll = latitude_isometrique phi e + and dl = lambda -. lambda_c in + let phi' = asin (sin dl /. cosh ll) in + let ll' = latitude_isometrique phi' 0. in + let lambda' = atan (sinh ll /. cos dl) in + let z = C.make lambda' ll' + and c = serie5 coeff_proj_mercator e in + let z' = ref (C.scal c.(0) z) in + for k = 1 to Array.length c - 1 do + z' := C.add !z' (C.scal c.(k) (C.sin (C.scal (float (2*k)) z))) + done; + z' := C.scal n !z'; + { utm_zone = zone; utm_x = xs +. C.im !z'; utm_y = ys +. C.re !z' };; + +let of_utm geo { utm_zone = f; utm_x = x; utm_y = y } = + let ellipsoid = ellipsoid_of geo in + let k0 = 0.9996 + and xs = 500000. + and ys = 0. in + let e = ellipsoid.e + and n = k0 *. ellipsoid.a in + let c = serie5 coeff_proj_mercator_inverse e in + + let lambda_c = (Deg>>Rad) (float (6 * f - 183)) in + let z' = C.scal (1./.n/.c.(0)) (C.make (y-.ys) (x-.xs)) in + let z = ref z' in + for k = 1 to Array.length c - 1 do + z := C.sub !z (C.scal c.(k) (C.sin (C.scal (float (2*k)) z'))) + done; + let ll = C.re !z and lls = C.im !z in + let lambda = lambda_c +. atan (sinh lls /. cos ll) + and phi' = asin (sin ll /. cosh lls) in + let ll = latitude_isometrique phi' 0. in + let phi = inverse_latitude_isometrique ll e 1e-11 in + {posn_long = lambda; posn_lat = phi};; + + +let (<<) geo1 geo2 {posn_long = lambda; posn_lat = phi} = + let elps1 = ellipsoid_of geo1 + and elps2 = ellipsoid_of geo2 in + let d12 = sin phi + and d13 = cos phi + and d14 = sin lambda + and d15 = cos lambda in + + let d16 = Ellipse.e_square elps2.df + and d17 = Ellipse.e_square elps1.df in + let d18 = elps2.a /. sqrt (1.0 -. d16 *. d12 ** 2.0) in + let d20 = d18 *. d13 *. d15 in + let d21 = d18 *. d13 *. d14 in + let d22 = d18 *. (1.0 -. d16) *. d12 in + let d23 = d20 -. elps1.dx +. elps2.dx in + let d24 = d21 -. elps1.dy +. elps2.dy in + let d25 = d22 -. elps1.dz +. elps2.dz in + let d26 = sqrt (d23 ** 2.0 +. d24 ** 2.0) in + let d27 = atan2 d25 (d26 *. (1.0 -. elps1.df)) in + let d28 = elps1.a *. (1.0 -. elps1.df) in + let d29 = Ellipse.e_prime_square elps1.df in + let d3 = atan2 (d25 +. d29 *. d28 *. (sin d27) ** 3.0) (d26 -. d17 *. elps1.a *. (cos d27) ** 3.0) in + let d4 = atan2 d24 d23 in + {posn_long = d4; posn_lat = d3};; + +let cartesian_of ellips {posn_long = lambda; posn_lat = phi} h = + let geo = ellipsoid_of ellips in + let w = sqrt (1. -. geo.e**2. *. sin phi ** 2.)in + let n = geo.a /. w in + let x = (n+.h) *. cos phi *. cos lambda + and y = (n+.h) *. cos phi *. sin lambda + and z = (n*.(1.-.geo.e**2.)+.h) *. sin phi in + { x = x; y = y; z = z} + +let of_cartesian ellips {x=x;y=y;z=z} = + let geo = ellipsoid_of ellips in + let epsilon = 1e-11 in + let xy = sqrt (x**2. +. y**2.) + and r = sqrt (x**2. +. y**2. +. z**2.) + and e2 = geo.e**2. in + let z_xy = z /. xy in + let lambda = 2. *. atan (y /. (x +. xy)) + and phi0 = atan (z_xy /. sqrt (1.-.geo.a*.e2/.r)) in + let rec iter phi = + let phi' = atan (z_xy /. (1.-.geo.a*.e2*.cos phi/.xy/.sqrt (1.-.e2*. sin phi ** 2.))) in + if abs_float (phi -. phi') > epsilon then iter phi' else phi' in + let phi = iter phi0 in + let h = xy/.cos phi -. geo.a /. sqrt (1.-.e2*.sin phi ** 2.) in + ({posn_long = lambda; posn_lat = phi}, h) + +let distance = fun {lbt_x=x1; lbt_y=y1} {lbt_x=x2; lbt_y=y2} -> + truncate (sqrt ((float x1 -. float x2)**2. +. (float y1 -. float y2)**2.)) + +let wgs84_of_lambertIIe = fun x y -> (WGS84<> ) : angle_unit -> angle_unit -> float -> float +(** [(Unit1>>Unit2) a] converts angle [a] expressed in [Unit1] in [Unit2] *) + +val decimal : int -> int -> float -> float +val dms : float -> dms +(** Conversions between decimal degrees and degree, minutes and seconds *) + + +(** {b Geodesic datum} *) + +type geodesic = NTF | ED50 | WGS84 | NAD27 +(** Geodesic referential *) + +type lambert_zone +val lambertI : lambert_zone +val lambertII : lambert_zone +val lambertIIe : lambert_zone +val lambertIII : lambert_zone +val lambertIV : lambert_zone +(** French lambert zones *) + + +(** {b Positions} *) + +type semicircle = { lat : semi; long : semi; } +type geographic = { posn_lat : radian; posn_long : radian; } +type cartesian = { x : float; y : float; z : float; } +type meter = int +type fmeter = float +type lambert = { lbt_x : meter; lbt_y : meter; } +type utm = { utm_x : fmeter; utm_y : fmeter; utm_zone : int; } +(** Position units. Coordinates are in meters in the [cartesian] type. *) + +val string_degrees_of_geographic : geographic -> string +(** Pretty printing *) + + +(** {b Conversions} *) + +val ( << ) : geodesic -> geodesic -> geographic -> geographic +(** [(Geo1< geographic +val semicircle_of : geographic -> semicircle + +type ntf = geographic +(** Type alias for documentation purpose *) + +val of_lambert : lambert_zone -> lambert -> ntf +val lambert_of : lambert_zone -> ntf -> lambert +(** Conversions between geographic (in NTF) and lambert *) + +val utm_of : geodesic -> geographic -> utm +val of_utm : geodesic -> utm -> geographic +(** Conversions between geographic and UTM *) + +val cartesian_of : geodesic -> geographic -> float -> cartesian +(** [cartesian_of geode geo alt] converts position [geo] at altitude [alt] +expressed in [geode] into cartesian coordinates *) + +val of_cartesian : geodesic -> cartesian -> geographic * float +(** [of_cartesian geode xyz] converts cartesian coordinates [xyz] into +geographic coordinates and altitude expressed in geodesic referential +[geode] *) + +val distance : lambert -> lambert -> meter + +val wgs84_of_lambertIIe : meter -> meter -> geographic diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml new file mode 100644 index 0000000000..acdc49f7cf --- /dev/null +++ b/sw/lib/ocaml/mapCanvas.ml @@ -0,0 +1,218 @@ +open Latlong +open Printf + +let pan_step = 50 + +type meter = float +type en = { east : meter; north : meter } + +let _ = Srtm.add_path "SRTM" + +(* world_unit: m:pixel at scale 1. *) +class widget = fun ?(height=800) ?width ?wgs84_of_en () -> + + let frame = GPack.vbox ~height ?width () in + + let menubar = GMenu.menu_bar ~packing:frame#pack () in + + let adj = GData.adjustment + ~value:1. ~lower:0.25 ~upper:10. + ~step_incr:0.25 ~page_incr:1.0 ~page_size:1.0 () in + + let canvas = GnoCanvas.canvas ~height ~packing:(frame#pack ~expand:true) () in + + let bottom = GPack.hbox ~height:30 ~packing:frame#pack () in + let w = GEdit.spin_button ~adjustment:adj ~rate:0. ~digits:2 ~width:50 ~height:20 ~packing:bottom#pack () in + + + let lbl_xy = GMisc.label ~height:50 ~packing:bottom#pack () in + let lbl_geo = GMisc.label ~height:50 ~packing:bottom#pack () in + let lbl_alt = GMisc.label ~height:50 ~packing:bottom#pack () in + let lbl_group = GMisc.label ~height:50 ~packing:bottom#pack () in + + let factory = new GMenu.factory menubar in + let file_menu = factory#add_submenu "Nav" in + let menu_fact = new GMenu.factory file_menu in + + let srtm = menu_fact#add_check_item "SRTM" ~active:false in + + object (self) + + initializer ignore (menu_fact#add_item "Goto" ~callback:self#goto) + initializer ignore (canvas#event#connect#motion_notify (self#mouse_motion)); + initializer ignore (canvas#event#connect#button_press (self#button_press)); + initializer ignore (canvas#event#connect#button_release self#button_release); + initializer ignore (canvas#event#connect#after#key_press self#key_press) ; + initializer ignore (canvas#event#connect#enter_notify (fun _ -> self#canvas#misc#grab_focus () ; false)); + initializer ignore (canvas#event#connect#any self#any_event); + + initializer ignore (adj#connect#value_changed (fun () -> self#zoom adj#value)); + initializer canvas#set_center_scroll_region false ; + initializer canvas#set_scroll_region (-2500000.) (-2500000.) 2500000. 2500000.; + + + val mutable current_zoom = 1. + val mutable dragging = None + val mutable grouping = None + val mutable rectangle = None + val mutable world_unit = 1. + val mutable wgs84_of_en = wgs84_of_en + + method set_wgs84_of_en = fun x -> wgs84_of_en <- Some x + + method set_world_unit = fun x -> world_unit <- x + + method current_zoom = current_zoom + + method canvas = canvas + method frame = frame + method menu_fact = menu_fact + method window_to_world = canvas#window_to_world + method root = canvas#root + method zoom_adj = adj + method factory = factory + + + method world_of_en = fun en -> en.east /. world_unit, -. en.north /. world_unit + method en_of_world = fun wx wy -> { east = wx *. world_unit; north = -. wy *. world_unit } + + method geo_string = fun en -> + match wgs84_of_en with + None -> "" + | Some f -> string_degrees_of_geographic (f en) + + method altitude = fun wgs84 -> + try + Srtm.of_wgs84 wgs84 + with + Srtm.Tile_not_found x -> + srtm#set_active false; + GToolbox.message_box "SRTM" (sprintf "SRTM tile %s not found: %s ?" x (Srtm.error x)); + 0 + + method moveto = fun en -> + let (xw, yw) = self#world_of_en en in + let (xc, yc) = canvas#world_to_window xw yw in + canvas#scroll_to (truncate xc) (truncate yc) + + method goto = fun () -> + let dialog = GWindow.window ~border_width:10 ~title:"Geo ref" () in + let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in + let lat = GEdit.entry ~packing:dvbx#add () in + let lon = GEdit.entry ~packing:dvbx#add () in + let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in + let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in + ignore(cancel#connect#clicked ~callback:dialog#destroy); + ignore(ok#connect#clicked ~callback: + begin fun _ -> + let x = float_of_string lat#text in + let y = float_of_string lon#text in + self#moveto {east=x; north=y}; + dialog#destroy () + end); + dialog#show () + + + method display_map = fun ?(scale = 1.) en image -> + let p = GnoCanvas.pixbuf ~pixbuf:image ~props:[`ANCHOR `NW] self#root in + p#lower_to_bottom (); + let wx, wy = self#world_of_en en in + p#move wx wy; + let a = p#i2w_affine in + a.(0) <- scale; a.(3) <- scale; + p#affine_absolute a; + p + + method zoom = fun value -> + canvas#set_pixels_per_unit value; + current_zoom <- value + + + method mouse_motion = fun ev -> + let xc = GdkEvent.Motion.x ev + and yc = GdkEvent.Motion.y ev in + let (xw, yw) = self#window_to_world xc yc in + let en = self#en_of_world xw yw in + lbl_xy#set_text (sprintf "%.0fm %.0fm\t" en.east en.north); + lbl_geo#set_text (self#geo_string en); + begin + match wgs84_of_en, srtm#active with + Some wgs84_of_en, true -> + lbl_alt#set_text (sprintf "\t%dm"(self#altitude (wgs84_of_en en))) + | _ -> () + end; + begin + match dragging with + Some (x0, y0 ) -> + let (x, y) = self#canvas#get_scroll_offsets in + self#canvas#scroll_to (x+truncate (x0-.xc)) (y+truncate (y0-.yc)) + | None -> () + end; + begin + match grouping with + Some (xw1, yw1) -> + let en1 = self#en_of_world xw1 yw1 in + lbl_group#set_text (sprintf "[%.1fkm %.1fkm]" ((en1.east -. en.east)/.1000.) ((en1.north-.en.north)/.1000.)) + | None -> () + end; + false + + method button_release = fun ev -> + let state = GdkEvent.Button.state ev in + match GdkEvent.Button.button ev, grouping with + 2, _ -> + dragging <- None; false + | 1, Some (xw1, yw1) -> + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + let (xw2, yw2) = self#window_to_world xc yc in + rectangle <- Some ((xw1, yw1), (xw2, yw2)); + lbl_group#set_text ""; + grouping <- None; + false + | _ -> false + + method button_press = fun ev -> + let state = GdkEvent.Button.state ev in + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + match GdkEvent.Button.button ev with + 1 -> + let xyw = self#window_to_world xc yc in + grouping <- Some xyw; + true + | 2 -> + dragging <- Some (xc, yc); + true + | _ -> false + + method key_press = fun ev -> + let (x, y) = canvas#get_scroll_offsets in + match GdkEvent.Key.keyval ev with + | k when k = GdkKeysyms._Up -> canvas#scroll_to x (y-pan_step) ; true + | k when k = GdkKeysyms._Down -> canvas#scroll_to x (y+pan_step) ; true + | k when k = GdkKeysyms._Left -> canvas#scroll_to (x-pan_step) y ; true + | k when k = GdkKeysyms._Right -> canvas#scroll_to (x+pan_step) y ; true + | _ -> false + + method any_event = fun ev -> + match GdkEvent.get_type ev with + | `SCROLL -> begin + match GdkEvent.Scroll.direction (GdkEvent.Scroll.cast ev) with + `UP -> + adj#set_value (adj#value+.adj#step_increment) ; + true + | `DOWN -> adj#set_value (adj#value-.adj#step_increment) ; true + | _ -> false + end + | _ -> false + + + method segment = fun ?(width=1) ?fill_color en1 en2 -> + let (x1, y1) = self#world_of_en en1 + and (x2, y2) = self#world_of_en en2 in + let l = GnoCanvas.line ?fill_color ~props:[`WIDTH_PIXELS width] ~points:[|x1;y1;x2;y2|] canvas#root in + l#show (); + l +end + diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml new file mode 100644 index 0000000000..e58f434459 --- /dev/null +++ b/sw/lib/ocaml/mapTrack.ml @@ -0,0 +1,77 @@ +open Printf + +module G = MapCanvas + +let affine_pos_and_angle z xw yw angle = + let rad_angle = angle /. 180. *. acos (-1.) in + let cos_a = cos rad_angle in + let sin_a = sin rad_angle in + [| cos_a /. z ; sin_a /. z ; ~-. sin_a /. z; cos_a /. z; xw ; yw |] + +class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas.widget) -> + let group = GnoCanvas.group geomap#canvas#root in + let empty = ({ G.east = 0.; north = 0. }, GnoCanvas.line group) in + + let aircraft = GnoCanvas.group group in + let ac_icon = + ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 4;`CAP_STYLE `ROUND] ~points:[|0.;-6.;0.;14.|] aircraft); + ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 4;`CAP_STYLE `ROUND] ~points:[|-9.;0.;9.;0.|] aircraft); + ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 4;`CAP_STYLE `ROUND] ~points:[|-4.;10.;4.;10.|] aircraft) in + let ac_label = + GnoCanvas.text group ~props:[`TEXT name; `X 25.; `Y 25.; `ANCHOR `SW; `FILL_COLOR color] in + + object (self) + val mutable segments = Array.create size empty + val mutable top = 0 + val mutable last = None + method clear_one = fun i -> + if segments.(i) != empty then begin + (snd segments.(i))#destroy (); + segments.(i) <- empty + end + method incr = + let s = Array.length segments in + top <- (top + 1) mod s + method clear = + for i = 0 to Array.length segments - 1 do + self#clear_one i + done; + top <- 0 + method add_point = fun en -> + self#clear_one top; + begin + match last with + None -> + segments.(top) <- (en, geomap#segment ~fill_color:color en en) + | Some last -> + segments.(top) <- (en, geomap#segment ~width:2 ~fill_color:color last en) + end; + self#incr; + last <- Some en + method move_icon = fun en heading -> + let (xw,yw) = geomap#world_of_en en in + aircraft#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw heading); + ac_label#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.); + method zoom = fun z -> + let a = aircraft#i2w_affine in + let z' = sqrt (a.(0)*.a.(0)+.a.(1)*.a.(1)) in + for i = 0 to 3 do a.(i) <- a.(i) /. z' *. 1./.z done; + aircraft#affine_absolute a + method resize = fun new_size -> + let a = Array.create new_size empty in + let size = Array.length segments in + let m = min new_size size in + let j = ref ((top - m + size) mod size) in + for i = 0 to m - 1 do + a.(i) <- segments.(!j); + j := (!j + 1) mod size + done; + for i = 1 to size - new_size do (* Never done if new_size > size *) + self#clear_one !j; + j := (!j + 1) mod size + done; + top <- m mod new_size; + segments <- a + method size = Array.length segments + initializer ignore(geomap#zoom_adj#connect#value_changed (fun () -> self#zoom geomap#zoom_adj#value)) +end diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml new file mode 100644 index 0000000000..071336ef53 --- /dev/null +++ b/sw/lib/ocaml/mapWaypoints.ml @@ -0,0 +1,122 @@ +open Printf + +let s = 5. +let losange = [|s;0.; 0.;s; -.s;0.; 0.;-.s|] + +class group = fun ?(color="red") ?(editable=true) (geomap:MapCanvas.widget) -> + let g = GnoCanvas.group geomap#canvas#root in + object + method group=g + method geomap=geomap + method color=color + method editable=editable +end + +class waypoint = fun (group:group) (name :string) ?(alt=0.) en -> + let geomap=group#geomap + and color = group#color + and editable = group#editable in + let xw, yw = geomap#world_of_en en in + object (self) + val mutable x0 = 0. + val mutable y0 = 0. + val item = + GnoCanvas.polygon group#group ~points:losange + ~props:[`FILL_COLOR color; `OUTLINE_COLOR "midnightblue" ; `WIDTH_UNITS 1.; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] + + val label = GnoCanvas.text group#group ~props:[`TEXT name; `X s; `Y 0.; `ANCHOR `SW] + val mutable name = name + val mutable alt = alt + initializer self#move xw yw + method name = name + method set_name n = + if n <> name then + name <- n + method alt = alt + method label = label + method xy = let a = item#i2w_affine in (a.(4), a.(5)) (*** item#i2w 0. 0. causes Seg Fault !***) + method move dx dy = item#move dx dy; label#move dx dy + method edit = + let dialog = GWindow.window ~border_width:10 ~title:"Waypoint Edit" () in + let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in + let en = self#en in + let ename = GEdit.entry ~text:name ~packing:dvbx#add () in + let ex = GEdit.entry ~text:(string_of_float en.MapCanvas.east) ~packing:dvbx#add () in + let ey = GEdit.entry ~text:(string_of_float en.MapCanvas.north) ~packing:dvbx#add () in + let ea = GEdit.entry ~text:(string_of_float alt) ~packing:dvbx#add () in + let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in + let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in + ignore(cancel#connect#clicked ~callback:dialog#destroy); + ignore(ok#connect#clicked ~callback: + begin fun _ -> + self#set_name ename#text; + alt <- float_of_string ea#text; + label#set [`TEXT name]; + self#set {MapCanvas.east = float_of_string ex#text; + north = float_of_string ey#text}; + dialog#destroy () + end); + dialog#show () + + + + method event (ev : GnoCanvas.item_event) = + begin + match ev with + | `BUTTON_PRESS ev -> + let state = GdkEvent.Button.state ev in + begin + match GdkEvent.Button.button ev with + | 1 -> self#edit + | 3 -> self#delete + | 2 -> + let x = GdkEvent.Button.x ev + and y = GdkEvent.Button.y ev in + x0 <- x; y0 <- y; + let curs = Gdk.Cursor.create `FLEUR in + item#grab [`POINTER_MOTION; `BUTTON_RELEASE] curs + (GdkEvent.Button.time ev) + | x -> printf "%d\n" x; flush stdout; + end + | `MOTION_NOTIFY ev -> + let state = GdkEvent.Motion.state ev in + if Gdk.Convert.test_modifier `BUTTON2 state then begin + let x = GdkEvent.Motion.x ev + and y = GdkEvent.Motion.y ev in + let dx = geomap#current_zoom *. (x-. x0) + and dy = geomap#current_zoom *. (y -. y0) in + self#move dx dy ; + x0 <- x; y0 <- y + end + | `BUTTON_RELEASE ev -> + if GdkEvent.Button.button ev = 2 then + item#ungrab (GdkEvent.Button.time ev) + | _ -> () + end; + true + initializer ignore(if editable then ignore (item#connect#event self#event)) + method item = item + method en = + let (dx, dy) = self#xy in + geomap#en_of_world dx dy + method set en = + let (xw, yw) = geomap#world_of_en en + and (xw0, yw0) = self#xy in + self#move (xw-.xw0) (yw-.yw0) + method delete = + item#destroy (); + label#destroy () + method zoom (z:float) = + let a = item#i2w_affine in + a.(0) <- 1./.z; a.(3) <- 1./.z; + item#affine_absolute a; + label#affine_absolute a + initializer self#zoom geomap#zoom_adj#value + initializer ignore(geomap#zoom_adj#connect#value_changed (fun () -> self#zoom geomap#zoom_adj#value)) + end + +let gensym = let n = ref 0 in fun prefix -> incr n; prefix ^ string_of_int !n + +let waypoint = fun group ?(name = gensym "wp") ?alt en -> + new waypoint group name ?alt en + diff --git a/sw/lib/ocaml/ml_gtkgl_hack.c b/sw/lib/ocaml/ml_gtkgl_hack.c new file mode 100644 index 0000000000..d097379166 --- /dev/null +++ b/sw/lib/ocaml/ml_gtkgl_hack.c @@ -0,0 +1,87 @@ +#include +#include +#include +#include +#include + +#include +#ifdef WIN32 +#include +#else +#include +#include +#include +#include +#endif + +#define MAX_FONTS 1000 +#ifndef WIN32 +static GLuint ListBase[MAX_FONTS]; +static GLuint ListCount[MAX_FONTS]; +#endif + +CAMLprim value _gtkgl_hack_load_bitmap_font (const char *font) { + static int FirstTime = 1; + int first, last, count; + int i ; + +#ifndef WIN32 + XFontStruct *fontinfo; + GLuint fontbase; + Display* dpy = GDK_DISPLAY(); + + if (FirstTime) { + for (i=0;imin_char_or_byte2; + last = fontinfo->max_char_or_byte2; + + /* Nombre de caracteres definis par cette fonte */ + count = last-first+1; + + /* Creation des listes OpenGL a partir de la fonte */ + fontbase = glGenLists( (GLuint) (last+1) ); + if (fontbase==0) return 0 ; + /* printf("Fontbase=%d first=%d last=%d\n", fontbase, first, last) ; fflush(stdout) ;*/ + + /* OpenGL doit l'utiliser */ + glXUseXFont(fontinfo->fid, first, count, (int) fontbase+first); + + + for (i=0;id2 then 1 else 0 + +(* ============================================================================= *) +(* = Compare deux chaines = *) +(* ============================================================================= *) +let cmp_string s1 s2 = if s1s2 then 1 else 0 + +(* ============================================================================= *) +(* = Decoupage d'une chaine de caracteres = *) +(* ============================================================================= *) +let split c s = + let i = ref (String.length s - 1) in + let j = ref !i and r = ref [] in + if !i >= 0 & String.get s 0 <> '#' (* skip lines starting with '#' *) + then + while !i >= 0 do + while !i >= 0 & String.get s !i <> c do decr i; done; + if !i < !j then r := (String.sub s (!i+1) (!j - !i)) :: !r; + while !i >= 0 & String.get s !i = c do decr i; done; + j := !i; + done; + !r + +(* ============================================================================= *) +(* = Decoupage d'une chaine de caracteres qui s'arrete apres une occurence du = *) +(* = caractere de decoupage contrairement a la fonction precedente = *) +(* ============================================================================= *) +let split2 c s = + let i = ref (String.length s - 1) in + let j = ref !i and r = ref [] in + if !i >= 0 & String.get s 0 <> '#' then (* skip lines starting with '#' *) + while !i >= 0 do + while !i >= 0 & String.get s !i <> c do decr i; done; + if !i <= !j then r := (String.sub s (!i+1) (!j - !i)) :: !r; + decr i ; + j := !i; + done; + !r + +(* ============================================================================= *) +(* = Decoupage d'une chaine de caracteres (separateurs multiples) = *) +(* ============================================================================= *) +let split_multiple lst_c s = + let match_char c = List.mem c lst_c in + + let i = ref (String.length s - 1) in + let j = ref !i and r = ref [] in + if !i >= 0 & String.get s 0 <> '#' then (* skip lines starting with '#' *) + while !i >= 0 do + while !i >= 0 & (not (match_char (String.get s !i))) do decr i; done; + if !i < !j then r := (String.sub s (!i+1) (!j - !i)) :: !r; + while !i >= 0 & (match_char (String.get s !i)) do decr i; done; + j := !i; + done; + !r + +(* ============================================================================= *) +(* = Decoupage d'une chaine de caracteres (separateurs multiples). = *) +(* = On s'arrete apres chaque occurence d'un separateur comme split2 = *) +(* ============================================================================= *) +let split_multiple2 lst_c s = + let match_char c = List.mem c lst_c in + + let i = ref (String.length s - 1) in + let j = ref !i and r = ref [] in + if !i >= 0 & String.get s 0 <> '#' then (* skip lines starting with '#' *) + while !i >= 0 do + while !i >= 0 & (not (match_char (String.get s !i))) do decr i; done; + if !i <= !j then r := (String.sub s (!i+1) (!j - !i)) :: !r; + decr i ; + j := !i; + done; + !r + +(* ============================================================================= *) +(* = Fonction d'ajout d'espaces a une chaine de caracteres = *) +(* = lg = longueur desiree = *) +(* ============================================================================= *) +let rec add_spaces lg str = + if (String.length str) >= lg then str else add_spaces lg (str ^ " ") + +(* ============================================================================= *) +(* = Fonction de suppression d'espaces en fin de chaine, pour gagner un peu en = *) +(* = memoire = *) +(* ============================================================================= *) +let delete_trailing_spaces s = + if s <> "" then begin + let i = ref ((String.length s)-1) in + while !i>=0 && (String.get s !i = ' ') do decr i done ; + if !i>=0 then String.sub s 0 (!i+1) else "" + end else s + +(* ============================================================================= *) +(* = Remplace le caractere c par c2 dans la chaine s = *) +(* ============================================================================= *) +let string_replace_char s c c2 = + let i = ref 0 in + String.iter (fun ch -> if ch=c then String.set s !i c2 ; incr i) s + +(* ============================================================================= *) +(* = Supprime les eventuels CTRL-M en fin de chaine (DOS->Unix) = *) +(* ============================================================================= *) +let string_dos2unix s = + if s<>"" && String.get s (String.length s -1) = ' +' then + String.sub s 0 (String.length s -1) + else s + +(* ============================================================================= *) +(* = Teste si la chaine matche la pattern = *) +(* ============================================================================= *) +let string_match pattern string = + let l = String.length pattern in + String.length string >= l && String.sub string 0 l = pattern +let string_exact_match pattern string = pattern = string + +(* Meme chose sans tenir compte de la case des caracteres *) +let string_match_no_case pattern string = + string_match (String.uppercase pattern) (String.uppercase string) + +let string_exact_match_no_case pattern string = + string_exact_match (String.uppercase pattern) (String.uppercase string) + +let string_match_in pattern string = + if pattern = "" then true else begin + let do_match = ref false and substr = ref string in + while not !do_match && String.length !substr>0 do + do_match := string_match pattern !substr ; + substr:=String.sub !substr 1 (String.length !substr -1) + done ; + !do_match + end + +let string_match_in_no_case pattern string = + string_match_in (String.uppercase pattern) (String.uppercase string) + +(* ============================================================================= *) +(* = Liste de chaines -> une chaine = *) +(* ============================================================================= *) +let string_of_string_list lst_string = + List.fold_left (fun str s -> if str <> "" then str^" "^s else s) "" lst_string + +(* ============================================================================= *) +(* = Creation d'une chaine en fonction de la valeur d'un entier = *) +(* = = *) +(* = XXX n -> aucun(e) XXX si n=0, un(e) XXX si n=1, XXXs sinon = *) +(* = female indique si on a un nom feminin et first_char_upper indique si le = *) +(* = premier caractere doit etre en majuscule ou pas = *) +(* ============================================================================= *) +let eval_string base_string n female first_char_upper = + let get_upper_and_female str = + let str = if female then str^"e" else str in + if first_char_upper then String.capitalize str + else str + in + + match n with + 0 -> (get_upper_and_female "aucun") ^ " " ^ base_string + | 1 -> (get_upper_and_female "un") ^ " " ^ base_string + | _ -> + if String.get base_string (String.length base_string -1) = 'x' then + Printf.sprintf "%d %s" n base_string + else Printf.sprintf "%d %ss" n base_string + +(* ============================================================================= *) +(* = Fonction de parsing d'une chaine = *) +(* ============================================================================= *) +let do_parse_string s no_ligne_parsing parser_main lexer_token end_func = + let lexbuf = Lexing.from_string s in + let fin = ref false in + no_ligne_parsing := 1 ; + while not !fin do + try parser_main lexer_token lexbuf ; + with Parsing.Parse_error -> (* Erreur de syntaxe *) + raise Parsing.Parse_error + | Failure("lexing: empty token") -> (* Est-ce la fin de la chaine ? *) + fin := lexbuf.Lexing.lex_eof_reached ; + | x -> raise x + done ; + (* Appel a la fonction de fin de lecture *) + end_func () + +(* ============================================================================= *) +(* = Supprime les doublons dans une liste triee = *) +(* ============================================================================= *) +let rec supprime_dbl_list = function + [] -> [] + | x::xs -> + match xs with + [] -> [x] + | x'::xs' -> + if x = x' then supprime_dbl_list xs' else x::supprime_dbl_list xs' + +(* ============================================================================= *) +(* = Supprime l'element d'index idx dans la liste lst = *) +(* ============================================================================= *) +let del_elt_lst lst idx = + if (idx>=0) && (idx 1 then begin + let a = Array.of_list lst in + let a1 = Array.sub a 0 idx and + a2 = Array.sub a (idx+1) ((List.length lst)-1-idx) in + Array.to_list (Array.append a1 a2) + end else [] + end else lst + +(* ============================================================================= *) +(* = Ouverture d'un fichier compresse avec gzip, bzip2, zip ou non compresse = *) +(* ============================================================================= *) +let open_compress file = + if Filename.check_suffix file "gz" or Filename.check_suffix file "Z" or + Filename.check_suffix file "zip" or Filename.check_suffix file "ZIP" then + Unix.open_process_in ("gunzip -c "^file) + else if Filename.check_suffix file "bz2" then + Unix.open_process_in ("bunzip2 -c "^file) + else Pervasives.open_in file + + +let extensions = ["";".gz";".Z";".bz2";".zip";".ZIP"] +let find_file = fun path file -> + let rec loop_path = function + [] -> raise Not_found + | p::ps -> + let rec loop_ext = function + [] -> loop_path ps + | ext::es -> + let f = Filename.concat p file ^ ext in + if Sys.file_exists f then f else loop_ext es in + loop_ext extensions in + loop_path path + +(* ============================================================================= *) +(* = Fermeture d'un fichier = *) +(* ============================================================================= *) +let close_compress file inchan = + if (Filename.check_suffix file "gz") or (Filename.check_suffix file "bz2") or + (Filename.check_suffix file "Z") or (Filename.check_suffix file "zip") or + (Filename.check_suffix file) "ZIP"then + ignore(Unix.close_process_in inchan) + else close_in inchan + +(* ============================================================================= *) +(* = Gestion des fichiers gzippes = *) +(* ============================================================================= *) +let open_gzip file = + if Filename.check_suffix file "gz" or Filename.check_suffix file "Z" then + Unix.open_process_in ("gunzip -c "^file) + else Pervasives.open_in file +let close_gzip file inchan = + if Filename.check_suffix file "gz" or Filename.check_suffix file "Z" then + ignore(Unix.close_process_in inchan) + else close_in inchan +(* ============================================================================= *) + +(* ============================================================================= *) +(* = Gestion des fichiers bzippes = *) +(* ============================================================================= *) +let open_bzip file = + if Filename.check_suffix file "bz2" then + Unix.open_process_in ("bunzip2 -c "^file) + else Pervasives.open_in file +let close_bzip file inchan = + if Filename.check_suffix file "bz2" then ignore(Unix.close_process_in inchan) + else close_in inchan +(* ============================================================================= *) + +(* ============================================================================= *) +(* = Gestion des fichiers zippes = *) +(* ============================================================================= *) +let open_zip file = + if Filename.check_suffix file "zip" or Filename.check_suffix file "ZIP" then + Unix.open_process_in ("gunzip -c "^file) + else Pervasives.open_in file +let close_zip file inchan = + if Filename.check_suffix file "zip" or Filename.check_suffix file "ZIP" then + ignore(Unix.close_process_in inchan) + else close_in inchan +(* ============================================================================= *) + +(* ============================================================================= *) +(* = Compression d'un fichier suivant son extension = *) +(* ============================================================================= *) +let do_compress_file filename ext = + ignore(match ext with + "gz" | ".gz" -> Unix.system ("gzip "^filename) + | "bz2" | ".bz2" -> Unix.system ("bzip2 "^filename) + | _ -> Unix.WEXITED(0)) + +(* ============================================================================= *) +(* = Lecture d'un fichier et copie dans une chaine de caracteres = *) +(* ============================================================================= *) +let string_of_file filename = + let c = open_compress filename and texte = ref "" in + (try + while true do + if !texte = "" then texte := input_line c + else texte := !texte ^ "\n" ^ (input_line c) + done + with End_of_file -> close_compress filename c) ; + !texte + +(* ============================================================================= *) +(* = Fonction de lecture d'un fichier avec precision du separateur = *) +(* ============================================================================= *) +let do_read_file_with_separators filename match_func end_func separators = + let no_line = ref 0 in + let error_func s = + Printf.printf("Erreur ligne %d (%s)\n") !no_line s ; flush stdout in + + try + let c = open_compress filename in + let error = ref false in + (try + while not !error do + let s = input_line c in + no_line := !no_line + 1 ; + (* Passe les commentaires et decoupe la ligne *) + let splitted = split_multiple separators s in + (* On saute les lignes vides *) + if splitted <> [] then + (try match_func splitted (fun () -> error_func s) + with | _ -> + (* S'il y a une erreur ici, c'est qu'une ligne ne contient *) + (* Pas ce qui est attendu : ex pb de int_of_string *) + close_compress filename c ; + error_func s ;error := true) + done + with End_of_file -> close_compress filename c ; end_func ()) + with _ -> Printf.printf("Erreur d'ouverture du fichier %s\n") filename; + flush stdout + +(* ============================================================================= *) +(* = Fonction de lecture d'un fichier avec precision du separateur = *) +(* ============================================================================= *) +let do_read_file_with_separator filename match_func end_func separator = + do_read_file_with_separators filename match_func end_func [separator] + +(* ============================================================================= *) +(* = Fonction de lecture d'un fichier (separateur par defaut = espace) = *) +(* ============================================================================= *) +let do_read_file filename match_func end_func = + do_read_file_with_separators filename match_func end_func [' '] + +(* ============================================================================= *) +(* = Fonction de lecture d'un fichier avec precision du separateur = *) +(* ============================================================================= *) +let do_read_file_with_separators2 filename match_func end_func separators = + let no_line = ref 0 in + let error_func s = + Printf.printf("Erreur ligne %d (%s)\n") !no_line s ; flush stdout in + + try + let c = open_compress filename in + let error = ref false in + (try + while not !error do + let s = input_line c in + no_line := !no_line + 1 ; + (* Passe les commentaires et decoupe la ligne *) + let splitted = split_multiple2 separators s in + (* On saute les lignes vides *) + if splitted <> [] then + (try match_func splitted (fun () -> error_func s) + with | _ -> + (* S'il y a une erreur ici, c'est qu'une ligne ne contient *) + (* Pas ce qui est attendu : ex pb de int_of_string *) + close_compress filename c ; + error_func s ;error := true) + done + with End_of_file -> close_compress filename c ; end_func ()) + with _ -> Printf.printf("Erreur d'ouverture du fichier %s\n") filename; + flush stdout + +(* ============================================================================= *) +(* = Fonction de lecture d'un fichier avec precision du separateur = *) +(* ============================================================================= *) +let do_read_file_with_separator2 filename match_func end_func separator = + do_read_file_with_separators2 filename match_func end_func [separator] + +(* ============================================================================= *) +(* = Fonction de lecture d'un fichier (separateur par defaut = espace) = *) +(* ============================================================================= *) +let do_read_file2 filename match_func end_func = + do_read_file_with_separators2 filename match_func end_func [' '] + +(* ============================================================================= *) +(* = Fonction de parsing d'un fichier = *) +(* ============================================================================= *) +let do_parse_file filename no_ligne_parsing parser_main lexer_token end_func = + let c = + (try Some (open_compress filename) + with _ -> Printf.printf "Erreur d'ouverture du fichier %s\n" filename; + flush stdout; None) in + + match c with + None -> () + | Some c -> + let lexbuf = Lexing.from_channel c in + let fin = ref false in + no_ligne_parsing := 1 ; + while not !fin do + try parser_main lexer_token lexbuf ; + with Parsing.Parse_error -> (* Erreur de syntaxe *) + Printf.printf "Erreur ligne %d : *%s*\n" + !no_ligne_parsing (Lexing.lexeme lexbuf); flush stdout + | Failure("lexing: empty token") -> (* Est-ce la fin du fichier ? *) + fin := lexbuf.Lexing.lex_eof_reached ; + | x -> raise x + done ; + close_compress filename c ; + (* Appel a la fonction de fin de lecture *) + end_func () + +(* ============================================================================= *) +(* = Parsing d'un fichier de configuration = *) +(* ============================================================================= *) +let parse_config_file config_file spec_list anofun usage_msg = + let c = open_compress config_file in + (try + while true do + let s = input_line c in + let l = (split ' ' s) in + if l<>[] then begin + Arg.current := 0; + Arg.parse_argv (Array.of_list ("CMDE"::l)) spec_list anofun usage_msg; + end; + done; + with End_of_file -> ()); + Arg.current := 0; close_compress config_file c + +(* ============================================================================= *) +(* = Indique si le nom indique correspond a un repertoire = *) +(* ============================================================================= *) +let is_directory filename = + let stats = Unix.stat filename in stats.Unix.st_kind = Unix.S_DIR + +(* ============================================================================= *) +(* = Renvoie la liste des fichiers et repertoires contenus dans un repertoire = *) +(* ============================================================================= *) +let get_files_from_dir dirname = + let lst = ref [] in + try + let d = Unix.opendir dirname in + try + while true do let filename = Unix.readdir d in lst := filename::!lst done ; + [] + with End_of_file -> (* Lecture terminee *) + List.fast_sort cmp_string !lst + with _ -> [] + +(* ============================================================================= *) +(* = Renvoie la liste des repertoires = *) +(* ============================================================================= *) +let get_dirs_only_from_dir dirname = + let l = get_files_from_dir dirname in + List.fold_right (fun file l -> + if (file<>"."&&file<>".."&&(is_directory (dirname^file))) + then file::l else l) l [] + +(* ============================================================================= *) +(* = Renvoie la liste des fichiers = *) +(* ============================================================================= *) +let get_files_only_from_dir dirname = + let l = get_files_from_dir dirname in + List.fold_right (fun file l -> + if not (is_directory (dirname^file)) then file::l else l) l [] + +(* ============================================================================= *) +(* = Supprime l'eventuel chemin dans un nom de fichier = *) +(* ============================================================================= *) +let del_path_in_filename filename = Filename.basename filename + + +(* ***************************************************************************** *) +(* ***************************************************************************** *) +(* Manipulations de dates *) +(* ***************************************************************************** *) +(* ***************************************************************************** *) + +(* ============================================================================= *) +(* = Date sous la forme 20020114 -> 14 01 2002 = *) +(* ============================================================================= *) +let decompose_date date = (date mod 100, (date mod 10000)/100, date/10000) + +(* ============================================================================= *) +(* = Date sous la forme 14 01 2002 -> 20020114 = *) +(* ============================================================================= *) +let compose_date (jj, mm, aa) = aa*10000+mm*100+jj + +(* ============================================================================= *) +(* = Renvoie le nom du mois en fonction de son numero = *) +(* ============================================================================= *) +let get_month_of_num num = + match num with + 1 -> "Janvier" | 2 -> "Fevrier" | 3 -> "Mars" | 4 -> "Avril" + | 5 -> "Mai" | 6 -> "Juin" | 7 -> "Juillet" | 8 -> "Aout" + | 9 -> "Septembre" | 10 -> "Octobre" | 11 -> "Novembre" | 12 -> "Decembre" + | _ -> "???" + +(* ============================================================================= *) +(* = Renvoie le numero du jour de la semaine en fonction d'une date = *) +(* ============================================================================= *) +let get_day_of_date (jj, mm, aa) = + let jour_sem = [|"Dimanche"; "Lundi"; "Mardi"; "Mercredi"; "Jeudi"; + "Vendredi"; "Samedi"|] in + + let (aa, mm) = if mm < 3 then (aa-1, mm+10) else (aa, mm-2) in + let siecle = aa/100 and an = aa mod 100 in + let js = (((26 * mm - 2) / 10) + jj + an + (an / 4) + + (siecle / 4) - (2 * siecle)) mod 7 in + if js<0 then jour_sem.(js+7) else jour_sem.(js) + +let get_day_of_date2 date = get_day_of_date (decompose_date date) + +(* ============================================================================= *) +(* = L'annee indiquee est-elle bissextile ? = *) +(* = Elle l'est si elle est divisible par 4, sauf si c'est un siecle. Cependant= *) +(* = tous les 4 siecles, elle est bissextile... = *) +(* ============================================================================= *) +let is_year_bis aa = (aa mod 400 = 0) || ((aa mod 4 = 0) && (aa mod 100 <> 0)) + +(* ============================================================================= *) +(* = Renvoie le nombre de jours d'un mois en fonction du mois et de l'annee = *) +(* ============================================================================= *) +let get_nb_days_in_month mm aa = + if mm = 2 then (* Annee bissextile ? *) if is_year_bis aa then 29 else 28 + else match mm with + 1 -> 31 | 2 -> 28 | 3 -> 31 | 4 -> 30 | 5 -> 31 | 6 -> 30 + | 7 -> 31 | 8 -> 31 | 9 -> 30 | 10 -> 31 | 11 -> 30 | 12 -> 31 + | _ -> (-1) + +(* ============================================================================= *) +(* = Renvoie la date augmentee de delta jours (delta peut etre negatif) = *) +(* ============================================================================= *) +let get_delta_date (jj, mm, aa) delta = + let d = ref delta and j = ref jj and m = ref mm and a = ref aa in + let adding = delta>=0 in + while !d <> 0 do + let lg_month = get_nb_days_in_month !m !a in + if !d+ !j<=lg_month && !d+ !j>0 then begin j:=!j+ !d; d:=0 end else begin + if adding then begin + if !m<12 then incr m else begin m:=1; incr a end ; + d:=!d-(lg_month- !j)-1 ; + j:=1 + end else begin + if !m>1 then decr m else begin m:=12; decr a end ; + d:=!d+ !j ; + j:=get_nb_days_in_month !m !a + end ; + end + done ; + (!j, !m, !a) + +let get_delta_date2 date delta = + compose_date (get_delta_date (decompose_date date) delta) + +(* ============================================================================= *) +(* = Renvoie la date precedente/suivante = *) +(* ============================================================================= *) +let get_next_date (jj, mm, aa) = get_delta_date (jj, mm, aa) 1 +let get_next_date2 date = get_delta_date2 date 1 +let get_prev_date (jj, mm, aa) = get_delta_date (jj, mm, aa) (-1) +let get_prev_date2 date = get_delta_date2 date (-1) + +(* ============================================================================= *) +(* = Fonction renvoyant le nombre de jours entre deux dates d2-d1 = *) +(* ============================================================================= *) +let get_diff_date (jj1, mm1, aa1) (jj2, mm2, aa2) = + (* On se debarrasse du cas trivial *) + if mm1=mm2 && aa1=aa2 then jj2-jj1 else begin + (* Sens dans lequel on se deplace *) + let delta = + if compose_date (jj1, mm1, aa1)(jj2, mm2, aa2) do + nb_days:= !nb_days+delta ; + current_date:=get_delta_date !current_date delta + done ; + !nb_days + end + +let get_diff_date2 d1 d2 = get_diff_date (decompose_date d1) (decompose_date d2) + +(* ============================================================================= *) +(* = Date -> Chaine JJ/MM/AAAA = *) +(* ============================================================================= *) +let string_of_date (jj, mm, aa) = Printf.sprintf "%02d/%02d/%d" jj mm aa +let string_of_date2 date = string_of_date (decompose_date date) + +(* ============================================================================= *) +(* = Temps en secondes -> Chaine = *) +(* ============================================================================= *) +let string_of_time s = + Printf.sprintf "%02d:%02d:%02d" (s/3600) (s/60 mod 60) (s mod 60) + +(* ============================================================================= *) +(* = Temps en secondes -> Chaine sans les secondes = *) +(* ============================================================================= *) +let string_of_time_without_seconds s = + Printf.sprintf "%02d:%02d" (s/3600) (s/60 mod 60) + +(* ============================================================================= *) +(* = Chaine -> Temps en secondes = *) +(* ============================================================================= *) +let time_of_string t = + match split ':' t with + [h;m;s]->(int_of_string h*60 + int_of_string m)*60 + int_of_string s |_->0 + +(* ============================================================================= *) +(* = Renvoie l'heure = *) +(* ============================================================================= *) +let timer_get_time () = Unix.localtime (Unix.time ()) + +(* ============================================================================= *) +(* = Formate l'heure dans une chaine de caracteres = *) +(* ============================================================================= *) +let timer_string_of_time tm = + Printf.sprintf "%02d:%02d:%02d" tm.Unix.tm_hour tm.Unix.tm_min tm.Unix.tm_sec + +(* ============================================================================= *) +(* = Formate la date dans une chaine de caracteres = *) +(* ============================================================================= *) +let timer_string_of_date tm = + Printf.sprintf "%02d/%02d/%04d" tm.Unix.tm_mday (tm.Unix.tm_mon+1) + (tm.Unix.tm_year+1900) + +(* ============================================================================= *) +(* = Renvoie le temps ecoule entre deux heures, en secondes = *) +(* ============================================================================= *) +let timer_sub tm1 tm2 = + let (h1, m1, s1, d1) = + (tm1.Unix.tm_hour,tm1.Unix.tm_min,tm1.Unix.tm_sec, tm1.Unix.tm_mday) + and (h2, m2, s2, d2) = + (tm2.Unix.tm_hour,tm2.Unix.tm_min,tm2.Unix.tm_sec, tm2.Unix.tm_mday) in + (d2-d1)*24*3600+(h2-h1)*3600+(m2-m1)*60+s2-s1 + +(* ============================================================================= *) +(* = Chaine en Heures, Minutes, Secondes indiquant le temps ecoule = *) +(* ============================================================================= *) +let timer_string_of_secondes sec = + let h = sec / 3600 in + let reste = sec-h*3600 in + let m = reste/60 and s = reste mod 60 in + Printf.sprintf "%d:%02d:%02d" h m s + +(* ============================================================================= *) +(* = Tirage aleatoire d'un element dans une liste = *) +(* ============================================================================= *) +let tirage_aleatoire_lst (lst:'a list) = List.nth lst (Random.int (List.length lst)) + +(* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/ocaml_tools.mli b/sw/lib/ocaml/ocaml_tools.mli new file mode 100644 index 0000000000..daf598a080 --- /dev/null +++ b/sw/lib/ocaml/ocaml_tools.mli @@ -0,0 +1,393 @@ +(* + * $Id$ + * + * Utilities + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(** Version de la librairie Ocaml_tools sous la forme d'une chaine *) +val ocaml_tools_version : string + + +(** {6 Fonctions de comparaison} *) + + +(** [cmp_string entier1 entier2] compare les 2 entiers (pour un List.sort) *) +val cmp_int : int -> int -> int + +(** [cmp_float float1 float2] compare les 2 flottants (pour un List.sort) *) +val cmp_float : 'a -> 'a -> int + +(** [cmp_string chaine1 chaine2] compare les 2 chaines (pour un List.sort) *) +val cmp_string : 'a -> 'a -> int + + +(** {6 Manipulation de chaines de caractères} *) + + +(** [split caractere chaine] découpe [chaine] suivant [caractere] et renvoie la + liste des mots decoupés. Toutes les lignes commencant par '#' sont supprimées *) +val split : char -> string -> string list + +(** [split2 caractere chaine] découpe [chaine] suivant [caractere] et renvoie la + liste des mots decoupés. Toutes les lignes commencant par '#' sont supprimées. + Ici, on découpe aprés chaque occurence de [caractere], il peut donc y avoir + des mots vides renvoyés *) +val split2 : char -> string -> string list + +(** [split_multiple lst_caracteres chaine] découpe [chaine] suivant les + caractères précisés comme la fonction [split] sauf qu'ici on peut préciser + plusieurs caractères de découpage *) +val split_multiple : char list -> string -> string list + +(** [split_multiple2 lst_caracteres chaine] découpe [chaine] suivant les + caractères précisés comme la fonction [split] sauf qu'ici on peut préciser + plusieurs caractères de découpage. On découpe après chaque occurence d'un des + caractères indiqués comme avec {!Ocaml_tools.split2} *) +val split_multiple2 : char list -> string -> string list + +(** [add_spaces longueur chaine] ajoute des espaces à [chaine] pour qu'elle fasse + [longueur] caractères *) +val add_spaces : int -> string -> string + +(** [delete_trailing_spaces chaine] supprime les espaces éventuels à la fin de + [chaine] *) +val delete_trailing_spaces : string -> string + +(** [string_replace_char chaine caractere1 caractere2] remplace toutes les + occurences de [caractere1] par [caractere2] dans [chaine] *) +val string_replace_char : string -> char -> char -> unit + +(** [string_dos2unix chaine] supprime l'eventuel caractère CTRL-M en fin + de chaine (transformation d'un fichier DOS vers Unix). *) +val string_dos2unix : string -> string + +(** [string_match pattern chaine] indique si le début de [chaine] est identique + à [pattern] *) +val string_match : string -> string -> bool + +(** [string_exact_match pattern chaine] indique si [chaine] est identique + à [pattern] (i.e chaine=pattern) *) +val string_exact_match : 'a -> 'a -> bool + +(** [string_match_no_case pattern chaine] identique à {!Ocaml_tools.string_match} + excepté que le test ne tient pas compte de la case des caractères *) +val string_match_no_case : string -> string -> bool + +(** [string_exact_match_no_case pattern chaine] identique à + {!Ocaml_tools.string_exact_match} + except& que le test ne tient pas compte de la case des caractères *) +val string_exact_match_no_case : string -> string -> bool + +(** [string_match_in pattern chaine] teste si la chaine contient [pattern] *) +val string_match_in : string -> string -> bool + +(** [string_match_in_no_case pattern chaine] teste si la chaine contient + [pattern]. Le test ne tient pas compte de la case des caractères *) +val string_match_in_no_case : string -> string -> bool + +(** [string_of_string_list liste_de_chaines] construit une chaine à partir + d'une liste de chaines de caractères *) +val string_of_string_list : string list -> string + +(** [eval_string chaine_base n feminin premier_char_capital] construit une chaine + de caractères à partir d'un nombre : + - [chaine_base] indique le nom + - [n] indique le nombre + - [feminin] indique si le nom est féminin + - [premier_char_capital] indique si le premier caractère de la chaine doit etre + en majuscule + + ex : + - [eval_string "secteur" 0 false true] donne "Aucun secteur" + - [eval_string "secteur" 1 false true] donne "Un secteur" + - [eval_string "secteur" 2 false true] donne "2 secteurs" + *) +val eval_string : string -> int -> bool -> bool -> string + +(** [do_parse_string chaine numero_ligne parser_main lexer_token end_func] parse + une chaine de caractères au lieu d'un fichier*) +val do_parse_string : + string -> + int ref -> ('a -> Lexing.lexbuf -> unit) -> 'a -> (unit -> 'b) -> 'b + +(** [parse_config_file fichier spec_list anofun usage_msg] parse un fichier de configuration + comme si c'était des options passées sur la ligne de commandes *) +val parse_config_file : string -> + (string * Arg.spec * string) list -> (string -> unit) -> string -> unit + +(** {6 Listes} *) + + +(** [supprime_dbl_list liste] supprime les doublons dans une liste d'entiers + triée *) +val supprime_dbl_list : 'a list -> 'a list + +(** [del_elt_lst liste index] supprime l'élément d'index [index] dans la liste *) +val del_elt_lst : 'a list -> int -> 'a list + + +(** {6 Ouverture/fermeture de fichiers compressés} *) + + +(** [open_compress fichier] ouvre [fichier] qui peut etre non compressé, + compressé avec gzip (se terminant par .gz ou .Z), avec bzip2 (se terminant par .bz2) + ou avec zip (extension en .zip ou .ZIP). Dans ce dernier cas, seuls les fichiers + zippés contenant UN seul fichier sont pris en compte *) +val open_compress : string -> in_channel + +(** [close_compress fichier channel] ferme un fichier ouvert + avec {!Ocaml_tools.open_compress} *) +val close_compress : string -> in_channel -> unit + +(** [find_file path file] Search for [file] or a compressed extension of it in +[path]. Returns the first occurence found. Checked extensions are .gz, .Z, .bz2, .zip +and , ZIP *) +val find_file : string list -> string -> string + +(** [open_gzip fichier] ouvre un fichier non compressé ou compressé avec gzip + (.gz ou .Z) *) +val open_gzip : string -> in_channel + +(** [close_gzip fichier channel] ferme un fichier non compressé ou + compressé avec gzip (.gz ou .Z) *) +val close_gzip : string -> in_channel -> unit + +(** [open_bzip fichier] ouvre un fichier non compressé ou compressé avec bzip2 + (.bz2) *) +val open_bzip : string -> in_channel + +(** [close_bzip fichier channel] ferme un fichier non compressé ou + compressé avec bzip2 (.bz2) *) +val close_bzip : string -> in_channel -> unit + +(** [open_zip fichier] ouvre un fichier non compressé ou compressé avec zip + (.zip ou .ZIP) *) +val open_zip : string -> in_channel + +(** [close_zip fichier channel] ferme un fichier non compressé ou + compressé avec zip *) +val close_zip : string -> in_channel -> unit + +(** [do_compress_file fichier extension] effectue la compression du fichier avec + gzip si [extension] vaut ".gz" ou bzip2 si [extension] vaut ".bz2" *) +val do_compress_file : string -> string -> unit + + +(** {6 Lecture/parsing de fichiers} *) + + +(** [string_of_file fichier] lit le fichier (éventuellement compressé) et renvoie + une chaine de caractères correspondant à son contenu *) +val string_of_file : string -> string + +(** [do_read_file_with_separators fichier match_func end_func separateurs] lit + un fichier et decoupe chacune de ses lignes suivant les caractères [separateurs]. + Les lignes ainsi decoupées sont passées à la fonction utilisateur [match_func] + qui les traite. Cette fonction reçoit comme arguments la liste des mots de + la ligne decoupée ainsi qu'une fonction d'erreur à appeler si la ligne + n'est pas au format voulu. + En fin de lecture, la fonction [end_func] est appelée *) +val do_read_file_with_separators : + string -> + (string list -> (unit -> unit) -> unit) -> (unit -> unit) -> char list -> unit + +(** [do_read_file_with_separator fichier match_func end_func separateur] lit + un fichier et decoupe chacune de ses lignes suivant le caractère [separateur]. + Les lignes ainsi decoupées sont passées à la fonction utilisateur [match_func] + qui les traite. Cette fonction reçoit comme arguments la liste des mots de + la ligne decoupée ainsi qu'une fonction d'erreur à appeler si la ligne + n'est pas au format voulu. + En fin de lecture, la fonction [end_func] est appelée *) +val do_read_file_with_separator : + string -> + (string list -> (unit -> unit) -> unit) -> (unit -> unit) -> char -> unit + +(** [do_read_file fichier match_func end_func] : meme chose que + {!Ocaml_tools.do_read_file_with_separator} sauf que le caractère séparateur + est implicitement l'espace *) +val do_read_file : + string -> (string list -> (unit -> unit) -> unit) -> (unit -> unit) -> unit + +(** [do_read_file_with_separators2 fichier match_func end_func separateurs] : + identique à {!Ocaml_tools.do_read_file_with_separators} sauf qu'on découpe + après chaque occurence des caractères de séparation *) +val do_read_file_with_separators2 : + string -> + (string list -> (unit -> unit) -> unit) -> (unit -> unit) -> char list -> unit + +(** [do_read_file_with_separator2 fichier match_func end_func separateur] : + identique à {!Ocaml_tools.do_read_file_with_separator} sauf qu'on découpe + après chaque occurence du caractère de séparation *) +val do_read_file_with_separator2 : + string -> + (string list -> (unit -> unit) -> unit) -> (unit -> unit) -> char -> unit + +(** [do_read_file2 fichier match_func end_func] : meme chose que + {!Ocaml_tools.do_read_file} sauf qu'on découpe + après chaque occurence du caractère de séparation *) +val do_read_file2 : + string -> (string list -> (unit -> unit) -> unit) -> (unit -> unit) -> unit + +(** [do_parse_file fichier numero_ligne parser_main lexer_token end_func] lit + un fichier qui est analysé par un parser. Les arguments utilisés sont : + - [fichier] : le nom du fichier à traiter + - [numero_ligne] : référence sur un entier contenant le numéro de la ligne + en cours de lecture + - [parser_main] : la fonction [main] du parser + - [lexer_token] : la fonction [token] du lexer + - [end_func] : fonction utilisateur appelée en fin de lecture du fichier + *) +val do_parse_file : + string -> + int ref -> ('a -> Lexing.lexbuf -> unit) -> 'a -> (unit -> unit) -> unit + + +(** {6 Répertoires/Noms de fichiers} *) + + +(** [is_directory nom] test si [nom] est un répertoire *) +val is_directory : string -> bool + +(** [get_files_from_dir repertoire] renvoie une liste de chaines de caractères + contenant les répertoires et fichiers contenus dans [repertoire] *) +val get_files_from_dir : string -> string list + +(** [get_dirs_only_from_dir repertoire] renvoie une liste de chaines de caractères + contenant les répertoires contenus dans [repertoire] (sans "." et "..") *) +val get_dirs_only_from_dir : string -> string list + +(** [get_files_only_from_dir repertoire] renvoie une liste de chaines de caractères + contenant les fichiers contenus dans [repertoire] *) +val get_files_only_from_dir : string -> string list + +(** [del_path_in_filename fichier] supprime l'éventuel chemin contenu dans + la chaine [fichier] *) +val del_path_in_filename : string -> string + + +(** {6 Manipulation de dates} *) + + +(** [decompose_date date] decompose une [date] de la forme 20020114 en un triplet + contenant le jour, le mois et l'année (ici (14, 01, 2002)) *) +val decompose_date : int -> int * int * int + +(** [compose_date (jour, mois, annee)] effectue l'operation inversion de + {!Ocaml_tools.decompose_date} *) +val compose_date : int * int * int -> int + +(** [get_month_of_num numero_du_mois] renvoie une chaine de caractères contenant + le mois donné par son numéro. 1 -> "Janvier" *) +val get_month_of_num : int -> string + +(** [get_day_of_date (jour, mois, annee)] renvoie une chaine indiquant le jour + de la semaine correspondant à la date donnée *) +val get_day_of_date : int * int * int -> string + +(** [get_day_of_date2 date] renvoie une chaine indiquant le jour + de la semaine correspondant à la date donnée *) +val get_day_of_date2 : int -> string + +(** [is_year_bis annee] indique si l'année donnée est bissextile ou pas *) +val is_year_bis : int -> bool + +(** [get_nb_days_in_month mois annee] indique le nombre de jour du mois de l'année + indiquée ([mois]=1 correspond a Janvier) *) +val get_nb_days_in_month : int -> int -> int + +(** [get_delta_date (jour, mois, annee) delta_jours] renvoie un triplet contenant + la date augmentée de [delta_jours]. [delta_jours] peut etre négatif *) +val get_delta_date : int * int * int -> int -> int * int * int + +(** [get_delta_date date delta_jours] renvoie une date entière correspondant à + [date] augmentée de [delta_jours]. [delta_jours] peut etre négatif *) +val get_delta_date2 : int -> int -> int + +(** [get_next_date (jour, mois, annee)] renvoie un triplet correspondant à la date + augmentée de 1 jour *) +val get_next_date : int * int * int -> int * int * int + +(** [get_next_date2 date] renvoie une date correspondant à [date] + augmentée de 1 jour *) +val get_next_date2 : int -> int + +(** [get_prev_date (jour, mois, annee)] renvoie un triplet correspondant à la date + diminuée de 1 jour *) +val get_prev_date : int * int * int -> int * int * int + +(** [get_prev_date2 date] renvoie une date correspondant à [date] diminuée de + 1 jour *) +val get_prev_date2 : int -> int + +(** [get_diff_date (jour1, mois1, annee1) (jour2, mois2, annee2)] renvoie le nombre de jours (signé) séparant la première date de la seconde (d2-d1) *) +val get_diff_date : int * int * int -> int * int * int -> int + +(** [get_diff_date2 date1 date2] idem à la fonction précédente avec des dates en entiers sous la forme AAAAMMJJ *) +val get_diff_date2 : int -> int -> int + +(** [string_of_date (jour, mois, annee)] fournit une chaine correspondant à la date + sous la forme JJ/MM/AAAA *) +val string_of_date : int * int * int -> string + +(** [string_of_date2 date] fournit une chaine correspondant à la date + sous la forme JJ/MM/AAAA *) +val string_of_date2 : int -> string + + +(** {6 Manipulation de temps} *) + + +(** [string_of_time temps] transforme un [temps] en secondes en une chaine + au format hh:mm:ss *) +val string_of_time : int -> string + +(** [string_of_time_without_seconds temps] transforme un [temps] en + secondes en une chaine au format hh:mm *) +val string_of_time_without_seconds : int -> string + +(** [time_of_string chaine] transforme une chaine au format hh:mm:ss en secondes *) +val time_of_string : string -> int + +(** renvoie l'heure actuelle *) +val timer_get_time : unit -> Unix.tm + +(** [timer_string_of_time heure] renvoie une chaine sous la forme hh:mm:ss *) +val timer_string_of_time : Unix.tm -> string + +(** [timer_string_of_date heure] renvoie une chaine au format JJ/MM/AAAA *) +val timer_string_of_date : Unix.tm -> string + +(** [timer_sub heure1 heure2] renvoie le nombre de secondes ecoulées entre + [heure1] et [heure2] *) +val timer_sub : Unix.tm -> Unix.tm -> int + +(** [timer_string_of_secondes secondes] renvoie une chaine à partir du temps donné + en secondes. La chaine est au format h:mm:ss *) +val timer_string_of_secondes : int -> string + + +(** {6 Tirages aléatoires} *) + +(** [tirage_aleatoire_lst liste] effectue un tirage aléatoire d'un des éléments de la + liste et renvoie cet élément *) +val tirage_aleatoire_lst : 'a list -> 'a diff --git a/sw/lib/ocaml/platform.ml b/sw/lib/ocaml/platform.ml new file mode 100644 index 0000000000..1f3f03d7c4 --- /dev/null +++ b/sw/lib/ocaml/platform.ml @@ -0,0 +1,31 @@ +(* + * $Id$ + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +let platform_name = + let os = Sys.os_type in + if os = "Win32" then Unix.putenv "GTK_RC_FILES" (Unix.getcwd () ^ "/wingtk.rc") ; + os + +let platform_is_unix = platform_name = "Unix" +let platform_is_win32 = platform_name = "Win32" diff --git a/sw/lib/ocaml/platform.mli b/sw/lib/ocaml/platform.mli new file mode 100644 index 0000000000..3d9cdf015a --- /dev/null +++ b/sw/lib/ocaml/platform.mli @@ -0,0 +1,32 @@ +(* + * $Id$ + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec + * + * 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. + * + *) + +(** Renvoie le nom de la plateforme : Unix ou Win32 *) +val platform_name : string + +(** Teste si la plateforme courante est Unix *) +val platform_is_unix : bool + +(** Teste si la plateforme courante est Windows (Win32) *) +val platform_is_win32 : bool diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml new file mode 100644 index 0000000000..9fc404bf87 --- /dev/null +++ b/sw/lib/ocaml/pprz.ml @@ -0,0 +1,192 @@ +(* + * $Id$ + * + * Downlink protocol (handling messages.xml) + * + * 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. + * + *) + +open Printf + + +type message_id = int +type class_name = string +type format = string +type _type = string +type value = string +type field = { + _type : _type; + fformat : format; + } + +type message = { + name : string; + fields : (string * field) list + } + +type type_descr = { + format : string ; + glib_type : string; + size : int; + value : string + } + + + + +let (//) = Filename.concat +let messages_xml = Xml.parse_file (Env.paparazzi_src // "conf" // "messages.xml") + +external float_of_bytes : string -> int -> float = "c_float_of_indexed_bytes" +external int32_of_bytes : string -> int -> int32 = "c_int32_of_indexed_bytes" +let types = [ + ("uint8", { format = "%u"; glib_type = "guint8"; size = 1; value="42" }); + ("uint16", { format = "%u"; glib_type = "guint16"; size = 2; value="42" }); + ("uint32", { format = "%lu" ; glib_type = "guint32"; size = 4; value="42" }); + ("int8", { format = "%d"; glib_type = "gint8"; size = 1; value="42" }); + ("int16", { format = "%d"; glib_type = "gint16"; size = 2; value="42" }); + ("int32", { format = "%ld" ; glib_type = "gint32"; size = 4; value="42" }); + ("float", { format = "%f" ; glib_type = "gfloat"; size = 4; value="4.2" }) +] + +let size_of_field = fun f -> (List.assoc f._type types).size +let default_format = fun x -> (List.assoc x types).format +let default_value = fun x -> (List.assoc x types).value + +let size_of_message = fun message -> + List.fold_right + (fun (_, f) s -> size_of_field f + s) + message.fields + 4 + +let field_of_xml = fun xml -> + let t = ExtXml.attrib xml "type" in + let f = try Xml.attrib xml "format" with _ -> default_format t in + (ExtXml.attrib xml "name", { _type = t; fformat = f }) + + +(** Table of msg classes indexed by name. Each class is a table of messages +indexed by ids *) +let classes = Hashtbl.create 13 +let _ = + List.iter + (fun xml_class -> + let by_id = Hashtbl.create 13 + and by_name = Hashtbl.create 13 in + List.iter + (fun xml_msg -> + try + let name = ExtXml.attrib xml_msg "name" in + let msg = { + name = name; + fields = List.map field_of_xml (Xml.children xml_msg) + } in + let id = int_of_string (ExtXml.attrib xml_msg "id") (* - 1 !!!!*) in + Hashtbl.add by_id id msg; + Hashtbl.add by_name name (id, msg) + with _ -> + fprintf stderr "Warning: Ignoring '%s'\n" (Xml.to_string xml_msg)) + (Xml.children xml_class); + Hashtbl.add classes (ExtXml.attrib xml_class "name") (by_id, by_name) + ) + (Xml.children messages_xml) + +let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format) + + let format_field = fun buffer index (field:field) -> + let format = field.fformat in + match field._type with + "uint8" | "int8" -> sprintf (magic format) (Char.code buffer.[index]) + | "uint16" | "int16" -> sprintf (magic format) (Char.code buffer.[index] lsl 8 + Char.code buffer.[index+1]) + | "float" -> sprintf (magic format) (float_of_bytes buffer index) + | "int32" | "uint32" -> sprintf (magic format) (int32_of_bytes buffer index) + | _ -> failwith "format_field" + +module type CLASS = sig val name : string end + +exception Unknown_msg_name of string + +module Protocol(Class:CLASS) = struct + let stx = Char.chr 0x05 + let index_start = fun buf -> + String.index buf stx + + let messages_by_id, messages_by_name = Hashtbl.find classes Class.name + let message_of_id = fun id -> Hashtbl.find messages_by_id (id (*** +1 ***)) + let message_of_name = fun name -> Hashtbl.find messages_by_name name + + let length = fun buf start -> + let len = String.length buf - start in + if len >= 2 then + let id = Char.code buf.[start+1] in + let msg = message_of_id id in + let l = size_of_message msg in + Debug.call 'T' (fun f -> fprintf f "Pprz id=%d len=%d\n" id l); + l + else + raise Serial.Not_enough + + let (+=) = fun r x -> r := (!r + x) land 0xff + let checksum = fun msg -> + let l = String.length msg in + let ck_a = ref 0 and ck_b = ref 0 in + for i = 1 to l - 3 do + ck_a += Char.code msg.[i]; + ck_b += !ck_a + done; + Debug.call 'T' (fun f -> fprintf f "Pprz ck: %d %d\n" !ck_a (Char.code msg.[l-2])); + !ck_a = Char.code msg.[l-2] && !ck_b = Char.code msg.[l-1] + + let values_of_bin = fun buffer -> + let id = Char.code buffer.[1] in + let message = message_of_id id in + Debug.call 'T' (fun f -> fprintf f "Pprz.values id=%d\n" id); + let rec loop = fun index fields -> + match fields with + [] -> [] + | (field_name, field_descr)::fs -> + let n = size_of_field field_descr in + (field_name, format_field buffer index field_descr) :: loop (index+n) fs in + (id, loop 2 message.fields) + + let space = Str.regexp "[ \t]+" + let values_of_string = fun s -> + match Str.split space s with + msg_name::args -> + begin + try + let msg_id, msg = message_of_name msg_name in + let values = List.map2 (fun (field_name, _) v -> (field_name, v)) msg.fields args in + (msg_id, values) + with + Not_found -> raise (Unknown_msg_name msg_name) + end + | [] -> invalid_arg "Pprz.values_of_string" + + let string_of_message = fun msg values -> + String.concat " " + (msg.name:: + List.map + (fun (field_name, field) -> + try List.assoc field_name values with Not_found -> default_value field._type) + msg.fields) +end + diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli new file mode 100644 index 0000000000..d2439a7f90 --- /dev/null +++ b/sw/lib/ocaml/pprz.mli @@ -0,0 +1,57 @@ +(* + * $Id$ + * + * Downlink protocol (handling messages.xml) + * + * 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. + * + *) + +type class_name = string +type message_id = int +type format = string +type _type = string +type value = string +type field = { _type : _type; fformat : format; } +type message = { name : string; fields : (string * field) list; } +type type_descr = { format : format; glib_type : string; size:int; value:string} +val types : (string * type_descr) list +val size_of_field : field -> int +val default_format : string -> string + +exception Unknown_msg_name of string + +module type CLASS = sig val name : string end +module Protocol : functor (Class : CLASS) -> sig + include Serial.PROTOCOL + val message_of_id : message_id -> message + val message_of_name : string -> message_id * message + val values_of_bin : string -> message_id * (string * string) list +(** [values raw_message] Parses a raw message, returns the + message id and the liste of (field_name, value) *) + + val values_of_string : string -> message_id * (string * string) list + (** May raise [(Unknown_msg_name msg_name)] *) + + val string_of_message : message -> (string * string) list -> string + (** [string_of_message msg values] *) +end + + diff --git a/sw/lib/ocaml/serial.ml b/sw/lib/ocaml/serial.ml new file mode 100644 index 0000000000..577c49c5a8 --- /dev/null +++ b/sw/lib/ocaml/serial.ml @@ -0,0 +1,114 @@ +(* + * $Id$ + * + * Serial Port handling + * + * Copyright (C) 2004 CENA/ENAC, Pascal Brisset + * + * 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 + +type speed = + B0 + | B50 + | B75 + | B110 + | B134 + | B150 + | B200 + | B300 + | B600 + | B1200 + | B1800 + | B2400 + | B4800 + | B9600 + | B19200 + | B38400 + | B57600 + | B115200 + | B230400 + + +external init_serial : string -> speed -> Unix.file_descr = "c_init_serial";; + +let opendev device speed = + try + init_serial device speed + with + Failure x -> + failwith (Printf.sprintf "%s (%s)" x device) + +let close = Unix.close + + +let buffer_len = 256 +let input = fun f -> + let buffer = String.create buffer_len + and index = ref 0 in + + let wait = fun start n -> + String.blit buffer start buffer 0 n; + index := n in + + fun fd -> + let n = !index + Unix.read fd buffer !index (buffer_len - !index) in + Debug.call 'T' (fun f -> fprintf f "input: %d %d\n" !index n); + let rec parse = fun start n -> + Debug.call 'T' (fun f -> fprintf f "input parse: %d %d\n" start n); + let nb_used = f (String.sub buffer start n) in +(* Printf.fprintf stderr "n'=%d\n" nb_used; flush stderr; *) + if nb_used > 0 then + parse (start + nb_used) (n - nb_used) + else + wait start n in + parse 0 n + + +exception Not_enough + +module type PROTOCOL = sig + val index_start : string -> int (* raise Not_found *) + val length : string -> int -> int (* raise Not_enough *) + val checksum : string -> bool +end + +module Transport(Protocol:PROTOCOL) = struct + let rec parse = fun use buf -> + let start = ref 0 + and n = String.length buf in + try + start := Protocol.index_start buf; + let length = Protocol.length buf !start in + let end_ = !start + length in + if n < end_ then + raise Not_enough; + let msg = String.sub buf !start length in + if Protocol.checksum msg then begin + use msg + end else + Debug.call 'T' (fun f -> fprintf f "Transport.chk: %s\n" (Debug.xprint msg)) + ; + end_ + parse use (String.sub buf end_ (String.length buf - end_)) + with + Not_found -> String.length buf + | Not_enough -> !start +end diff --git a/sw/lib/ocaml/serial.mli b/sw/lib/ocaml/serial.mli new file mode 100644 index 0000000000..93e0f6a9f2 --- /dev/null +++ b/sw/lib/ocaml/serial.mli @@ -0,0 +1,76 @@ +(* + * $Id$ + * + * Serial Port handling + * + * Copyright (C) 2004 CENA/ENAC, Pascal Brisset + * + * 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. + * + *) + +type speed = + B0 + | B50 + | B75 + | B110 + | B134 + | B150 + | B200 + | B300 + | B600 + | B1200 + | B1800 + | B2400 + | B4800 + | B9600 + | B19200 + | B38400 + | B57600 + | B115200 + | B230400 + +val opendev : string -> speed -> Unix.file_descr +val close : Unix.file_descr -> unit + +val input : (string -> int) -> Unix.file_descr -> unit +(** [input f fd] Calls [f] on the buffer of available characters on [fd] each +time a new character arrives. [f] must return the number of consumed +characters *) + +exception Not_enough +module type PROTOCOL = + sig + val index_start : string -> int + (** Must return the index of the first char of the the first message. + May raise Not_found or Not_enough *) + + val length : string -> int -> int + (** [length buf start] Must return the length of the message starting at + [start]. May raise Not_enough *) + val checksum : string -> bool + (** [checksum message] *) + end + +module Transport : + functor (Protocol : PROTOCOL) -> + sig + val parse : (string -> unit) -> string -> int + (** [parse f buf] Scans [buf] according to [Protocol] and applies [f] on + every recognised message. Returns the number of consumed bytes. *) + end diff --git a/sw/lib/ocaml/srtm.ml b/sw/lib/ocaml/srtm.ml new file mode 100644 index 0000000000..bae494b903 --- /dev/null +++ b/sw/lib/ocaml/srtm.ml @@ -0,0 +1,82 @@ +(* + * $Id$ + * + * Acces functions to SRTM data (http://edcftp.cr.usgs.gov/pub/data/srtm) + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec, 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 Latlong + +type error = string +exception Tile_not_found of string + +let srtm_url = "ftp://e0dps01u.ecs.nasa.gov/srtm" + +let error = fun string -> + Printf.sprintf "wget %s/Eurasia/%s.hgt.zip" srtm_url string + +let tile_size = 1201 + +(* Previously opened tiles *) +let htiles = Hashtbl.create 13 + +(* Path to data files *) +let path = ref ["."] + +let add_path = fun p -> path := p :: !path + +let open_compressed = fun f -> + Ocaml_tools.open_compress (Ocaml_tools.find_file !path f) + +let find tile = + if not (Hashtbl.mem htiles tile) then begin + try + let f = open_compressed (tile^".hgt") in + let n = tile_size*tile_size*2 in + let buf = String.create n in + really_input f buf 0 n; + Hashtbl.add htiles tile buf + with Not_found -> + raise (Tile_not_found tile) + end; + Hashtbl.find htiles tile + +let get = fun tile y x -> + let tile = find tile in + let pos0 = (2*((tile_size-y)*tile_size+x)) in + let rec skip_bad = fun pos -> + let a = (Char.code tile.[pos] lsl 8) lor Char.code tile.[pos+1] in + if a > 8848 || a < 0 then skip_bad (pos+2) else a in + skip_bad pos0 + +let of_wgs84 = fun geo -> + let lat = (Rad>>Deg)geo.posn_lat + and long = (Rad>>Deg)geo.posn_long in + let bottom = floor lat and left = floor long in + let tile = + Printf.sprintf "%c%.0f%c%03.0f" (if lat > 0. then 'N' else 'S') bottom (if long > 0. then 'E' else 'W') (abs_float left) in + + get tile (truncate ((lat-.bottom)*.1200.+.0.5)) (truncate ((long-.left)*.1200.+.0.5)) + +let of_utm = fun utm -> + of_wgs84 (Latlong.of_utm WGS84 utm) + diff --git a/sw/lib/ocaml/srtm.mli b/sw/lib/ocaml/srtm.mli new file mode 100644 index 0000000000..e7ab560649 --- /dev/null +++ b/sw/lib/ocaml/srtm.mli @@ -0,0 +1,42 @@ +(* + * $Id$ + * + * Acces functions to SRTM data (http://edcftp.cr.usgs.gov/pub/data/srtm) + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec, 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. + * + *) + +val srtm_url : string + +val add_path : string -> unit +(** [add_path directory] Adds [directory] to the current path where, possibly +compressed, SRTM data files are searched. *) + +type error = string +exception Tile_not_found of error + +val error : error -> string + +val of_utm : Latlong.utm -> int +(** [of_utm utm_pos] Returns the altitude of the given UTM position *) + +val of_wgs84 : Latlong.geographic -> int +(** [of_utm utm_pos] Returns the altitude of the given geographic position *) diff --git a/sw/lib/ocaml/ubx.ml b/sw/lib/ocaml/ubx.ml new file mode 100644 index 0000000000..a067460c8f --- /dev/null +++ b/sw/lib/ocaml/ubx.ml @@ -0,0 +1,159 @@ +(* + * $Id$ + * + * UBX protocol handling + * + * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec, 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. + * + *) +module Protocol = struct + let index_start = fun buf -> + let rec loop = fun i -> + let i' = String.index_from buf i (Char.chr 0xb5) in + if String.length buf > i'+1 && buf.[i'+1] = Char.chr 0x62 then + i' + else + loop (i'+1) in + loop 0 + + let payload_length = fun buf start -> + Char.code buf.[start+5] lsl 8 + Char.code buf.[start+4] + + let length = fun buf start -> + let len = String.length buf - start in + if len > 6 then + payload_length buf start + 8 + else + raise Serial.Not_enough + + let payload = fun buf start -> + String.sub buf (start+6) (payload_length buf start) + + let uint8_t = fun x -> x land 0xff + let (+=) = fun r x -> r := uint8_t (!r + x) + let checksum = fun buf start payload -> + let ck_a = ref 0 and ck_b = ref 0 in + let l = String.length payload in + for i = 0 to l - 1 do + ck_a += Char.code payload.[i]; + ck_b += !ck_a + done; + !ck_a = Char.code buf.[start+l+6] && !ck_b = Char.code buf.[start+l+7] +end + +let (//) = Filename.concat + +let ubx_xml = + Xml.parse_file (Env.paparazzi_src // "conf" // "ubx.xml") + +let ubx_get_class = fun name -> + ExtXml.child ubx_xml ~select:(fun x -> ExtXml.attrib x "name" = name) "class" + +let ubx_nav = ubx_get_class "NAV" +let ubx_nav_id = int_of_string (ExtXml.attrib ubx_nav "ID") +let ubx_get_msg = fun ubx_class name -> + ExtXml.child ubx_class ~select:(fun x -> ExtXml.attrib x "name" = name) "message" + +let ubx_get_nav_msg = fun name -> ubx_get_msg ubx_nav name + +let nav_posllh = ubx_nav_id, ubx_get_nav_msg "POSLLH" +let nav_posutm = ubx_nav_id, ubx_get_nav_msg "POSUTM" +let nav_status = ubx_nav_id, ubx_get_nav_msg "STATUS" +let nav_velned = ubx_nav_id, ubx_get_nav_msg "VELNED" + + +let send_start_sequence = fun gps -> + output_byte gps 0xB5; + output_byte gps 0x62 + + +let sizeof = function + "U4" | "I4" -> 4 + | "U2" | "I2" -> 2 + | "U1" | "I1" -> 1 + | x -> failwith (Printf.sprintf "Ubx.sizeof: unknown format '%s'" x) + +let assoc = fun label fields -> + let rec loop o = function + [] -> raise Not_found + | f::fs -> + let format = ExtXml.attrib f "format" in + if ExtXml.attrib f "name" = label + then (o, format) + else loop (o + sizeof format) fs in + loop 0 fields + +let byte = fun x -> Char.chr (x land 0xff) + +let make_payload = fun msg_xml values -> + let n = int_of_string (ExtXml.attrib msg_xml "length") in + let p = String.make n '#' in + let fields = Xml.children msg_xml in + List.iter + (fun (label, value) -> + let (pos, fmt) = + try + assoc label fields + with + Not_found -> failwith (Printf.sprintf "Field '%s' not found in %s" label (Xml.to_string msg_xml)) + in + match fmt with + | "U1" -> + assert(value >= 0 && value < 0x100); + p.[pos] <- byte value + | "I4" | "U4" -> + assert(fmt <> "U4" || value >= 0); + p.[pos+3] <- byte (value asr 24); + p.[pos+2] <- byte (value lsr 16); + p.[pos+1] <- byte (value lsr 8); + p.[pos+0] <- byte value + | "U2" | "I2" -> + p.[pos+1] <- byte (value lsr 8); + p.[pos+0] <- byte value + | _ -> failwith (Printf.sprintf "Ubx.make_payload: unknown format '%s'" fmt) + ) + values; + p + + + + + +let send = fun gps (msg_class, msg) values -> + let msg_id = int_of_string (Xml.attrib msg "ID") in + let payload = make_payload msg values in + let n = String.length payload in + send_start_sequence gps; + + let ck_a = ref 0 and ck_b = ref 0 in + let output_byte_ck = fun c -> + ck_a := (!ck_a+c) land 0xff; ck_b := (!ck_b+ !ck_a) land 0xff; + output_byte gps c in + + output_byte_ck msg_class; + output_byte_ck msg_id; + output_byte_ck (n land 0xff); + output_byte_ck ((n land 0xff00) lsr 8); + String.iter (fun c -> output_byte_ck (Char.code c)) payload; + output_byte gps !ck_a; + output_byte gps !ck_b; + flush gps + + diff --git a/sw/lib/ocaml/utm_of.ml b/sw/lib/ocaml/utm_of.ml new file mode 100644 index 0000000000..2a1b8ea269 --- /dev/null +++ b/sw/lib/ocaml/utm_of.ml @@ -0,0 +1,6 @@ +open Latlong + +let _ = + let f = fun i -> (Deg>>Rad)(float_of_string Sys.argv.(i)) in + let utm = utm_of WGS84 { posn_lat = f 1 ; posn_long = f 2 } in + Printf.printf "%d %d\n" utm.utm_x utm.utm_y diff --git a/sw/lib/ocaml/wavecard.ml b/sw/lib/ocaml/wavecard.ml new file mode 100644 index 0000000000..2ea9e77a5a --- /dev/null +++ b/sw/lib/ocaml/wavecard.ml @@ -0,0 +1,131 @@ +open Printf + +type cmd_name = string +type data = string + +type cmd = cmd_name * string + +let cmd_names = [ + 0x06, "ACK"; + 0x15, "NAK"; + 0x00, "ERROR"; + 0x40, "REQ_WRITE_RADIO_PARAM"; + 0x41, "RES_WRITE_RADIO_PARAM"; + 0x50, "REQ_READ_RADIO_PARAM"; + 0x51, "RES_READ_RADIO_PARAM"; + 0x60, "REQ_SELECT_CHANNEL"; + 0x61, "RES_SELECT_CHANNEL"; + 0x62, "REQ_READ_CHANNEL"; + 0x63, "RES_READ_CHANNEL"; + 0x64, "REQ_SELECT_PHYCONFIG"; + 0x65, "RES_SELECT_PHYCONFIG"; + 0x66, "REQ_READ_PHYCONFIG"; + 0x67, "RES_READ_PHYCONFIG"; + 0x68, "REQ_READ_REMOTE_RSSI"; + 0x69, "RES_READ_REMOTE_RSSI"; + 0x6A, "REQ_READ_LOCAL_RSSI"; + 0x6B, "RES_READ_LOCAL_RSSI"; + 0xA0, "REQ_FIRMWARE_VERSION"; + 0xA1, "RES_FIRMWARE_VERSION"; + 0xB0, "MODE_TEST"; + 0x20, "REQ_SEND_FRAME"; + 0x21, "RES_SEND_FRAME"; + 0x22, "REQ_SEND_MESSAGE"; + 0x26, "REQ_SEND_POLLING"; + 0x28, "REQ_SEND_BROADCAST"; + 0x30, "RECEIVED_FRAME"; + 0x31, "RECEPTION_ERROR"; + 0x32, "RECEIVED_FRAME_POLLING"; + 0x34, "RECEIVED_FRAME_BROADCAST"; + 0x36, "RECEIVED_MULTIFRAME"; + 0x80, "REQ_SEND_SERVICE"; + 0x81, "RES_SEND_SERVICE"; + 0x82, "SERVICE_RESPONSE"] + +let rec cossa = fun x l -> + match l with + [] -> raise Not_found + | (v, k)::xs -> if k = x then v else cossa x xs + +let cmd_name_of = fun x -> try List.assoc x cmd_names with Not_found -> failwith (sprintf "Unknown command: %2x" x) +let of_cmd_name = fun x -> try cossa x cmd_names with Not_found -> failwith (sprintf "Unknown command: %s" x) + +let sync = Char.chr 0xff +let stx = Char.chr 0x02 +let etx = Char.chr 0x03 + +let length = fun buf -> Char.code buf.[2] +let total_length = fun buf -> length buf + 3 + +let payload = fun buf -> + let l = length buf in + let data = String.sub buf 4 (l-4) in + (cmd_name_of (Char.code buf.[3]), data) + +let (^=) r x = r := !r lxor x + +let compute_checksum = + let poly = 0x8408 in + fun buf -> + let lg = length buf - 2 + and crc = ref 0 in + for j = 0 to lg - 1 do + crc ^= Char.code buf.[j+2]; + for i = 0 to 7 do + let carry = !crc land 0x01 = 1 in + crc := !crc lsr 1; + if carry then + crc ^= poly + done + done; + !crc + +let checksum = fun buf -> + let crc = compute_checksum buf + and l = length buf in + crc land 0xff = Char.code buf.[l] && crc lsr 8 = Char.code buf.[l+1] + + + + +let parse = fun buf ?ack f -> + let n = String.length buf in + if n < 3 || n < total_length buf then + 0 (* Not enough chars to read *) + else if buf.[0] <> sync then + 1 + else if buf.[1] <> stx || not (checksum buf) then + 2 + else begin + f (payload buf); + begin + match ack with + None -> () + | Some ack -> ack () + end; + total_length buf + end + + +let receive = fun ?ack f -> + Serial.input (fun b -> parse b ?ack f) + +let send = fun fd (cmd, data) -> + let l = String.length data + 4 in + if l >= 256 then + invalid_arg "Wavecard.send"; + let buf = String.create (l+3) in + buf.[0] <- sync; + buf.[1] <- stx; + buf.[2] <- Char.chr l; + buf.[3] <- Char.chr (of_cmd_name cmd); + for i = 4 to l - 1 do + buf.[i] <- data.[i-4] + done; + let crc = compute_checksum buf in + buf.[l] <- Char.chr (crc land 0xff); + buf.[l+1] <- Char.chr (crc lsr 8); + buf.[l+2] <- etx; + let o = Unix.out_channel_of_descr fd in + Printf.fprintf o "%s" buf; + flush o diff --git a/sw/lib/ocaml/wavecard.mli b/sw/lib/ocaml/wavecard.mli new file mode 100644 index 0000000000..c2f344112a --- /dev/null +++ b/sw/lib/ocaml/wavecard.mli @@ -0,0 +1,7 @@ +type cmd_name = string +type data = string +type cmd = cmd_name * data + +val send : Unix.file_descr -> cmd -> unit + +val receive : ?ack:(unit -> unit) -> (cmd -> 'a) -> (Unix.file_descr -> unit) diff --git a/sw/lib/ocaml/xml2h.ml b/sw/lib/ocaml/xml2h.ml new file mode 100644 index 0000000000..8b417b6635 --- /dev/null +++ b/sw/lib/ocaml/xml2h.ml @@ -0,0 +1,69 @@ +(* + * $Id$ + * + * XML preprocessing tools + * + * 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. + * + *) + +open Printf + +exception Error of string + +let nl = print_newline + +let define = fun n x -> + printf "#define %s %s\n" n x + +let define_string = fun n x -> + define n ("\""^x^"\"") + + +let xml_error s = failwith ("Bad XML tag: "^s^ " expected") + + +let sprint_float_array = fun l -> + let rec loop = function + [] -> "}" + | [x] -> x ^ "}" + | x::xs -> x ^","^ loop xs in + "{" ^ loop l + + +let start_and_begin = fun xml_file h_name -> + let xml = Xml.parse_file xml_file in + + printf "/* This file has been generated from %s */\n" xml_file; + printf "/* Please DO NOT EDIT */\n\n"; + + printf "#ifndef %s\n" h_name; + define h_name ""; + nl (); + xml + +let finish = fun h_name -> + printf "\n#endif // %s\n" h_name + +let warning s = + Printf.fprintf stderr "\n##################################################\n"; + Printf.fprintf stderr "\n %s\n" s; + Printf.fprintf stderr "\n##################################################\n" + diff --git a/sw/lib/ocaml/xml_get.ml b/sw/lib/ocaml/xml_get.ml new file mode 100644 index 0000000000..4b19004c74 --- /dev/null +++ b/sw/lib/ocaml/xml_get.ml @@ -0,0 +1,16 @@ +open Xml2h +let _ = + if Array.length Sys.argv <> 4 then + failwith "Usage: conf_get "; + let xml_file = Sys.argv.(1) + and path = Sys.argv.(2) + and attribute = Sys.argv.(3) in + let xml = + try + Xml.parse_file xml_file + with + Xml.Error e -> + Printf.fprintf stderr "\nError in \"%s\": %s\n\n" xml_file (Xml.error e); + exit 1 + in + Printf.printf "%s\n" (ExtXml.get_attrib xml path attribute) diff --git a/sw/lib/perl/Makefile b/sw/lib/perl/Makefile new file mode 100644 index 0000000000..a0cbee8405 --- /dev/null +++ b/sw/lib/perl/Makefile @@ -0,0 +1,21 @@ + + +#all: IvyMsgs.pm + +#OCAMLC = ocamlc -I ../../lib/ocaml +#CONF_DIR = ../../../conf + +#GEN_MESSAGES = ./gen_messages.out +#MESSAGES_XML = $(CONF_DIR)/messages.xml + + +#$(GEN_MESSAGES) : gen_messages.ml +# $(OCAMLC) -o $@ lib.cma $< + +#IvyMsgs.pm : $(MESSAGES_XML) $(GEN_MESSAGES) +# $(GEN_MESSAGES) $< > /tmp/x.pm +# mv /tmp/x.pm $@ + +clean: + rm -f *~ +#IvyMsgs.pm gen_messages.out *.cmo *.cmi diff --git a/sw/lib/perl/Paparazzi/Environment.pm b/sw/lib/perl/Paparazzi/Environment.pm new file mode 100644 index 0000000000..de4f9fc353 --- /dev/null +++ b/sw/lib/perl/Paparazzi/Environment.pm @@ -0,0 +1,69 @@ +package Paparazzi::Environment; + +use File::NCopy; +use Getopt::Long; + +use constant INST_PREFIX => "/usr"; + +my $paparazzi_src = undef; +my $paparazzi_home = $ENV{HOME}."/paparazzi"; + +if (defined $ENV{PAPARAZZI_SRC}) { + $paparazzi_src = $ENV{PAPARAZZI_SRC}; + $paparazzi_home = $ENV{PAPARAZZI_SRC}; +} +$paparazzi_home = $ENV{PAPARAZZI_HOME} if (defined $ENV{PAPARAZZI_HOME}); +print "\nEnvironment : "; +if (defined $paparazzi_src) { + print "source directory mode\n paparazzi_src $paparazzi_src\n"; +} +else { + print "system mode\n inst_prefix INST_PREFIX"; +} +print " paparazzi_home $paparazzi_home\n\n"; + +sub parse_command_line { + my ($options) = @_; + my $getopt_h = {"b=s" => \$options->{ivy_bus}}; + foreach my $option (keys %{$options}) { + $getopt_h->{$option."=s"} = \$options->{$option}; + } + return GetOptions (%{$getopt_h}); +} + +sub check_paparazzi_home { + unless (defined $paparazzi_src) { + unless (-e $paparazzi_home) { + print "\nDirectory $paparazzi_home doesn't exist\n"; + print "This directory is needed to store user configuration and data\n"; + print "Shall I create it and populate it with examples? (Y/n)\n"; + my $ans = ; + chop($ans); + if ($ans eq "" || $ans eq "Y" || $ans eq "y") { + print "Creating directory $paparazzi_home\n"; + mkdir($paparazzi_home, 0755); + print "Copying default config and examples\n"; + my $copier = File::NCopy->new(recursive => 1); + foreach my $dir ("conf", "var", "data") { + $copier->copy(INST_PREFIX."/share/paparazzi/".$dir, $paparazzi_home); + } + print "done.\n\n"; + } + else { + print "exiting...\n"; + exit(1); + } + } + } +} + +sub paparazzi_src { + return $paparazzi_src; +} + +sub paparazzi_home { + return $paparazzi_home; +} + + +1; diff --git a/sw/lib/perl/Paparazzi/IvyProtocol.pm b/sw/lib/perl/Paparazzi/IvyProtocol.pm new file mode 100644 index 0000000000..1dd4cb0ca3 --- /dev/null +++ b/sw/lib/perl/Paparazzi/IvyProtocol.pm @@ -0,0 +1,134 @@ +package Paparazzi::IvyProtocol; + +use strict; +use XML::DOM; + +use Data::Dumper; + +my $classes_by_name = {}; + +my $req_id = int rand(65534); +# TODO: make real timeout instead of single request +my $res_regexp = undef; + +sub request_message { + my ($msg_class, $msg_name, $args, $ivy, $callback) = @_; + my $message = $classes_by_name->{$msg_class}->{$msg_name}; + return unless defined $message; + # unbind previous request + $ivy->bindRegexp($res_regexp) if defined $res_regexp; + $res_regexp = "^(".$args->{id}.") ".$msg_name."_RES ".++$req_id; + foreach my $field (@{$message}) {$res_regexp.= " (\\S+)" unless $field->{name} eq "id"}; + print "res_regexp \'$res_regexp\'\n"; + $ivy->bindRegexp ($res_regexp, $callback); + my $req_msg = $args->{id}." ".$msg_name."_REQ ".$req_id; + print "req_msg \'$req_msg\'\n"; + $ivy->sendMsgs($req_msg); + return $req_id; +} + +sub bind_request_message { + my ($msg_class, $msg_name, $args, $ivy, $callback) = @_; + + + +} + +sub bind_message { + my ($msg_class, $msg_name, $args, $ivy, $callback) = @_; + my $regexp = get_regexp($msg_class, $msg_name, $args); + print ((defined $callback ? "binding":"removing binding")." on \'$regexp\'\n"); + $ivy->bindRegexp ($regexp, $callback); +} + +sub get_regexp { + my ($msg_class, $msg_name, $args) = @_; + warn "no such class : $msg_class" unless defined $classes_by_name->{$msg_class}; + my $message = $classes_by_name->{$msg_class}->{$msg_name}; + return "" if !defined $message; + my $nb_fields = @{$message}; + my $regexp = ""; + if( $msg_class eq "ground") { + $regexp = "^".$msg_class." ".$msg_name; + foreach (@{$message}) {$regexp.= " (\\S+)"}; + } + elsif ( $msg_class eq "aircraft_info") { + $regexp = "^(".$args->{id}.") ".$msg_name; + foreach my $field (@{$message}) {$regexp.= " (\\S+)" unless $field->{name} eq "id"}; + } + else { + $regexp = "^.*".$msg_name."\\s+"; + foreach (@{$message}) {$regexp.= " (\\S+)"}; + } + return $regexp; +} + +sub get_values_by_name { + my ($msg_class, $msg_name, $ivy_args) = @_; + my $values_by_name = {}; + my $message = $classes_by_name->{$msg_class}->{$msg_name}; + return {} unless defined $message; + for (my $i=0; $i<@{$message}; $i++) { + my $field = $message->[$i]; + $values_by_name->{$field->{name}} = $ivy_args->[$i+1]; + } + return $values_by_name; +} + +sub getMsg { + my ($msg_class, $msg_name, $args) = @_; + my $message = $classes_by_name->{$msg_class}->{$msg_name}; + my $str = ""; + if ( $msg_class eq "ground") { + $str .= $msg_class." ".$msg_name; + } + else { + $str .= $msg_name; + } + foreach my $field (@{$message}) { + $str.= " ".$args->{$field->{name}}; + } + return $str; +} + +sub sendMsg { + my ($ivy, $msg_class, $msg_name, $args) = @_; + $ivy->sendMsgs(getMsg($msg_class, $msg_name, $args)); +} + +sub read_protocol { + my ($filename, $classe_name) = @_; + my $parser = XML::DOM::Parser->new(); + my $doc = $parser->parsefile($filename); + my $protocol = $doc->getElementsByTagName('protocol')->[0]; + foreach my $class ($protocol->getElementsByTagName('class')) { + if ($class->getAttribute("name") eq $classe_name) { + my $messages_by_name = {}; + foreach my $message ($class->getElementsByTagName('message')) { + my $message_name = $message->getAttribute("name"); + my @msg_a = (); + if ($classe_name eq "aircraft_info") { push @msg_a, { name =>"id", type =>"string"}}; + foreach my $field ($message->getElementsByTagName('field')) { + my $field_name = $field->getAttribute("name"); + my $field_h = { name => $field->getAttribute("name"), + type => $field->getAttribute("type"), + format => $field->getAttribute("format"), + unit => $field->getAttribute("unit"), + values => $field->getAttribute("values"), + }; + push @msg_a, $field_h; + # print "$classe_name $message_name $field_name\n"; + } + $messages_by_name->{$message_name} = \@msg_a; + } + $classes_by_name->{$classe_name} = $messages_by_name; + } + } + # use Data::Dumper; + # print Dumper($messages_by_name); +} + + + +1; + diff --git a/sw/lib/perl/Paparazzi/Utils.pm b/sw/lib/perl/Paparazzi/Utils.pm new file mode 100644 index 0000000000..7c8e4d4f5d --- /dev/null +++ b/sw/lib/perl/Paparazzi/Utils.pm @@ -0,0 +1,40 @@ +package Utils; + +use Data::Dumper; +use Math::Trig; + +sub trim { + my ($x, $min, $max) = @_; + return $min if ($x < $min); + return $max if ($x > $max); + return $x; +} + +sub diff_array { + my ($a, $b) = @_; +# print "diff_array [ @{$a} ] - [ @{$b} ] => "; + my @aonly; + my %seen; + @seen{@{$b}} = (); + foreach my $ac (@{$a}) { + push(@aonly, $ac) unless exists $seen{$ac}; + } +# print "[ @aonly ]\n"; + return @aonly; +} + +sub rad_of_deg { + return (shift @_) * Math::Trig::pip2() /90.; +} + +sub deg_of_rad { + return (shift @_) * 90. / Math::Trig::pip2(); +} + +sub min { + my ($a, $b) = @_; + return $a if ($a lt $b); + return $b; +} + +1; diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile new file mode 100644 index 0000000000..c3423ec6a3 --- /dev/null +++ b/sw/logalizer/Makefile @@ -0,0 +1,32 @@ +# +# $Id$ +# Copyright (C) 2004 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. +# + +OCAMLC = ocamlc -I ../lib/ocaml +OCAMLOPT = ocamlopt -I ../lib/ocaml + +all: play.opt + +clean: + rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* + +play.opt : play.ml + $(OCAMLOPT) -o $@ unix.cmxa glibivy-ocaml.cmxa -I +lablgtk2 lablgtk.cmxa gtkInit.cmx $^ diff --git a/sw/logalizer/README b/sw/logalizer/README new file mode 100644 index 0000000000..914e57b072 --- /dev/null +++ b/sw/logalizer/README @@ -0,0 +1,27 @@ +# +# $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. +# + + +Tools for manipulating flight logs + + + diff --git a/sw/logalizer/play.ml b/sw/logalizer/play.ml new file mode 100644 index 0000000000..e324262d7f --- /dev/null +++ b/sw/logalizer/play.ml @@ -0,0 +1,117 @@ +let log = ref [||] + +let load_log = fun window (adj:GData.adjustment) name -> + let f = + if Filename.check_suffix name ".gz" + then Unix.open_process_in (Printf.sprintf "zcat %s" name) + else open_in name in + let lines = ref [] in + try + while true do + let l = input_line f in + try + Scanf.sscanf l "%f %[^\n]" (fun t m -> lines := (t,m):: !lines) + with + _ -> () + done + with + End_of_file -> + close_in f; + log := Array.of_list (List.rev !lines); + let start = fst !log.(0) in + let end_ = fst !log.(Array.length !log - 1) -. start in + adj#set_bounds ~upper:end_ (); + window#set_title (Filename.basename name) + + +let timer = ref None +let was_running = ref false + +let stop = fun () -> + match !timer with + None -> () + | Some t -> GMain.Timeout.remove t; timer := None + + +let file_dialog ~title ~callback () = + let sel = GWindow.file_selection ~title ~filename:"*.xml" ~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 window adj () -> + stop (); + ignore (file_dialog ~title:"Open Log" ~callback:(fun name -> load_log window adj name) ()) + +let index_of_time log t = + let t = t +. fst log.(0) in + let rec loop = fun a b -> + if a >= b then a else + let c = (a+b)/ 2 in + if t <= fst log.(c) then loop a c else loop (c+1) b in + loop 0 (Array.length log - 1) + +let rec run log adj i speed = + let (t, m) = log.(i) in + Ivy.send (Printf.sprintf "%.2f %s" t m); + adj#set_value (t -. fst log.(0)); + if i + 1 < Array.length log then + let dt = fst log.(i+1) -. t in + timer := Some (GMain.Timeout.add (truncate (1000. *. dt /. speed#value)) (fun () -> run log adj (i+1) speed; false)) + +let play adj speed = + stop (); + if Array.length !log > 1 then + run !log adj (index_of_time !log adj#value) speed + + + +let _ = + let window = GWindow.dialog ~title:"Paparazzi Replay" ~width:300 () in + let quit = fun () -> GMain.Main.quit (); exit 0 in + ignore (window#connect#destroy ~callback:quit); + + let adj = GData.adjustment + ~value:0. ~lower:0. ~upper:1000. + ~step_incr:0.5 ~page_incr:1.0 ~page_size:1.0 () in + + let speed = GData.adjustment ~value:1. ~lower:0.05 ~upper:10. + ~step_incr:0.25 ~page_incr:1.0 () in + + let bus = ref "127.255.255.255:2010" in + Arg.parse + [ "-b", Arg.String (fun x -> bus := x), "Bus\tDefault is 127.255.255.25:2010"] + (fun x -> load_log window adj x) + "Usage: "; + + + let menubar = GMenu.menu_bar ~packing:window#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 + + ignore (file_menu_fact#add_item "Open Log" ~key:GdkKeysyms._O ~callback:(open_log window adj)); + ignore (file_menu_fact#add_item "Play" ~key:GdkKeysyms._X ~callback:(fun () -> play adj speed)); + ignore (file_menu_fact#add_item "Stop" ~key:GdkKeysyms._S ~callback:(fun () -> stop ())); + ignore (file_menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit); + + + let timescale = GRange.scale `HORIZONTAL ~adjustment:adj ~packing:window#vbox#pack () in + let speed_button = GEdit.spin_button ~adjustment:speed ~rate:0. ~digits:2 ~width:50 ~packing:window#vbox#add () in + + (** #move_slider is not working ??? **) ignore (timescale#event#connect#button_release ~callback:(fun _ -> if !was_running then play adj speed; false)); + ignore (timescale#event#connect#button_press ~callback:(fun _ -> was_running := !timer <> None; stop (); false)); + + window#add_accel_group accel_group; + window#show (); + + Ivy.init "Paparazzi replay" "READY" (fun _ _ -> ()); + Ivy.start !bus; + + GMain.Main.main () diff --git a/sw/logalizer/plot.pl b/sw/logalizer/plot.pl new file mode 100755 index 0000000000..104c918890 --- /dev/null +++ b/sw/logalizer/plot.pl @@ -0,0 +1,342 @@ +#!/usr/bin/perl -w +use Getopt::Long; +use Tk; + +package Ploter; + +use Tk::LabEntry; +use Tk::FileSelect; +use Tk::DialogBox; +use XML::Parser; +use Expect; + +my $paparazzi_home; +BEGIN { + $paparazzi_home = "/home/drouin/work/savannah/paparazzi2"; + $paparazzi_home = $ENV{PAPARAZZI_HOME} if defined $ENV{PAPARAZZI_HOME}; +} +use lib ($paparazzi_home.'/sw/lib/perl'); + +#use ChildrenSpawner; +@ISA = qw(Subject); +use strict; +use warnings; +use diagnostics; + +use Subject; + + +my $log_date; +my $log_duration; +my $log_filename; + +my $time_range="[]"; +my $tr_entry; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-log => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef]); + $self->configspec(-log_start_date => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef]); + $self->configspec(-protocol => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef]); + $self->configspec(-listbox => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef]); + $self->configspec(-gnuplots => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef]); + # print("in Ploter::populate\n"); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; + $self->configure('-protocol' => undef); + $self->build_gui(); + # print("in Ploter::completeinit\n"); +} + +# +# XML +# +sub parse_messages_xml() { + my $filename = $paparazzi_home."/conf/messages.xml"; + my $p = new XML::Parser (Style => 'Tree') ; + my @msg_xml_tree = $p->parsefile ($filename) ; + print STDOUT "successfully parsed $filename\n"; + return \@msg_xml_tree; +} + +# +# Menus +# +sub build_menu_msg() { + my ($self, $xml_top_node, $menubar) = @_; + my $plot_menu = $menubar->cascade(-label => "~Data"); + while ( defined ( my $element = shift @{ $xml_top_node } )) { + my $child = shift @{ $xml_top_node }; + if ( ref $child ) { + my %attr = %{ shift @{ $child } }; + if ($element eq "protocol") { + print "found protocol\n"; + $self->build_msg_menu($child, $plot_menu); + } + } + } +} + +sub build_msg_menu() { + my ($self, $msg_node, $data_menu) = @_; +# sort @{$msg_node}; + while ( defined ( my $element = shift @{ $msg_node } )) { + my $child = shift @{ $msg_node }; + if ( ref $child ) { + my $attr = shift @{ $child }; + if ($element eq "message") { + my $id = $attr->{id}; +# print "found message $id\n"; + my $msg_menu = $data_menu->cascade(-label => $id); + $self->build_field_commands($child, $id, $msg_menu); + } + } + } +} + +sub build_field_commands() { + my ($self, $fields_node, $msg_name, $msg_menu) = @_; + my $no_field = 0; + while ( defined ( my $element = shift @{ $fields_node } )) { + my $child = shift @{ $fields_node }; + if ( ref $child ) { + my $attr = shift @{ $child } ; + if ($element eq "field") { + my $field_name = $attr->{id}; +# print "found field $field_name\n"; + my $no_field1 = $no_field; + my $file_menu = $msg_menu->command(-label => $field_name, + -command => sub { on_plot($self, $msg_name, $field_name, $no_field1 )}); + $no_field++; + } + } + } +} + +sub build_gui() { + my ($self) = @_; + my $width = 450; + my $height = 300; + my $mw = MainWindow->new; + $mw->geometry(sprintf("%dx%d", $width, $height)); + $mw->title("Paparazzi (gnu)plotter"); + + my $mb = $mw->Menu(); + my $log_menu = $mb->command(-label => "~Log", + -command => sub { on_load($self, $mw)}); + $self->build_menu_msg(@{$self->parse_messages_xml()}, $mb); + $mw->configure(-menu => $mb); + + my $padx = 10; + + my $filename_label = $self->add_label("filename :", \$log_filename, 0, $padx, $mw); + my $date_label = $self->add_label("date :", \$log_date, 1, $padx, $mw); + my $duration_label = $self->add_label("duration :", \$log_duration, 2, $padx, $mw); + + my $time_range_label = $mw->Label( -text => "time range")->pack(-side=>'left'); + $time_range_label->grid (-column=>0, -row=>3, -ipadx=>$padx); + $tr_entry = $mw->Entry(-width => 25); + $tr_entry->grid (-column=>1, -row=>3, -ipadx=>$padx); + $tr_entry->insert(0, $time_range); + + my $button = $mw->Button (-text => "update", + -command => sub { update_time_range($self)}, + ); + $button->grid (-column=>2, -row=>3, -ipadx=>$padx); + + my $listbox = $mw->Listbox(); + $listbox->grid (-column=>0, -columnspan => 3, -row=>4, -ipadx=>$padx); + $listbox->bind('', sub {$self->on_list_clicked($listbox, $mw)}); + $self->configure('-listbox' => $listbox); +} + +sub add_label() { + my ($self, $text, $text_variable, $row, $padx, $mw) = @_; + my $label1 = $mw->Label( -text => $text); + $label1->grid (-column=>0, -row=>$row, -ipadx=>$padx, -sticky => 'e' ); + my $label2 = $mw->Label( -textvariable => $text_variable); + $label2->grid (-column=>1, -row=>$row, -ipadx=>$padx, -sticky => 'w'); + return $label2; +} + +sub on_list_clicked() { + my ($self, $listbox, $mw) = @_; + my $key = $listbox->get('active'); + my $dialog = $mw->DialogBox( -title => "Plot command", + -buttons => [ "Replot", "Cancel" ], + ); + $dialog->add("Label", -text => "Plot command")->pack(); + my $gnuplots = $self->get('-gnuplots'); + my $gnuplot = $gnuplots->{$key}; + my $plot_cmd = $gnuplot->{'plot_cmd'}; + print "plot_cmd $plot_cmd\n"; + my $entry = $dialog->add("Entry", -width => 150)->pack(); + $entry->insert(0,$plot_cmd); + print "selected key $key\n"; + my $answer = $dialog->Show(); + print "selected $answer\n"; + if ($answer eq "Replot") { + my $new_plot_cmd = $entry->get(); + print("new_plot_cmd $new_plot_cmd \n"); + $gnuplot->{'plot_cmd'} = $new_plot_cmd; + my $exp = $gnuplot->{'exp'}; + print "exp $exp\n"; + $exp->send($new_plot_cmd."\n"); + my $timeout = 1; + $exp->expect($timeout); + } +} + +sub update_time_range() { + my ($self) = @_; + my $gnuplots = $self->get('-gnuplots'); + $time_range = $tr_entry->get(); + foreach my $key (keys %{$gnuplots}) { + print "update_range_for_key $key ($gnuplots->{$key})\n"; + my $gnuplot = $gnuplots->{$key}; + my $plot_cmd = $gnuplot->{'plot_cmd'}; + my $exp = $gnuplot->{'exp'}; + print "plot_cmd $time_range [$plot_cmd]\n"; + $plot_cmd =~ s/\[.*\]/$time_range/; + print "new_plot_cmd $plot_cmd\n\n"; + $gnuplot->{'plot_cmd'} = $plot_cmd; + $exp->send($plot_cmd."\n"); + my $timeout = 1; + $exp->expect($timeout); + } +} + +sub on_load() { + my ($self, $mw) = @_; + my $fs = $mw->FileSelect(-directory => $paparazzi_home."/var"); + my $file_name = $fs->Show(); + if (defined $file_name) { + print "file_name: $file_name\n"; + $self->load_log($file_name); + } +} + +sub on_plot() { + my ($self, $msg_name, $field_name, $field_pos) = @_; + # print "in on_plot msg_name $msg_name field_name $field_name field_pos $field_pos\n"; + + my $key = $msg_name.".".$field_name.".".$field_pos; + $self->gen_data_file($msg_name); + $self->add_plot($key); +} + + + +sub load_log() { + my ($self, $filename) = @_; + $log_filename = $filename; + my $nb_lines = 0; + open(INFILE, $filename) or die print STDERR "Cant open $filename: $!"; + my $log = $self->get('-log'); + $log_date = undef; + my $line; + while ($line = ) { # assigns each line in turn to $_ + if ($line =~ /(^\d+\.\d+) (\w+) (.+)/) { + $log_date = $1 unless defined $log_date; + my $rel_date = $1 - $log_date; + push (@{$log}, { date=>$rel_date, type=>$2, args=>$3}); + $nb_lines++; + } + } + close INFILE; + $self->configure('-log' => $log); + $self->configure( '-log_start_date' => $log_date); + $log_duration = "aaa"; + + + print STDERR "read $nb_lines lines\n" +} + + + +sub add_plot() { + my ($self, $data_key) = @_; + + $data_key =~ /([^\.]+).([^\.]+).([^\.]+)/ or return; + my ($msg_name, $field_name, $field_pos) = ($1, $2, $3); + + my $gnuplots = $self->get('-gnuplots'); + + my $exp = new Expect(); + $exp->raw_pty(1); + + my $rpos = $field_pos + 3; + my $nb_plots = scalar(keys(%{$gnuplots})); + my $h = $nb_plots * 240; + my $plot_cmd = "plot $time_range \"/tmp/plot_data.$msg_name\" using 1:$rpos t \"$field_name\" w l"; + my $pid = $exp->spawn("/usr/bin/gnuplot", ("-geometry","1600x200+0+$h" )); + my $gnuplot = { 'plot_cmd' => $plot_cmd, + 'exp' => $exp + }; + $gnuplots->{$data_key} = $gnuplot; + $self->configure('-gnuplots' => $gnuplots); + + $exp->send($plot_cmd."\n"); + my $timeout = 1; + $exp->expect($timeout); + + my $listbox = $self->get('-listbox'); + $listbox->insert('end', "$data_key"); + +} + +sub remove_plot() { + my ($self, $data_key) = @_; + my $gnuplots = $self->get('-gnuplots'); + my $gnuplot = $gnuplots->{$data_key}; + my $exp = $gnuplot->{'exp'}; + $exp->soft_close(); + $gnuplots->{$data_key} = undef; + $self->configure('-gnuplots' => $gnuplots); + my $listbox = $self->get('-listbox'); +# my $idx = $listbox->index($key); +# $listbox->delete($idx); +} + + +sub gen_data_file() { + my ($self, $msg_name) = @_; + my $nb_msgs = 0; + my $tmp_file = "/tmp/plot_data.$msg_name"; + open(OUTFILE, ">".$tmp_file) or die "Can t open $tmp_file: $!"; + foreach (@{$self->get('-log')}) { +# print "$_->{type} eq $msg_name \n"; + if ($_->{type} eq $msg_name) { + print OUTFILE "$_->{date} $_->{type} $_->{args}\n"; + $nb_msgs++; + } + } + close OUTFILE; + print STDERR "$nb_msgs $msg_name msgs\n"; +} + +sub catchSigTerm() { + my ($self) = @_; + printf("in catchSigTerm\n"); + my %gnuplots = $self->get('-gnuplots'); + foreach my $key (keys %gnuplots) { + print ("killing $key (%gnuplots{$key}->{'pid'})\n"); + $self->kill_gnuplot($key); + } +} + + + +$SIG{TERM} = \&catchSigTerm ; +#$SIG{KILL} = \&catchSigTerm ; +my $ploter = Ploter->new(); +#$ploter->load_log("../../var/log_04_07_01__13_06_33"); +Tk::MainLoop(); +$ploter->catchSigTerm(); +printf STDOUT "ploter over\n"; + +1; diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile new file mode 100644 index 0000000000..d60c602640 --- /dev/null +++ b/sw/simulator/Makefile @@ -0,0 +1,123 @@ +# Paparazzi simulator $Id$ +# +# Copied from autopilot (autopilot.sf.net) thanx alot Trammell +# +# Copyright (C) 2003 Trammell Hudson +# 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. + +include ../../conf/Makefile.local + +ACDIR= $(PAPARAZZI_HOME)/var/$(AIRCRAFT) +OBJDIR= $(ACDIR)/sim + +SIMHML = stdlib.ml types.ml data.ml flightModel.ml sirf.ml gps.ml hitl.ml sim.ml +SIMHCMO=$(SIMHML:%.ml=%.cmo) +SIMSML = stdlib.ml data.ml flightModel.ml gps.ml sitl.ml sim.ml +SIMSCMO=$(SIMSML:%.ml=%.cmo) +SIMSCMX=$(SIMSML:%.ml=%.cmx) +SIMSC = sim_ir.c sim_gps.c sim_ap.c estimator.c pid.c nav.c main.c +SIMSO=$(SIMSC:%.c=$(OBJDIR)/%.o) + +OCAMLC = ocamlc +OCAMLOPT=ocamlopt -p +INCLUDES= -I +lablgtk2 -I $(SIMDIR)/../lib/ocaml +OCAMLCC = gcc -O2 -I /usr/include/glib-2.0 -I /usr/lib/glib-2.0/include -DUBX -DCTL_BRD_V1_2 -I $(OBJDIR) -I $(ACDIR) + +FBW = ../airborne/fly_by_wire +AP = ../airborne/autopilot +VARINCLUDE=$(PAPARAZZI_HOME)/var/include +ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) + +MESSAGES = ../../conf/messages.xml +GEN_DOWNLINK = ./gen_downlink.out + +SIMDIR=$(shell echo `pwd`) + + + +#all : simhitl.out sitl.cma $(GEN_DOWNLINK) +all : sitl.cma $(GEN_DOWNLINK) + +sim_sitl : $(OBJDIR)/simsitl.out + +simhitl.out : $(SIMHCMO) simhitl.cmo + $(OCAMLC) $(INCLUDES) -o $@ str.cma xml-light.cma unix.cma lib.cma lablgtk.cma gtkInit.cmo $^ + +sitl.cma : $(SIMSCMO) + ocamlc -o $@ -a $^ + +sitl.cmxa : $(SIMSCMX) + ocamlopt -o $@ -a $^ + +$(OBJDIR)/simsitl.out : sitl.cma $(SIMSO) $(OBJDIR)/simsitl.cmo + $(OCAMLC) $(INCLUDES) -custom -o $@ glibivy-ocaml.cma xml-light.cma unix.cma lib.cma lablgtk.cma gtkInit.cmo $(SIMSO) sitl.cma $(OBJDIR)/simsitl.cmo + +$(OBJDIR)/simsitl.opt : $(SIMSO) $(OBJDIR)/simsitl.cmx + $(OCAMLOPT) $(INCLUDES) -o $@ str.cmxa glibivy-ocaml.cmxa xml-light.cmxa unix.cmxa lib.cmxa lablgtk.cmxa gtkInit.cmx $(SIMSO) sitl.cmxa $(OBJDIR)/simsitl.cmx + +$(OBJDIR)/%.o : %.c + $(OCAMLCC) -c -o $@ -I $(SIMDIR) -I $(FBW) -I $(AP) -I ../include -I $(VARINCLUDE) $< + +$(OBJDIR)/%.o : $(AP)/%.c + $(OCAMLCC) -c -o $@ -I $(SIMDIR) -I $(FBW) -I $(AP) -I ../include -I $(VARINCLUDE) $< + +$(OBJDIR)/main.o : $(OBJDIR)/main.c + $(OCAMLCC) -c -o $@ -I $(SIMDIR) -I $(FBW) -I $(AP) -I ../include -I $(VARINCLUDE) $< + +sim_gps.o nav.o main.o sim_ir.o sim_ap.o pid.o estimator.o : $(ACINCLUDE)/flight_plan.h $(ACINCLUDE)/airframe.h + +$(OBJDIR)/main.c : $(OBJDIR)/downlink.h + cp $(AP)/main.c $(@) + +$(OBJDIR)/downlink.h : $(MESSAGES) $(GEN_DOWNLINK) + $(GEN_DOWNLINK) $< > $@ + +$(GEN_DOWNLINK) : gen_downlink.ml + $(OCAMLC) $(INCLUDES) -o $@ str.cma xml-light.cma lib.cma $< + + +$(OBJDIR)/simsitl.cmo : $(OBJDIR)/simsitl.ml + $(OCAMLC) $(INCLUDES) -c -o $@ $< + +$(OBJDIR)/simsitl.cmx : $(OBJDIR)/simsitl.ml + $(OCAMLOPT) $(INCLUDES) -c -o $@ $< + +$(OBJDIR)/simsitl.ml : simsitl.ml + echo "Sim.ac_name := \"$(AIRCRAFT)\"" > $@ + cat $< >> $@ + +%.cmo : %.ml + $(OCAMLC) $(INCLUDES) -c $< + +%.cmx : %.ml + $(OCAMLOPT) $(INCLUDES) -c $< + +%.cmi : %.mli + $(OCAMLC) $(INCLUDES) -c $< + +clean : + \rm -f *.cm* *~ *.out .depend *.o + +.depend: + ocamldep *.ml* > .depend + +ifneq ($(MAKECMDGOALS),clean) +-include .depend +endif diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml new file mode 100644 index 0000000000..8e4de393e8 --- /dev/null +++ b/sw/simulator/data.ml @@ -0,0 +1,46 @@ +let (//) = Filename.concat + +(* let pprz_conf_path = Env.paparazzi_src // "conf" *) +let user_conf_path = Env.paparazzi_home // "conf" + +let conf_xml = Xml.parse_file (user_conf_path // "conf.xml") +let ground = ExtXml.child conf_xml "ground" + +let messages_ap = +(* let xml = Xml.parse_file (pprz_conf_path // "messages.xml") in *) + let xml = Xml.parse_file (user_conf_path // "messages.xml") in + try + ExtXml.child xml ~select:(fun x -> Xml.attrib x "name" = "telemetry_ap") "class" + with + Not_found -> failwith "'telemetry_ap' class missing in messages.xml" + +(* let ubx_xml = Xml.parse_file (pprz_conf_path // "ubx.xml") *) +let ubx_xml = Xml.parse_file (user_conf_path // "ubx.xml") + +type aircraft = { + name : string; + id : int; + airframe : Xml.xml; + flight_plan : Xml.xml; + radio: Xml.xml + } + + +let aircraft = fun name -> + let aircraft_xml, id = + let rec loop i = function + [] -> failwith ("Aicraft not found : "^name) + | x::_ when Xml.tag x = "aircraft" && Xml.attrib x "name" = name -> + (x, i) + | x::xs -> loop (i+1) xs in + loop 0 (Xml.children conf_xml) in + + let airframe_file = user_conf_path // ExtXml.attrib aircraft_xml "airframe" in + + { id = id; name = name; + airframe = Xml.parse_file airframe_file; + flight_plan = Xml.parse_file (user_conf_path // ExtXml.attrib aircraft_xml "flight_plan"); + radio = Xml.parse_file (user_conf_path // ExtXml.attrib aircraft_xml "radio") + } + +module type MISSION = sig val ac : aircraft end diff --git a/sw/simulator/events.ml b/sw/simulator/events.ml new file mode 100644 index 0000000000..dca6d0aba8 --- /dev/null +++ b/sw/simulator/events.ml @@ -0,0 +1,105 @@ +(* + * $Id$ + * + * High-level events handling + * + * Copyright (C) 2004 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. + * + *) + +type callback = unit -> unit +type time = float (* Unix time *) +type period = float (* Seconds *) + +type fd = Unix.file_descr + +type timer = { + mutable next_wake : time; + period : period; + cb : callback + } + + +type on_input_id = Unix.file_descr +let dummy_on_input_id = Unix.stdin +let on_fds = Hashtbl.create 11 +let register_on_input fd cb = + Hashtbl.add on_fds fd cb; fd +let remove_on_input fd = + Hashtbl.remove on_fds fd + +type timer_id = callback +let timers = ref [] (* Is a priority queue really necessary ? *) +let register_timer period cb = + timers := { next_wake = Unix.gettimeofday () +. period; period = period; cb = cb } :: !timers; cb +let remove_timer cb = + let rec loop = function + [] -> [] + | t::ts -> if t.cb == cb then ts else t :: loop ts in + timers := loop !timers + + +let get_input_fds () = + let l = ref [] in + Hashtbl.iter (fun fd _ -> l := fd :: !l) on_fds; + !l +let get_fd_callbacks fds = + List.map (Hashtbl.find on_fds) fds + +(** Returns next timer timeout and callback. May return a dummy callback + if no timers are set. Wrap the update of the selected timer in the + callback *) +let never = 2e9 +let get_next_timeout () = + let rec loop earlier_wake earlier_cb = function + [] -> (earlier_wake, earlier_cb) + | timer :: es -> + if timer.next_wake < earlier_wake then + let t = timer.next_wake in + loop t (fun () -> timer.next_wake <- t +. timer.period; timer.cb ()) es + else + loop earlier_wake earlier_cb es in + loop never (fun () -> ()) !timers + + + + + + + + +let mainloop () = + while true do + let (next_timeout, timeout_cb) = get_next_timeout () in + let timeout = next_timeout -. Unix.gettimeofday () in + if timeout <= 0. then + timeout_cb () + else + let input_fds = get_input_fds () in + let (ready_inputs, _, _) = Unix.select input_fds [] [] timeout in + + match ready_inputs with + [] -> timeout_cb () + | _ -> + let fd_callbacks = get_fd_callbacks ready_inputs in + List.iter (fun cb -> cb ()) fd_callbacks + done + + diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml new file mode 100644 index 0000000000..cf07c2a85b --- /dev/null +++ b/sw/simulator/flightModel.ml @@ -0,0 +1,157 @@ +open Stdlib + +type meter = float +type meter_s = float +type radian = float +type radian_s = float +type state = { + start : float; + mutable t : float; + mutable x : meter; + mutable y : meter; + mutable z : meter; + mutable psi : radian; (* Trigonometric *) + mutable phi : radian; + mutable phi_dot : radian_s; + mutable delta_a : float; + mutable thrust : float; + mutable air_speed : meter_s + } + +let init route = { + start = Unix.gettimeofday (); t = 0.; x = 0.; y = 0. ; z = 0.; + psi = route; phi = 0.; phi_dot = 0.; + delta_a = 0.; thrust = 0.; air_speed = 0. +} + +let get_xyz state = (state.x, state.y, state.z) +let get_time state = state.t +let get_phi state = state.phi + +let set_air_speed state x = state.air_speed <- x + +let drag = 0.45 +let c_lp = -10. +let g = 9.81 +let weight = 1. *. 1.4 + +let max_phi = 0.7 (* rad *) +let bound = fun x mi ma -> if x > ma then ma else if x < mi then mi else x + + + +module Make(A:Data.MISSION) = struct + open Data +(* Minimum complexity *) +(* + http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf + http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf + *) + + let state_update = fun state (wx, wy) -> + let now = Unix.gettimeofday () -. state.start in + let dt = now -. state.t in + if state.air_speed > 0. then begin + let phi_dot_dot = state.delta_a +. c_lp *. state.phi_dot /. state.air_speed in + state.phi_dot <- state.phi_dot +. phi_dot_dot *. dt; + state.phi <- bound (state.phi +. state.phi_dot *. dt) (-.max_phi) max_phi; + let psi_dot = -. g /. state.air_speed *. tan state.phi in + state.psi <- norm_angle (state.psi +. psi_dot *. dt); + let dx = state.air_speed *. cos state.psi *. dt +. wx *. dt + and dy = state.air_speed *. sin state.psi *. dt +. wy *. dt in + state.x <- state.x +.dx ; + state.y <- state.y +. dy; + let gamma = (state.thrust -. drag) /. weight in + let dz = sin gamma *. state.air_speed *. dt in + state.z <- state.z +. dz + end; + state.t <- now + + + let servos = + try + ExtXml.child A.ac.airframe "servos" + with + Not_found -> + failwith (Printf.sprintf "Child 'servos' expected in '%s'\n" (Xml.to_string A.ac.airframe)) + + let misc_section = + try + ExtXml.child A.ac.airframe ~select:(fun x -> ExtXml.attrib x "name" = "MISC") "section" + with + Not_found -> + failwith (Printf.sprintf "Child 'section' with 'name=MISC' expected in '%s'\n" (Xml.to_string A.ac.airframe)) + + let nominal_airspeed = + try + float_of_string (Xml.attrib (ExtXml.child misc_section ~select:(fun x -> ExtXml.attrib x "name" = "NOMINAL_AIRSPEED") "define") "value") + with + Not_found -> + failwith (Printf.sprintf "Child 'define' with 'name=NOMINAL_AIRSPEED' expected in '%s'\n" (Xml.to_string misc_section)) + + let get_servo name = + try + ExtXml.child servos ~select:(fun x -> ExtXml.attrib x "name" = name) "servo" + with + Not_found -> + failwith (Printf.sprintf "Child 'servo' with name='%s' expected in '%s'\n" name (Xml.to_string servos)) + + let us_attrib = fun x a -> int_of_string (ExtXml.attrib x a) + + let gaz = get_servo "GAZ" + let min_thrust = us_attrib gaz "min" + let max_thrust = us_attrib gaz "max" + + type servo_id = int + type ppm = int + + let no_thrust = int_of_string (ExtXml.attrib gaz "no") + + + + let some_aileron_left = try Some (get_servo "AILERON_LEFT") with _ -> None + let some_ailevon_left = try Some (get_servo "AILEVON_LEFT") with _ -> None + let some_ailevon_right = try Some (get_servo "AILEVON_RIGHT") with _ -> None + + let float_attrib = fun x a -> float_of_string (ExtXml.attrib x a) + let int_attrib = fun x a -> int_of_string (ExtXml.attrib x a) + + let sign = fun x -> + if float_attrib x "min" < float_attrib x "max" then 1 else -1 + + let do_thrust = fun state servo -> + state.thrust <- (float (servo.(no_thrust) - min_thrust) /. float (max_thrust - min_thrust)) + + let do_servos = + match some_aileron_left, some_ailevon_left, some_ailevon_right with + Some aileron_left, None, None -> + let c_lda = 16. *. 9e-5 in (* phi_dot_dot from aileron *) + + let sign_aileron_left = sign aileron_left + and n_delta_a = us_attrib aileron_left "neutral" + and no_aileron_left = int_attrib aileron_left "no" in + fun state servo -> + (** Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout; **) + state.delta_a <- c_lda *. float (- sign_aileron_left * (servo.(no_aileron_left) - n_delta_a)); + do_thrust state servo + | None, Some ailevon_left, Some ailevon_right -> + let c_lda = 2.5e-4 in (* phi_dot_dot from aileron *) + + let sign_ailevon_left = sign ailevon_left + and sign_ailevon_right = sign ailevon_right + and left_neutral = us_attrib ailevon_left "neutral" + and right_neutral = us_attrib ailevon_right "neutral" + and left_travel = float (us_attrib ailevon_left "max" - us_attrib ailevon_left "min") /. 1200. + and right_travel = float (us_attrib ailevon_right "max" - us_attrib ailevon_right "min") /. 1200. + and no_ailevon_left = int_attrib ailevon_left "no" + and no_ailevon_right = int_attrib ailevon_right "no" in + fun state servo -> + do_thrust state servo; + let sum = (float (servo.(no_ailevon_left) - left_neutral) /. left_travel +. + float (servo.(no_ailevon_right) - right_neutral) /. right_travel) /. 2. in +(* Printf.printf "%d %f\n" (servo no_ailevon_left - left_neutral) sum; flush stdout; *) + state.delta_a <- c_lda *. (-. sum) + | _ -> failwith "Aileron or Ailevon left and right PLEASE" + + let nb_servos = 10 (* 4017 *) +end diff --git a/sw/simulator/flightModel.mli b/sw/simulator/flightModel.mli new file mode 100644 index 0000000000..ef32dc9e6b --- /dev/null +++ b/sw/simulator/flightModel.mli @@ -0,0 +1,22 @@ +type meter = float +type meter_s = float +type radian = float +type radian_s = float +type state + +val init : radian -> state + +val get_xyz : state -> meter * meter * meter +val get_time : state -> float +val get_phi : state -> radian + +val set_air_speed : state -> meter_s -> unit + +module Make : + functor (A : Data.MISSION) -> + sig + val do_servos : state -> Stdlib.us array -> unit + val nb_servos : int + val nominal_airspeed : float (* m/s *) + val state_update : state -> float * float -> unit + end diff --git a/sw/simulator/gen_downlink.ml b/sw/simulator/gen_downlink.ml new file mode 100644 index 0000000000..796e324610 --- /dev/null +++ b/sw/simulator/gen_downlink.ml @@ -0,0 +1,92 @@ +open Printf + +let h_name = "DOWNLINK_H" + +let id_of = fun xml -> ExtXml.attrib xml "name" + +(** No dereferencement for arrays *) +let deref = fun xml -> try let _ = Xml.attrib xml "len" in "" with _ -> "*" + +let print_params = function + [] -> () + | f::fields -> + printf "%s" (id_of f); + List.iter (fun f -> printf ", %s" (id_of f)) fields + +let types = [ + "uint8", "%hhu"; + "uint16", "%hu"; + "uint32", "%u" ; + "int8", "%hhd"; + "int16", "%hd"; + "int32", "%d" ; + "float", "%f" +] + +let sprint_format = fun f -> + try + Xml.attrib f "format" + with _ -> + List.assoc (Xml.attrib f "type") types + + +let freq = 10 +let buffer_length = 5 +let step = 1. /. float freq +let nb_steps = (256 / freq) * freq + +let is_periodic = fun m -> try let _ = Xml.attrib m "period" in true with _ -> false +let period_of = fun m -> float_of_string (Xml.attrib m "period") + +let gen_periodic = fun avr_h messages -> + let periodic_messages = List.filter is_periodic messages in + + let scheduled_messages = + List.map + (fun m -> + let p = period_of m in + let period_steps = truncate (p /. step) in + (period_steps, id_of m)) + periodic_messages in + + fprintf avr_h "#define PeriodicSend() { /* %dHz */ \\\n" freq; + fprintf avr_h " static uint8_t i;\\\n"; + fprintf avr_h " i++; if (i == %d) i = 0;\\\n" nb_steps; + List.iter + (fun (p, id) -> + fprintf avr_h " if (i %% %d == 0) PERIODIC_SEND_%s();\\\n" p id) + scheduled_messages; + fprintf avr_h "}\n" + + + +let fprint_formats = fun c fields -> + List.iter (fun f -> fprintf c " %s" (sprint_format f)) fields + +let fprint_args = fun c fields -> + List.iter (fun f -> fprintf c ", %s(%s)" (deref f) (id_of f)) fields + +let one_message = fun m -> + let id = id_of m + and fields = Xml.children m in + printf "#define DOWNLINK_SEND_%s(" id; + print_params fields; + printf "){ \\\n"; + printf " IvySendMsg(\"%%d %s %a\",ac_id%a); \\\n" id fprint_formats fields fprint_args fields; + printf "}\n\n" + +let _ = + let xml = Xml2h.start_and_begin Sys.argv.(1) h_name in + let xml = ExtXml.child xml ~select:(fun x -> Xml.attrib x "name"="telemetry_ap") "class" in + let messages = (Xml.children xml) in + + printf "#include \n"; + printf "extern uint8_t ac_id;\n"; + printf "extern uint8_t modem_nb_ovrn;\n"; + + List.iter one_message messages; + + gen_periodic stdout messages; + + Xml2h.finish h_name + diff --git a/sw/simulator/gps.ml b/sw/simulator/gps.ml new file mode 100644 index 0000000000..1764c1cda0 --- /dev/null +++ b/sw/simulator/gps.ml @@ -0,0 +1,48 @@ +open Stdlib +open Latlong + +type state = { + wgs84 : Latlong.geographic; + alt : float; + time : float; + climb : float; + gspeed : float; + course : float + } + + +let earth_radius = 6378388. + + +let state = fun lat0 lon0 alt0 -> + let last_x = ref 0. and last_y = ref 0. + and last_t = ref 0. and last_z = ref 0. in + + fun (x, y, z) t -> + let dx = x -. !last_x + and dy = y -. !last_y + and dt = t -. !last_t in + let gspeed = sqrt (dx*.dx +. dy*.dy) /. dt + and course = norm_angle (pi/.2. -. atan2 dy dx) + and climb = (z -. !last_z) /. dt in + + let lat = lat0 +. y /. earth_radius + and long = lon0 +. x /.earth_radius /. cos lat0 + and alt = alt0 +. z in + + last_x := x; + last_y := y; + last_z := z; + last_t := t; + + let course = if course < 0. then course +. 2. *. pi else course in (* ???? *) + + { + wgs84 = { posn_lat=lat;posn_long=long }; + alt = alt0 +. z; + time = t; + climb = climb; + gspeed = gspeed; + course = course + } + diff --git a/sw/simulator/gui.ml b/sw/simulator/gui.ml new file mode 100644 index 0000000000..4b51574c0b --- /dev/null +++ b/sw/simulator/gui.ml @@ -0,0 +1,49 @@ +(* ocamlc -w a -I +lablgtk2 lablgtk.cma lablgnomecanvas.cma gtkInit.cmo gui.ml -o gui.out *) + +let main () = + + (* window *) + let window = GWindow.window ~title: "Paparazzi simulator" + ~border_width: 5 ~width: 400 ~height: 200 () in + ignore (window#connect#destroy ~callback:GMain.quit); + + + let hb = GPack.hbox ~border_width:5 ~spacing:5 ~packing:window#add () in + + + (* wind *) + let frame_w = GBin.frame ~label:"Wind" ~shadow_type:`IN ~packing:hb#pack () in + + let vb_w = GPack.vbox ~packing:frame_w#add () in + + let adj_d = + GData.adjustment ~lower:0. ~upper:100. ~step_incr:1. ~page_incr:10. () in + let sc_d = GRange.scale `HORIZONTAL ~adjustment:adj_d ~draw_value:false + ~packing:vb_w#pack () in + + let adj_s = + GData.adjustment ~lower:0. ~upper:100. ~step_incr:1. ~page_incr:10. () in + let sc_s = GRange.scale `HORIZONTAL ~adjustment:adj_s ~draw_value:false + ~packing:vb_w#pack () in + + (* infrared *) + let frame_i = GBin.frame ~label:"Infrared" ~shadow_type:`IN ~packing:hb#pack () in + + let vb_i = GPack.vbox ~packing:frame_i#add () in + + let adj_i = + GData.adjustment ~lower:0. ~upper:100. ~step_incr:1. ~page_incr:10. () in + let sc_i = GRange.scale `HORIZONTAL ~adjustment:adj_i ~draw_value:false + ~packing:vb_i#pack () in + + let button = GButton.button ~use_mnemonic:true ~label:"_Coucou" ~packing:(hb#pack ~padding:5) () in + ignore(button#connect#clicked ~callback: + (fun () -> prerr_endline "Coucou")); + + + + window#show (); + GMain.Main.main () + +let _ = main () + diff --git a/sw/simulator/hitl.ml b/sw/simulator/hitl.ml new file mode 100644 index 0000000000..76e6cac2cf --- /dev/null +++ b/sw/simulator/hitl.ml @@ -0,0 +1,142 @@ +(* + * $Id$ + * + * Hardware In The Loop + * + * Copyright (C) 2004 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 Stdlib +open Latlong + +let get_port = fun n -> + Xml.attrib (ExtXml.child ~select:(fun x -> Xml.attrib x "name"=n) Data.ground "link") "port" + +let tty0 = ref (get_port "ap") +let tty1 = ref (get_port "fbw") + +let uart_mcu0 = ref Unix.stdout +let uart_mcu1 = ref Unix.stdin + +let open_mcu tty = Serial.opendev tty Serial.B38400 + +module Make(A:Data.MISSION) = struct + + let init = fun (_:int) (_:GPack.box) -> + if !tty0 <> "" then uart_mcu0 := open_mcu !tty0; + if !tty1 <> "" then uart_mcu1 := open_mcu !tty1 + + let boot = fun () -> () + + + let scale = fun value s -> truncate (value *. s) + + open Gps + + let gps = fun gps -> + let uart = Unix.out_channel_of_descr !uart_mcu0 in + let utm = utm_of WGS84 gps.wgs84 in + Ubx.send uart Ubx.nav_posutm + ["EAST", scale utm.utm_x 1e2; + "NORTH", scale utm.utm_y 1e2; + "ALT", scale gps.alt 1e2]; + Ubx.send uart Ubx.nav_status ["GPSfix", 3]; + Ubx.send uart Ubx.nav_velned + ["ITOW",scale gps.time 1e3; + "VEL_D", -scale gps.climb 1e2; + "GSpeed", scale gps.gspeed 1e2; + "Heading", scale (deg gps.course) 1e5] + + + + let irs = + try + ExtXml.child A.ac.Data.airframe + ~select:(fun x -> try Xml.attrib x "prefix" = "IR_" with Xml.No_attribute _ -> false) + "section" + with Not_found -> + failwith "Do not find an IR section in airframe description" + let ir_roll_neutral = + try + float_of_string (ExtXml.attrib (ExtXml.child irs ~select:(fun x -> try Xml.attrib x "name" = "ROLL_NEUTRAL_DEFAULT" with Xml.No_attribute _ -> false) "define") "value") + with + Not_found -> + failwith "Do not find an ROLL_NEUTRAL_DEFAULT define in IR description" + + let ir_pitch_neutral = + try + float_of_string (ExtXml.attrib (ExtXml.child irs ~select:(fun x -> try Xml.attrib x "name" = "PITCH_NEUTRAL_DEFAULT" with Xml.No_attribute _ -> false) "define") "value") + with + Not_found -> + failwith "Do not find an PITCH_NEUTRAL_DEFAULT define in IR description" + + let infrared = fun phi ctrst -> + let uart = Unix.out_channel_of_descr !uart_mcu0 in + let ir_left = truncate (phi *. ctrst +. ir_roll_neutral) + and ir_front = truncate ir_pitch_neutral in + Ubx.send uart Ubx.usr_irsim + ["ROLL", ir_left; + "PITCH", ir_front] + + let size_servos_buf = 256 + let zero = '\000' + + let get_2bytes = fun buf i -> + (Char.code buf.[i] lsl 8) lor (Char.code buf.[i+1]) + +(* nb_servos 2 bytes values, prefixed by 00 ended by \n + Returns optionaly a function associating the read value to the index *) + let clock = 16 + let read_servos = fun servos -> + let servos_buf = String.create size_servos_buf + and buf_idx = ref 0 in + let nb_servos = Array.length servos in + let tty = Unix.in_channel_of_descr !uart_mcu1 in + + fun () -> + let n = input tty servos_buf !buf_idx (size_servos_buf- !buf_idx) in + let rec parse00 = fun i m -> + if m >= 2+2*nb_servos+1 then + (if servos_buf.[i] = zero then parse0 else parse00) (i+1) (m-1) + else (* Not enough chars : wait *) + i + + and parse0 = fun i m -> + if servos_buf.[i] = zero && servos_buf.[i+2*nb_servos+1] = '\n' then begin + for s = 0 to nb_servos - 1 do + servos.(s) <- get_2bytes servos_buf (i+1+2*s) / clock + done; + i+1+2*nb_servos+1 + end else (* 0 or \n missing *) + parse00 i m in + + let nb_available_chars = (!buf_idx + n) in + let nb_read_chars = parse00 0 nb_available_chars in + let rest = nb_available_chars - nb_read_chars in + String.blit servos_buf nb_read_chars servos_buf 0 rest; + buf_idx := rest + + + + let servos = fun servos -> + ignore (GMain.Io.add_watch [`IN] (fun _ -> read_servos servos (); true) (GMain.Io.channel_of_descr !uart_mcu1)) +end diff --git a/sw/simulator/hitl.mli b/sw/simulator/hitl.mli new file mode 100644 index 0000000000..5ac1b8d776 --- /dev/null +++ b/sw/simulator/hitl.mli @@ -0,0 +1,5 @@ +val tty0 : string ref +val tty1 : string ref + + +module Make : functor (A : Data.MISSION) -> Sim.AIRCRAFT diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml new file mode 100644 index 0000000000..c4276eead5 --- /dev/null +++ b/sw/simulator/sim.ml @@ -0,0 +1,188 @@ +(* + * $Id$ + * + * Hardware in the loop basic simulator (handling GPS, infrared and servos) + * + * Copyright (C) 2004 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 Stdlib +open Geometry_2d + +let float_attrib xml a = float_of_string (ExtXml.attrib xml a) + +let wind = (0., 0.) (* m/s in local ref *) + +(* Frequencies for perdiodic tasks are expressed in periods of 100Hz *) +let timebase = 10 (* ms *) +let ir_period = 5 +let fm_period = 4 + + +module type AIRCRAFT = + sig + val init : int -> GPack.box -> unit + val boot : unit -> unit + val servos : us array -> unit + (** Called once at init *) + + val infrared : float -> float -> unit + (** [infrared phi] Called on timer *) + + val gps : Gps.state -> unit + (** [gps state] Called on timer *) + end + + +module type AIRCRAFT_ITL = functor (A : Data.MISSION) -> AIRCRAFT + + +let ac_name = ref "" + + +let common_options = [] + +module Make(AircraftItl : AIRCRAFT_ITL) = struct + + module A = struct + let ac = Data.aircraft !ac_name + end + + module Aircraft = AircraftItl(A) + + module FM = FlightModel.Make(A) + + let flight_plan = A.ac.Data.flight_plan + + let lat0 = (float_attrib flight_plan "lat0") + let lon0 = (float_attrib flight_plan "lon0") + let qfu = (float_attrib flight_plan "qfu") + let alt0 = (float_attrib flight_plan "ground_alt") +(* + let gust_dir = 0 and + let gust_speed = 0 + + let wind = fun -> + let wind_dir = wind_dir_adj#value in + let wind_speed = wind_speed_adj#value in + let gust_max = gust_max_adj#value in + let gust_dir_fact = gust_dir_fact_adj#value in + let gust_speed_fact = gust_dir_fact_adj#value in + gust_speed = trim 0, gust_max (gust_speed + Random.float gust_dir_fact) + gust_dir = gust_dirspeed + Random.float gu) + +*) + let main () = + let window = GWindow.dialog ~title:("Aircraft "^ !ac_name) () in + let quit = fun () -> GMain.Main.quit (); exit 0 in + ignore (window#connect#destroy ~callback:quit); + + Aircraft.init A.ac.Data.id window#vbox; + + let gps_period = 25 in + + let compute_gps_state = Gps.state (rad_of_deg lat0) (rad_of_deg lon0) (alt0) in + + + let initial_state = FlightModel.init (pi/.2. -. qfu/.180.*.pi) in + + let state = ref initial_state in + + let reset = fun () -> state := initial_state in + + let servos = Array.create FM.nb_servos 0 in + + Aircraft.servos servos; + + let north_label = GMisc.label ~text:"000" () + and east_label = GMisc.label ~text:"000" () + and alt_label = GMisc.label ~text:"000" () in + let wind_dir_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:370. ~step_incr:1.0 () in + let wind_speed_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in + let gust_norm_max_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in + let gust_norm_ch_fact_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in + let gust_dir_ch_fact_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in + let infrared_contrast_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:1010. ~step_incr:10. () in + + let run = ref false in + let scheduler = + let t = ref 0 in + let f = + fun () -> + incr t; + if !t mod fm_period = 0 then begin + FM.do_servos !state servos; + let wind_dir_rad = deg2rad wind_dir_adj#value in + let wind_angle_rad = heading_of_to_angle_rad wind_dir_rad in + let wind_speed_polar = {r2D = wind_speed_adj#value; theta2D = oposite_heading_rad wind_angle_rad} in + let wind_speed_cart = polar2cart wind_speed_polar in + FM.state_update !state ( wind_speed_cart.x2D, wind_speed_cart.y2D) + end; + if !t mod ir_period = 0 then + Aircraft.infrared (FlightModel.get_phi !state) infrared_contrast_adj#value; + if !t mod gps_period = 0 then begin + let (x,y,z) = FlightModel.get_xyz !state in + east_label#set_text (Printf.sprintf "%.0f" x); + north_label#set_text (Printf.sprintf "%.0f" y); + alt_label#set_text (Printf.sprintf "%.0f" z); + Aircraft.gps (compute_gps_state (x,y,z) (FlightModel.get_time !state)) + end; + true in + fun () -> ignore (GMain.Timeout.add 10 f) in + + let boot = fun () -> + Aircraft.boot (); + scheduler () in + + let take_off = fun () -> prerr_endline "takeoff"; FlightModel.set_air_speed !state FM.nominal_airspeed in + + let hbox = GPack.hbox ~packing:window#vbox#pack () in + let s = GButton.button ~label:"Boot" ~packing:(hbox#pack ~padding:5) () in + ignore (s#connect#clicked ~callback:boot); + let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in + ignore (t#connect#clicked ~callback:take_off); + + let hbox = GPack.hbox ~packing:window#vbox#pack () in + let l = fun s -> ignore(GMisc.label ~text:s ~packing:hbox#pack ()) in + l "East:"; hbox#pack east_label#coerce; + l " North:"; hbox#pack north_label#coerce; + l " Height:"; hbox#pack alt_label#coerce; + + let hbox = GPack.hbox ~packing:window#vbox#pack () in + ignore (GMisc.label ~text:"wind dir:" ~packing:hbox#pack ()); + ignore (GRange.scale `HORIZONTAL ~adjustment:wind_dir_adj ~packing:hbox#add ()); + + let hbox = GPack.hbox ~packing:window#vbox#pack () in + ignore (GMisc.label ~text:"wind speed:" ~packing:hbox#pack ()); + ignore (GRange.scale `HORIZONTAL ~adjustment:wind_speed_adj ~packing:hbox#add ()); + + let hbox = GPack.hbox ~packing:window#vbox#pack () in + ignore (GMisc.label ~text:"gust max speed:" ~packing:hbox#pack ()); + ignore (GRange.scale `HORIZONTAL ~adjustment:gust_norm_max_adj ~packing:hbox#add ()); + + + let hbox = GPack.hbox ~packing:window#vbox#pack () in + ignore (GMisc.label ~text:"infrared:" ~packing:hbox#pack ()); + ignore (GRange.scale `HORIZONTAL ~adjustment:infrared_contrast_adj ~packing:hbox#add ()); + + window#show (); + Unix.handle_unix_error GMain.Main.main () +end diff --git a/sw/simulator/sim.mli b/sw/simulator/sim.mli new file mode 100644 index 0000000000..8d323cdec5 --- /dev/null +++ b/sw/simulator/sim.mli @@ -0,0 +1,24 @@ +(** Options for HITL and SITL simulators *) +val common_options : (string * Arg.spec * string) list + +val ac_name : string ref + +(** A complete aircraft with it mission *) +module type AIRCRAFT = + sig + val init : int -> GPack.box -> unit + val boot : unit -> unit + val servos : Stdlib.us array -> unit + val infrared : float -> float -> unit + val gps : Gps.state -> unit + end + +(** A simulated aircraft, without its conf *) +module type AIRCRAFT_ITL = functor (A : Data.MISSION) -> AIRCRAFT + +(** Functor to build the simulator *) +module Make : + functor (AircraftItl : AIRCRAFT_ITL) -> + sig + val main : unit -> unit + end diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c new file mode 100644 index 0000000000..e7a175088d --- /dev/null +++ b/sw/simulator/sim_ap.c @@ -0,0 +1,93 @@ +/* Definitions and declarations required to compile autopilot code on a + i386 architecture. Binding for OCaml. */ + +#include +#include +#include +#include "std.h" +#include "link_autopilot.h" +#include "autopilot.h" +#include "estimator.h" + +#include +#include + +uint8_t ir_estim_mode; +uint8_t vertical_mode; +uint8_t inflight_calib_mode; +bool_t rc_event_1, rc_event_2; +bool_t launch; +bool_t link_fbw_receive_valid; +uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err; + +uint8_t ac_id; + +struct inter_mcu_msg from_fbw, to_fbw; + +static int16_t values_from_ap[RADIO_CTL_NB]; + +void inflight_calib(void) { } + +void link_fbw_send(void) { + int i; + for(i = 0; i < RADIO_CTL_NB; i++) + values_from_ap[i] = to_fbw.channels[i] / CLOCK; +} + +value sim_periodic_task(value unit) { + periodic_task(); + return Val_unit; +} + + +value sim_rc_task(value unit) { + from_fbw.status = (1 << STATUS_RADIO_OK) | (1 << AVERAGED_CHANNELS_SENT); + link_fbw_receive_valid = TRUE; + radio_control_task(); + return Val_unit; +} + + +float ftimeofday(void) { + struct timeval t; + struct timezone z; + gettimeofday(&t, &z); + return (t.tv_sec + t.tv_usec/1e6); +} + +value sim_init(value id) { + pprz_mode = PPRZ_MODE_MANUAL; + estimator_init(); + ac_id = Int_val(id); + return Val_unit; +} + +value update_bat(value bat) { + from_fbw.vsupply = Int_val(bat); + return Val_unit; +} + +value update_rc_channel(value c, value v) { + from_fbw.channels[Int_val(c)] = Double_val(v)*MAX_PPRZ; + return Val_unit; +} + +// Defined in servo.c +#define _4017_NB_CHANNELS 10 +#define SERVO_NEUTRAL_(i) SERVOS_NEUTRALS_ ## i +#define SERVO_NEUTRAL(i) (SERVO_NEUTRAL_(i)) +#define SERVO_MIN (SERVO_MIN_US) +#define SERVO_MAX (SERVO_MAX_US) +#define ChopServo(x) ((x) < SERVO_MIN ? SERVO_MIN : ((x) > SERVO_MAX ? SERVO_MAX : (x))) + +value set_servos(value servos) { + int i; + + uint16_t servo_widths[_4017_NB_CHANNELS]; + ServoSet(values_from_ap); + + for(i=0; i < _4017_NB_CHANNELS; i++) + Store_field(servos, i, Val_int(servo_widths[i])); + + return Val_int(servo_widths[SERVO_GAZ]); +} diff --git a/sw/simulator/sim_gps.c b/sw/simulator/sim_gps.c new file mode 100644 index 0000000000..61d1bcbf7e --- /dev/null +++ b/sw/simulator/sim_gps.c @@ -0,0 +1,42 @@ +/* OCaml binding to link the simulator to autopilot functions. */ + +#include +#include "airframe.h" +#include "flight_plan.h" +#include "autopilot.h" + +#include + +uint8_t gps_mode; +float gps_ftow; /* ms */ +float gps_falt; /* m */ +float gps_fspeed; /* m/s */ +float gps_fclimb; /* m/s */ +float gps_fcourse; /* rad */ +int32_t gps_utm_east, gps_utm_north; +float gps_east, gps_north; /* m */ + +const int32_t utm_east0 = NAV_UTM_EAST0; +const int32_t utm_north0 = NAV_UTM_NORTH0; + +value sim_use_gps_pos(value x, value y, value c, value a, value s, value cl, value t) { + gps_mode = 3; + gps_utm_east = Int_val(x); + gps_utm_north = Int_val(y); + gps_fcourse = Double_val(c); + gps_falt = Double_val(a); + gps_fspeed = Double_val(s); + gps_fclimb = Double_val(cl); + gps_ftow = Double_val(t); + + gps_east = gps_utm_east / 100 - NAV_UTM_EAST0; + gps_north = gps_utm_north / 100 - NAV_UTM_NORTH0; + + use_gps_pos(); /* From main.c */ + return Val_unit; +} + +/* Second binding required because number of args > 5 */ +value sim_use_gps_pos_bytecode(value *a, int argn) { + return sim_use_gps_pos(a[0],a[1],a[2],a[3],a[4],a[5],a[6]); +} diff --git a/sw/simulator/sim_ir.c b/sw/simulator/sim_ir.c new file mode 100644 index 0000000000..4c97e74251 --- /dev/null +++ b/sw/simulator/sim_ir.c @@ -0,0 +1,24 @@ +/* Infrared soft simulation. OCaml binding. */ + +#include +#include "airframe.h" + +#include + +int16_t ir_roll; +int16_t ir_pitch; + +int16_t ir_contrast = IR_DEFAULT_CONTRAST; +int16_t ir_roll_neutral = IR_ROLL_NEUTRAL_DEFAULT; +int16_t ir_pitch_neutral = IR_PITCH_NEUTRAL_DEFAULT; +float ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / IR_DEFAULT_CONTRAST; + +void ir_update(void) { +} +void ir_gain_calib(void) { +} + +value set_ir_roll(value roll) { + ir_roll = Int_val(roll); + return Val_unit; +} diff --git a/sw/simulator/simhitl.ml b/sw/simulator/simhitl.ml new file mode 100644 index 0000000000..e50d053882 --- /dev/null +++ b/sw/simulator/simhitl.ml @@ -0,0 +1,14 @@ +open Stdlib + +let _ = + Arg.parse + (Sim.common_options@[set_string "-aircraft" Sim.ac_name "aircraft name"; + set_string "-fbw" Hitl.tty1 "Fly by wire MCU port"; + set_string "-ap" Hitl.tty0 "Autopilot MCU port"]) + (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x) + "Usage: "; + +module M = Sim.Make(Hitl.Make) + +let _ = + M.main () diff --git a/sw/simulator/simsitl.ml b/sw/simulator/simsitl.ml new file mode 100644 index 0000000000..69afabd146 --- /dev/null +++ b/sw/simulator/simsitl.ml @@ -0,0 +1,12 @@ + + +let _ = + Arg.parse (Sim.common_options@Sitl.options) + (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x) + "Usage: " + + +module M = Sim.Make(Sitl.Make) + +let _ = + M.main () diff --git a/sw/simulator/simsitl.pl b/sw/simulator/simsitl.pl new file mode 100755 index 0000000000..81b0aef25b --- /dev/null +++ b/sw/simulator/simsitl.pl @@ -0,0 +1,31 @@ +#!/usr/bin/perl -w + +use Getopt::Long; +my @paparazzi_lib; +BEGIN { + @paparazzi_lib = (defined $ENV{PAPARAZZI_SRC}) ? + ($ENV{PAPARAZZI_SRC}."/sw/lib/perl", $ENV{PAPARAZZI_SRC}."/sw/ground_segment/cockpit"):(); +} +use lib (@paparazzi_lib); + +use strict; +use Paparazzi::Environment; + +my $options = {}; +GetOptions ( + "b=s" => \$options->{ivy_bus}, + "a=s" => \$options->{aircraft}, + ); +my @args = (); +push @args, "-b", $options->{ivy_bus}; +my $sim_binary = Paparazzi::Environment::paparazzi_home()."/var/".$options->{aircraft}."/sim/simsitl.out"; +die "$sim_binary not found. try make AIRCRAFT=$options->{aircraft} ac\n" unless -e $sim_binary; +exec ($sim_binary, @args) + + + + + + + + diff --git a/sw/simulator/sirf.ml b/sw/simulator/sirf.ml new file mode 100644 index 0000000000..89c6a5aaac --- /dev/null +++ b/sw/simulator/sirf.ml @@ -0,0 +1,102 @@ +open Types + + exception BadChecksum + exception BadEndSequence + + let rec skip_until_start_sequence = fun gps -> + while input_byte gps <> 0xA0 do () done; + if input_byte gps <> 0xA2 then skip_until_start_sequence gps + + let send_start_sequence = fun gps -> + output_byte gps 0xA0; + output_byte gps 0xA2 + + let get_end_sequence = fun gps -> + if input_byte gps <> 0xB0 || input_byte gps <> 0xB3 then + raise BadEndSequence + + let send_end_sequence = fun gps -> + output_byte gps 0xB0; + output_byte gps 0xB3 + + let checksum = fun data -> + let cs = ref 0 in + String.iter (fun c -> cs := (!cs + Char.code c) land 0x7fff) data; + !cs + + let receive = fun gps -> + let gps = Unix.in_channel_of_descr gps in + skip_until_start_sequence gps; + let length_h = input_byte gps in + let length_l = input_byte gps in + let length = (length_h lsl 8) lor length_l in + let payload = String.create length in + for i = 0 to length - 1 do + payload.[i] <- input_char gps; + done; + let checksum_h = input_byte gps in + let checksum_l = input_byte gps in + get_end_sequence gps; + if checksum payload <> (checksum_h lsl 8) lor checksum_l then + raise BadChecksum; + payload + + let output_2bytes = fun gps x -> + output_byte gps ((x land 0xff00) lsr 8); + output_byte gps (x land 0xff) + + let send = fun gps payload -> + let n = String.length payload in + assert(n < 1023); + send_start_sequence gps; + output_2bytes gps n; + String.iter (output_char gps) payload; + output_2bytes gps (checksum payload); + send_end_sequence gps; + flush gps + + +let send1 = fun gps c -> + Printf.printf "send 0x%2x\n" c; flush stdout + +let send_int32 = fun gps x -> + Printf.printf "send32 0x%8x\n" x; flush stdout + + +let get = fun gps n -> + let buf = String.create n in + buf.[0] <- Char.chr 0x79; + assert(input gps buf 5 (n-5) = n-5); + buf + +let log_info = Bytes [ "MID", 1; + "S_First", 1; "S_Last", 1; + "A_First", 4; "A_Last", 4; "A_Start", 4; + "Size", 4 ] +let log_data = Bytes [ "MID", 1; "Start", 4; "Data", 256*2 ] + +let extended_nav = Bytes [ + "MID", 1; + "Latitude", 4; "Longitude", 4; "Altitude", 4; + "Speed", 4; "ClimbRate", 4; "Course", 4; + "Mode", 1; + "Year", 2; "Month", 1; "Day", 1; "Hour", 1; "Minute", 1; + "Second", 2; + "GDOP", 1; "HDOP", 1; "PDOP", 1; "TDOP", 1; "VDOP", 1;] + + +let get_message = fun gps expected -> + let s = size_of_message expected in + let data = get gps s in + (expected, data, 0) + + +let log_poll_info = fun gps -> + send1 gps 0xbb; + get_message gps log_info + +let log_read = fun gps a -> + send1 gps 0xb8; + send_int32 gps a; + get_message gps log_data + diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml new file mode 100644 index 0000000000..8697921a6b --- /dev/null +++ b/sw/simulator/sitl.ml @@ -0,0 +1,116 @@ +(* Software in the loop *) + +open Printf + +let ivy_bus = ref "127.255.255.255:2010" + +module Make(A:Data.MISSION) = struct + + let servos_period = 25 (* ms *) + let periodic_period = 16 (* ms *) + let rc_period = 25 (* ms *) + let id_period = 10_000 (* ms *) + + let periodic = fun p f -> + ignore (GMain.Timeout.add p (fun () -> f (); true)) + + + let msg = fun name -> + ExtXml.child Data.messages_ap ~select:(fun x -> ExtXml.attrib x "name" = name) "message" + let gps_msg = msg "GPS" + + +(* Servos handling (rservos is the intermediate storage) *) + let rc_channels = Array.of_list (Xml.children A.ac.Data.radio) + let nb_channels = Array.length rc_channels + let rc_channel_no = fun x -> + List.assoc x (Array.to_list (Array.mapi (fun i c -> Xml.attrib c "function", i) rc_channels)) + + let rservos = ref [||] + let adj_bat = GData.adjustment ~value:12.5 ~lower:0. ~upper:23. ~step_incr:0.1 () + + external set_servos : Stdlib.us array -> int = "set_servos" +(** Returns gaz servo value (us) *) + + let energy = ref 0. + let update_servos = fun () -> + let gaz = set_servos !rservos in + (* 100% = 1W *) + energy := !energy +. float (gaz-1000) /. 1000. *. float servos_period /. 1000. + + let update_adj_bat = fun () -> + let b = adj_bat#value in + adj_bat#set_value (b -. !energy *. 0.00259259259259259252); (* To be improved !!! *) + energy := 0. + + +(* Radio command handling *) + external update_channel : int -> float -> unit = "update_rc_channel" + + let inverted = ["ROLL"; "PITCH"; "YAW"; "GAIN1"; "GAIN2"] + + let rc = fun () -> + let name = Xml.attrib A.ac.Data.radio "name" ^ " " ^ A.ac.Data.name in + let window = GWindow.window ~title:name ~border_width:0 ~width:400 ~height:400 () in + let quit = fun () -> GMain.Main.quit (); exit 0 in + ignore (window#connect#destroy ~callback:quit); + let vbox = GPack.vbox ~packing:window#add () in + Array.iteri + (fun i c -> + let adj = GData.adjustment ~value:0. ~lower:(-100.) ~upper:110. ~step_incr:1.0 () in + let hbox = GPack.hbox ~packing:vbox#add () in + let f = (ExtXml.attrib c "function") in + let l = GMisc.label ~width:75 ~text:f ~packing:hbox#pack () in + let inv = List.mem f inverted in + let _scale = GRange.scale `HORIZONTAL ~inverted:inv ~adjustment:adj ~packing:hbox#add () in + let update = fun () -> update_channel i (adj#value /. 100.) in + + ignore (adj#connect#value_changed update); + update ()) + rc_channels; + + window#show () + + external periodic_task : unit -> unit = "sim_periodic_task" + external rc_task : unit -> unit = "sim_rc_task" + external sim_init : int -> unit = "sim_init" + external update_bat : int -> unit = "update_bat" + + let init = fun id vbox -> + Ivy.init (sprintf "Paparazzi sim %d" id) "READY" (fun _ _ -> ()); + Ivy.start !ivy_bus; + rc (); + sim_init id; + + let hbox = GPack.hbox ~packing:vbox#add () in + let l = GMisc.label ~text:"Bat:" ~packing:hbox#pack () in + let _scale = GRange.scale `HORIZONTAL ~adjustment:adj_bat ~packing:hbox#add () in + let update = fun () -> update_bat (truncate (adj_bat#value *. 10.)) in + ignore (adj_bat#connect#value_changed update); + update () + + let boot = fun () -> + periodic servos_period update_servos; + periodic periodic_period periodic_task; + periodic rc_period rc_task; + periodic 10000 update_adj_bat + + +(* Functions called by the simulator *) + let servos = fun s -> rservos := s + + external set_ir_roll : int -> unit = "set_ir_roll" + let infrared = fun phi ctrst -> + set_ir_roll (truncate (phi *. ctrst)) + + external use_gps_pos: int -> int -> float -> float -> float -> float -> float -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos" + open Latlong + let gps = fun gps -> + let utm = utm_of WGS84 gps.Gps.wgs84 in + let cm = fun f -> truncate (f *. 100.) in + use_gps_pos (cm utm.utm_x) (cm utm.utm_y) gps.Gps.course gps.Gps.alt gps.Gps.gspeed gps.Gps.climb gps.Gps.time + +end +let options = + [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"] + diff --git a/sw/simulator/sitl.mli b/sw/simulator/sitl.mli new file mode 100644 index 0000000000..30fe45954b --- /dev/null +++ b/sw/simulator/sitl.mli @@ -0,0 +1,6 @@ +(* Software In The Loop *) + +module Make : functor (A : Data.MISSION) -> Sim.AIRCRAFT +val options : (string * Arg.spec * string) list +(** Arg options specific to Sitl *) + diff --git a/sw/simulator/stdlib.ml b/sw/simulator/stdlib.ml new file mode 100644 index 0000000000..8b0f65bbf4 --- /dev/null +++ b/sw/simulator/stdlib.ml @@ -0,0 +1,16 @@ +type us = int + +let pi = 4. *. atan 1. +let rec norm_angle = fun x -> + if x > pi then norm_angle (x-.2.*.pi) + else if x < -.pi then norm_angle (x+.2.*.pi) + else x + +let deg = fun rad -> rad /. pi *. 180. + +let rad_of_deg = fun x -> x /. 180. *. pi + +let set_float = fun option var name -> + (option, Arg.Set_float var, Printf.sprintf "%s (%f)" name !var) +let set_string = fun option var name -> + (option, Arg.Set_string var, Printf.sprintf "%s (%s)" name !var) diff --git a/sw/simulator/timer.h b/sw/simulator/timer.h new file mode 100644 index 0000000000..fddeabaa72 --- /dev/null +++ b/sw/simulator/timer.h @@ -0,0 +1 @@ +#define bit_is_set(x, b) ((x >> b) & 0x1) diff --git a/sw/simulator/types.ml b/sw/simulator/types.ml new file mode 100644 index 0000000000..b0a2564ca9 --- /dev/null +++ b/sw/simulator/types.ml @@ -0,0 +1,116 @@ +type size = int +type label = string +type data_layout = + Bits of (label * size) list + | Bytes of (label * size) list + + + +type record = data_layout * string * int +let get_record = fun record_layout string offset -> + (record_layout, string, offset) + + +(* Little endian *) +let make_int_from_bytes = fun data pos size -> + if size < 4 then + let rec mk = fun pos s i -> + if s = 0 then i else mk (pos+1) (s-1) ((i lsl 8) lor Char.code data.[pos]) in + mk pos size 0 + else if size = 4 then begin + let c = fun i -> Int32.shift_left (Int32.of_int (Char.code data.[pos+i])) (8*(3-i)) in + let lor32 = Int32.logor in + Int32.to_int (lor32 (c 0) (lor32 (c 1) (lor32 (c 2) (c 3)))) + + end else invalid_arg "make_int_from_bytes" + + +(* Little endian *) +let make_int_from_bits = fun data offset pos size -> + assert(pos < 8); + assert(size < 31); + let nb_bits_in_first_byte = min (8-pos) size in + let i = ((Char.code data.[offset] lsl pos) land 0xff) lsr (8-nb_bits_in_first_byte) in + let rec mk = fun offset s i -> + if s = 0 + then i + else if s < 8 + then (i lsl s) lor (Char.code data.[offset] lsr (8 - s)) + else mk (offset+1) (s-8) ((i lsl 8) lor Char.code data.[offset]) in + mk (offset+1) (size-nb_bits_in_first_byte) i + +let assoc = fun label layout -> + let rec assoc pos = function + [] -> failwith ("get_int: unknown field "^label) + | (l, s)::lss -> + if l = label + then (pos, s) + else assoc (pos + s) lss in + assoc 0 layout + + +let get_int = fun signed label (record_layout, data, offset) -> + match record_layout with + Bits l -> + let (pos, size) = assoc label l in + let i = + if pos mod 8 = 0 && size mod 8 = 0 then + let pos = pos / 8 and size = size / 8 in + make_int_from_bytes data (offset+pos) size + else + make_int_from_bits data (offset+pos/8) (pos mod 8) size in + if signed then (i lsl (31-size)) asr (31-size) else i + | Bytes l -> + let (pos, size) = assoc label l in + let i = make_int_from_bytes data (offset+pos) size in + if size < 4 then + if signed then (i lsl (31-4*size)) asr (31-4*size) else i + else begin + assert(not signed); + i + end + +let get_int32 = get_int true +let get_u32 = get_int false +let get_uint = get_int false +let get_int = get_int true + +let get_raw = fun label (record_layout, data, offset) -> + match record_layout with + Bytes layout -> + let (pos, size) = assoc label layout in + String.sub data (offset+pos) size + | _ -> failwith "get_raw" + + + +let sum_sizes = List.fold_left (fun a (_, s) -> a+s) 0 +let size_of_message = function (* In bytes *) + Bytes l -> sum_sizes l + | Bits l -> sum_sizes l / 8 + +let make_payload = fun layout values -> + match layout with + (Bytes layout) -> + let p = String.create (sum_sizes layout) in + List.iter + (fun (label, value) -> + let (pos, size) = assoc label layout in + let byte = fun x -> Char.chr (x land 0xff) in + match size with + 1 -> p.[pos] <- byte value + | 2 -> + p.[pos] <- byte (value asr 8); + p.[pos+1] <- byte value + | 4 -> + p.[pos] <- byte (value asr 24); + p.[pos+1] <- byte (value lsr 16); + p.[pos+2] <- byte (value lsr 8); + p.[pos+3] <- byte value + | _ -> failwith "make_payload: unknown int size" + ) + values; + p + | _ -> failwith "make_payload" + + diff --git a/sw/supervision/Paparazzi/CpGui.pm b/sw/supervision/Paparazzi/CpGui.pm new file mode 100755 index 0000000000..c445d5e388 --- /dev/null +++ b/sw/supervision/Paparazzi/CpGui.pm @@ -0,0 +1,219 @@ +package Paparazzi::CpGui; + +use Subject; +use Paparazzi::CpSessionMgr; +@ISA = qw(Paparazzi::CpSessionMgr); + +use strict; + +use Tk; +use Tk::MainWindow; +use Tk::NoteBook; +use Tk::HList; +use Tk::ItemStyle; +use Data::Dumper; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-logo_file => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef], + -variables => [S_SUPER, S_SUPER, S_SUPER, S_SUPER, S_SUPER, undef], + ); +} +sub completeinit { + my ($self) = @_; + $self->SUPER::completeinit(); + $self->build_gui(); +} + +sub onProgramSelected { + my ($self, $pgm_name) = @_; + $self->toggle_program("NONE", $pgm_name, []); +} + +sub onSessionSelected { + my ($self, $session_name) = @_; + $self->start_session($session_name); +} + +use constant LIST_WIDTH => 80; +use constant LIST_HEIGHT => 20; + +sub build_gui { + my ($self) = @_; + my $mw = MainWindow->new(); + $mw->title ($self->{cp_name}); + # menu bar + my $menubar = + $mw->Frame( -relief => 'ridge')->pack(-side => 'top', -fill => 'x'); + my $session_menu = $menubar->Menubutton(-text => 'Sessions')->pack(-side => 'left');; + my $sessions = $self->get('-sessions'); + foreach my $session_name (keys %{$sessions}) { + $session_menu->command( -label => $session_name, + -command => [\&onSessionSelected, $self, $session_name] ); + } + my $program_menu = $menubar->Menubutton(-text => 'Programs')->pack(-side => 'left');; + my $programs = $self->get(-programs); + foreach my $pgm_name (keys %{$programs}) { + $program_menu->command( -label => $pgm_name, + -command => [\&onProgramSelected, $self, $pgm_name] + ); + } + # session frame + my $session_frame = $mw->Frame( -relief => 'groove')->pack(-side => 'bottom', -fill => 'both', -expand => "yes",); + my $notebook = $session_frame->NoteBook( -ipadx => 6, -ipady => 6); + $notebook->pack(-expand => "yes", + -fill => "both", + -padx => 5, -pady => 5, + -side => "top"); + $self->build_logo_page($notebook); + $self->build_list_page($notebook, "hosts", "Hosts", ["name", "ip", "status"], \&build_hosts_page); + $self->build_list_page($notebook, "variables", "Variables", ["name", "value"], \&build_variables_page); +# $self->build_list_page($notebook, "programs", "Programs", ["name", "command", "args"], \&build_programs_page); + $self->build_programs_page($notebook); + $self->build_list_page($notebook, "sessions", "Sessions", ["name", "command", "args"], \&build_sessions_page); +# $self->build_programs_page($notebook); +# my $programs_page = $notebook->add("programs", -label => "Programs", -underline => 0); +# my $sessions_page = $notebook->add("sessions", -label => "Sessions", -underline => 0); + + $self->{session_frame} = $session_frame; +} + + +sub build_logo_page { + my ($self, $notebook) = @_; + my $logo_filename = $self->get('-logo_file'); + return unless defined $logo_filename; + my $logo_page = $notebook->add("logo", -label => "Logo", -underline => 0); + my $image = $logo_page->Photo('logogif', + -format => 'GIF', + -file => $logo_filename); + my $labelImage = $logo_page->Label('-image' => 'logogif')->pack(); + return $logo_page; +} + + +sub build_hosts_page { + my ($self, $hlist, $e, $section_h, $item) = @_; + $hlist->itemCreate ($e, 0, + -itemtype => 'text', + -text => $item + ); + my $ip = $section_h->{$item}; + $hlist->itemCreate ($e, 1, + -itemtype => 'text', + -text => $ip?$ip:"unknown" + ); + $hlist->itemCreate ($e, 2, + -itemtype => 'text', + -text => "unknown" + ); +} + +sub build_variables_page { + my ($self, $hlist, $e, $section_h, $item) = @_; + $hlist->itemCreate ($e, 0, -itemtype => 'text', + -text => $item + ); + $hlist->itemCreate ($e, 1, -itemtype => 'text', + -text => $section_h->{$item}, + ); +} + +#sub build_programs_page { +# my ($self, $hlist, $e, $section_h, $item) = @_; +# $hlist->itemCreate ($e, 0, -itemtype => 'text', +# -text => $item +# ); +# $hlist->itemCreate ($e, 1, -itemtype => 'text', +# -text => $section_h->{$item}->{command}, +# ); + +# $hlist->itemCreate ($e, 2, -itemtype => 'text', +# -text => $section_h->{$item}->{args}, +# ); +#} + +sub build_sessions_page { + my ($self, $hlist, $e, $section_h, $item) = @_; + $hlist->itemCreate ($e, 0, -itemtype => 'text', + -text => $item + ); + $hlist->itemCreate ($e, 1, -itemtype => 'text', + -text => $section_h->{$item}->{command}, + ); + + $hlist->itemCreate ($e, 2, -itemtype => 'text', + -text => $section_h->{$item}->{args}, + ); +} + + +sub build_programs_page { + my ($self, $notebook) = @_; + my $page = $notebook->add("programs", -label => "Programs", -underline => 0); + my @header = ("name", "command", "args"); + my $hlist = $page->Scrolled ('HList', +# -selectmode => 'extended', + -header => 1, +# -columns => $#header + 1, + -width => LIST_WIDTH, + -height => LIST_HEIGHT, + -itemtype => 'imagetext', + -indent => 35, + -separator => '/', + )->grid(-sticky => 'nsew'); +# for my $i (0 .. $#header) { +# $hlist->header('create', $i, -text => $header[$i]); +# } + my $section_h = $self->get('-programs'); + foreach my $program (keys %{$section_h}) { +# print Dumper($section_h->{$program})."\n"; + $hlist->add($program, -text => $program ); + + $hlist->add($program."/command", -text => "command : ".$section_h->{$program}->{command}); + $hlist->add($program."/args", -text => "args :"); + my $args = $section_h->{$program}->{args}; + foreach my $argh (@{$args}) { + $hlist->add($program."/args/".$argh->{flag}, -text => $argh->{flag}."\t". $argh->{type}."\t". $argh->{value}); + } + } + return $page +} + + +sub build_list_page { + my ($self, $notebook, $section, $label, $header, $row_fun) = @_; + my $page = $notebook->add($section, -label => $label, -underline => 0); + my @header = @{$header}; + my $hlist = $page->Scrolled ('HList', + -header => 1, + -columns => $#header + 1, + -width => LIST_WIDTH, + -height => LIST_HEIGHT, + )->grid(-sticky => 'nsew'); + for my $i (0 .. $#header) { +# print("header $header[$i]\n"); + $hlist->header('create', $i, -text => $header[$i]); + } + my $section_h = $self->get('-'.$section); + print "CpGui variables ".Dumper($section_h) if ($section eq "variables"); + foreach my $item (keys %{$section_h}) { + my $e = $hlist->addchild(""); + &$row_fun($self, $hlist, $e, $section_h, $item); + # print("$hlist, $e, $section_h, $item\n"); + } + return $page +} + +1; + + + + + + + + + + diff --git a/sw/supervision/Paparazzi/CpPgmMgr.pm b/sw/supervision/Paparazzi/CpPgmMgr.pm new file mode 100644 index 0000000000..48b9a1d509 --- /dev/null +++ b/sw/supervision/Paparazzi/CpPgmMgr.pm @@ -0,0 +1,79 @@ +package Paparazzi::CpPgmMgr; + +use Subject; +@ISA = ("Subject"); + +use strict; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-children => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + ); +} + +sub completeinit { + my $self = shift; + $self->SUPER::completeinit; +} + +sub start_program() { + my ($self, $pgm, @options, @args, $keep_stdin) = @_; + my %children = %{$self->get('-children')}; + +# print("in ChildrenSpawner::start_programm args [$pgm @args]\n"); + my $pid = undef; + my $sleep_count = 0; + my $fh; + do { + $pid = fork(); + $SIG{PIPE} = sub { die "whoops, $pgm pipe broke" }; + unless (defined $pid) { + warn "cannot fork: $!"; + die "bailing out" if $sleep_count++ > 6; + sleep 1; + } + } until defined $pid; + + if (! $pid) { # child + $SIG{TERM} = 'IGNORE'; + exec ($pgm, @options, @args);# or die "couldnt exec foo: $pgm @args"; + # NOTREACHED + exit(1); + } + # parent + $children{$pid} = {cmd => $pgm, args => \@args};#, ktw => $fh}; + $self->configure('-children', \%children); + foreach my $key (keys %children) { +# print("in ChildrenSpawner::start_programm child: [$key $children{$key}]\n"); + } + return $pid; +} + +sub stop_program() { + my ($self, $pid) = @_; +# print "in_stop_program $pid\n"; + + my %children = %{$self->get('-children')}; + my $pgm = $children{$pid}; + + if (defined $pgm) { +# printf STDOUT "Killing Process %d [%s %s]\n", $pid, $pgm->{cmd}, $pgm->{args}; + kill 9, $pid; + $children{$pid} = undef; + $self->configure('-children', \%children); + } +} + + +sub terminate_all() { + my ($self) = @_; +# print("in ChildrenSpawner::terminate_all\n"); + my %pgms = %{$self->get('-children')}; + foreach my $pid (keys %pgms) { +# print "killing $pid ($pgms{$pid})\n"; + $self->stop_program($pid); + } +} + +1; diff --git a/sw/supervision/Paparazzi/CpSessionMgr.pm b/sw/supervision/Paparazzi/CpSessionMgr.pm new file mode 100644 index 0000000000..0579e90c6b --- /dev/null +++ b/sw/supervision/Paparazzi/CpSessionMgr.pm @@ -0,0 +1,192 @@ +package Paparazzi::CpSessionMgr; + +use Data::Dumper; +use XML::DOM; +use Subject; + +use Paparazzi::CpPgmMgr; +@ISA = qw(Paparazzi::CpPgmMgr); + +use strict; + +sub populate { + my ($self, $args) = @_; + $self->SUPER::populate($args); + $self->configspec(-config_file => [S_NEEDINIT, S_PASSIVE, S_RDONLY, S_OVRWRT, S_NOPRPG, undef], + -bin_base_dir => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, "/usr/bin"], + -log_dir => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, "/var/tmp"], + -variables => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + -hosts => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + -programs => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + -sessions => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, {}], + ); +} + +sub completeinit { + my ($self) = @_; + $self->SUPER::completeinit(); + my $cfg_file = $self->get('-config_file'); +# my $variables = $self->get('-variables'); +# print "initial variables\n".Dumper($variables); + $self->read_cfg($cfg_file); +# $variables = $self->get('-variables'); +# print "configured variables\n".Dumper($variables); +} + +sub prepare_args { + my ($self, $args) = @_; + my (@options, @rargs); + my $variables = $self->get('-variables'); + print "CpSessionMgr : variables ".Dumper($variables); + foreach my $opt (@{$args}) { + my $type = $opt->{type}; + my $flag = $opt->{flag}; + my $value = $type eq 'var' ? $variables->{$opt->{value}}: $opt->{value}; + if ($flag) { + if ($flag =~ /\.*=/) { push @options, $flag.$value} + else {push @options, $flag, $value} + } + else { push @rargs, $value} + } + return (@options, @rargs); +} + +sub toggle_program { + my ($self, $session_name, $pgm_name, $pgm_session_args, $session_idx) = @_; +# shift @_; +# print Dumper(@_); + my $programs = $self->get('-programs'); +# print "Progams ".Dumper($programs); + my $program = $programs->{$pgm_name}; +# print "Toggling Progam ".Dumper($program); + my $command; + if ($program->{command} =~ /^\/.*/) { + $command = $program->{command}; + } + else { + $command = $self->get('-bin_base_dir')."/".$program->{command}; + } + if ($session_name eq "NONE") { + if (defined $program->{pid}) { + $self->SUPER::stop_program($program->{pid}); + $program->{pid} = undef; + } + else { + my (@options, @args) = $self->prepare_args($program->{args}); +# print Dumper($program->{args}); + print "starting $pgm_name [$command @options, @args]\n"; + $program->{pid} = $self->SUPER::start_program($command, @options[0..$#options], @args[0..$#args]); + } + } + else { + my @pgm_args = $self->prepare_args($program->{args}); +# print "program->{args} ".Dumper($program->{args}); +# print "pgm_args ".Dumper(@pgm_args); +# print "session ".Dumper($self->{sessions}->{$session_name}); + + my $session_pgms = $self->get('-sessions')->{$session_name}->{pgms}; +# print "session_pgms ".Dumper($session_pgms); + my $_session_args = ($session_pgms->[$session_idx])->{args}; + my @session_args = defined $_session_args ? $self->prepare_args($_session_args) : []; +# print "session_args ".Dumper($_session_args); + + push @pgm_args , @session_args; + print "session $session_name starting program $pgm_name\n[$command @pgm_args]\n"; + $self->{sessions}->{$session_name}->{pgms}[$session_idx]->{pid} = $self->SUPER::start_program($command, @pgm_args[0..$#pgm_args]); + } +} + +sub start_session { + my ($self, $session_name) = @_; +# print "starting session $session_name\n"; + my $sessions = $self->get('-sessions'); + my $session = $sessions->{$session_name}; + my @progs = @{$session->{pgms}}; +# print "progs ".Dumper(@progs); + my $session_idx = 0; + foreach my $pgm (@progs) { + my $pgm_name = $pgm->{name}; + my $pgm_session_args = $pgm->{args}; + $self->toggle_program($session_name, $pgm_name, $pgm_session_args, $session_idx) if (!defined $self->{programs}->{$pgm_name}->{pid}); + $session_idx++; + } +} + +sub xml_parse_args { + my ($args) = @_; + my @args_a; + foreach my $arg (@{$args}){ + my $var = $arg->getAttribute('variable'); + my $args_h = { + flag => $arg->getAttribute('flag'), + type => $var eq '' ? 'const' : 'var', + value => $var eq '' ? $arg->getAttribute('constant'): $var, + }; + push @args_a, $args_h; + } +# print "@args_a\n"; + return \@args_a; +} + +sub xml_parse_section { + my ($self, $section) = @_; + my $section_name = $section->getAttribute('name'); + my ($items_name) = ($section_name =~ /(.*)s$/); +# print "section $section_name items_name $items_name\n"; + my $items = $section->getElementsByTagName($items_name); + my $h_name = '-'.$section_name; + print "h_name $h_name\n"; + my $tmp = $self->get($h_name); + foreach my $item (@{$items}){ + if ($section_name eq "hosts") { + $tmp->{$item->getAttribute('name')} = $item->getAttribute('ip'); + } + elsif ($section_name eq 'variables') { + $tmp->{$item->getAttribute('name')} = $item->getAttribute('value'); + } + elsif ($section_name eq 'programs') { + my $pgm_name = $item->getAttribute('name'); + my $args = $item->getElementsByTagName("arg"); + my $args_h = xml_parse_args($args); + $tmp->{$pgm_name} = + {name => $pgm_name, + command => $item->getAttribute('command'), + args => $args_h, + }; + } + elsif ($section_name eq 'sessions') { + my $session_name = $item->getAttribute('name'); + my $xsessions_pgms = $item->getElementsByTagName("program"); + my @sessions_pgms; + foreach my $session_pgm (@{$xsessions_pgms}){ + my $pgm_name = $session_pgm->getAttribute('name'); + my $session_args = $session_pgm->getElementsByTagName("arg"); + my $args_h = xml_parse_args($session_args); + push @sessions_pgms, { + name => $pgm_name, + args => $args_h + }; + } + $tmp->{$session_name} = { + name => $session_name, + pgms => \@sessions_pgms + }; + } + } + $self->configure($h_name => $tmp); +} + +sub read_cfg { + my ($self, $filename) = @_; + my $parser = XML::DOM::Parser->new(); + my $doc = $parser->parsefile($filename); + my $cp = $doc->getElementsByTagName("control_panel")->[0]; + $self->{cp_name} = $cp->getAttribute('name'); + my $sections = $cp->getElementsByTagName("section"); + foreach my $section (@{$sections}) { + $self->xml_parse_section($section); + } +} + + +1; diff --git a/sw/supervision/paparazzi.pl b/sw/supervision/paparazzi.pl new file mode 100755 index 0000000000..8a8640ec95 --- /dev/null +++ b/sw/supervision/paparazzi.pl @@ -0,0 +1,80 @@ +#!/usr/bin/perl -w +package Paparazzi; + +my $paparazzi_lib; +BEGIN { + $paparazzi_lib = (defined $ENV{PAPARAZZI_SRC}) ? + $ENV{PAPARAZZI_SRC}."/sw/lib/perl" : "/usr/lib/paparazzi/"; +} +use lib ($paparazzi_lib); + +use Paparazzi::CpGui; +@ISA = qw(Paparazzi::CpGui); + +use Paparazzi::Environment; + +use strict; + +use Tk; +use Subject; + +use Data::Dumper; +use Getopt::Long; + +sub populate { + my ($self, $args) = @_; + my $paparazzi_src = Paparazzi::Environment::paparazzi_src(); + my $paparazzi_home = Paparazzi::Environment::paparazzi_home(); + Paparazzi::Environment::check_paparazzi_home(); + $args->{-config_file} = $paparazzi_home."/conf/control_panel.xml"; + $args->{-variables} = {paparazzi_home => $paparazzi_home}; + $args->{-bin_base_dir} = $paparazzi_src; + $args->{-logo_file} = $paparazzi_home."/data/pictures/penguin_logo.gif"; + $self->SUPER::populate($args); + $self->configspec(-variables => [S_SUPER, S_SUPER, S_SUPER, S_SUPER, S_SUPER, undef]); +} +sub completeinit { + my ($self) = @_; + $self->SUPER::completeinit(); + $self->parse_args(); +} + +sub parse_args { + my ($self) = @_; + my $options = { + ivy_bus => "127.255.255.255:2005", + map => "maps/muret_UTM.xml", + render => "1", + }; + GetOptions("b=s" => \$options->{ivy_bus}, + "m=s" => \$options->{map}, + "r=s" => \$options->{render}, + ); + my $variables = $self->get('-variables'); + foreach my $var (keys %{$options}) { + $variables->{$var} = $options->{$var}; + } + $self->configure('-variables' => $variables); +} + +sub catchSigTerm() { + my ($paparazzi) = @_; + printf("in catchSigTerm\n"); + $paparazzi->terminate_all(); +} + +my $paparazzi = Paparazzi->new(); +$SIG{TERM} = sub {$paparazzi->catchSigTerm()}; +Tk::MainLoop(); +$paparazzi->catchSigTerm(); + +1; + + + + + + + + + diff --git a/sw/tools/Makefile b/sw/tools/Makefile new file mode 100644 index 0000000000..055d7e53da --- /dev/null +++ b/sw/tools/Makefile @@ -0,0 +1,39 @@ +OCAMLC=ocamlc -g -I ../lib/ocaml +OCAMLLEX=ocamllex +OCAMLYACC=ocamlyacc + +all: gen_aircraft.out gen_airframe.out gen_calib.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out + +FP_CMO = fp_syntax.cmo fp_parser.cmo fp_lexer.cmo fp_proc.cmo gen_flight_plan.cmo + +gen_flight_plan.out : $(FP_CMO) + $(OCAMLC) lib.cma $^ -o $@ + +fp_parser.cmo : fp_parser.cmi fp_syntax.cmi +fp_parser.cmi : fp_parser.ml fp_syntax.cmi +fp_lexer.cmi : fp_syntax.cmi +fp_lexer.cmo : fp_lexer.cmi +gen_flight_plan.cmo : fp_parser.cmi fp_proc.cmi +fp_syntax.cmo : fp_syntax.cmi + + +%.out : %.ml + $(OCAMLC) lib.cma $< -o $@ + +%.cmo : %.ml + $(OCAMLC) -c $< + +%.cmi : %.mli + $(OCAMLC) -c $< + +%.ml : %.mll + $(OCAMLLEX) $< + +%.ml : %.mly + $(OCAMLYACC) $< + +%.mli : %.mly + $(OCAMLYACC) $< + +clean: + rm -f *.cm* *.out *~ fp_parser.ml fp_parser.mli diff --git a/sw/tools/fp_lexer.mll b/sw/tools/fp_lexer.mll new file mode 100644 index 0000000000..b59124cc3c --- /dev/null +++ b/sw/tools/fp_lexer.mll @@ -0,0 +1,30 @@ +{ +open Fp_parser +} +rule token = parse + [' ' '\t' '\n'] { token lexbuf} + | "/*"([^'*']|'*'[^'/'])*'*'*'/' { token lexbuf} + | ['0'-'9']+ { INT (int_of_string (Lexing.lexeme lexbuf)) } + | ['0'-'9']+'.'['0'-'9']* { FLOAT (float_of_string (Lexing.lexeme lexbuf)) } + | ['a'-'z' 'A'-'Z'] (['a'-'z' 'A'-'Z' '_' '.' '0'-'9']*) { IDENT (Lexing.lexeme lexbuf) } + | ',' { COMMA } + | ';' { SEMICOLON } + | ':' { COLON } + | '(' { LP } + | ')' { RP } + | '{' { LC } + | '}' { RC } + | '[' { LB } + | ']' { RB } + | "==" { EQ } + | "&&" { AND } + | ">" { GT } + | ">=" { GEQ } + | "+" { PLUS } + | "=" { ASSIGN } + | "-" { MINUS } + | "*" { MULT } + | "/" { DIV } + | "!" { NOT } + | eof { EOF } + diff --git a/sw/tools/fp_parser.mly b/sw/tools/fp_parser.mly new file mode 100644 index 0000000000..402ee6039a --- /dev/null +++ b/sw/tools/fp_parser.mly @@ -0,0 +1,51 @@ +/* $Id$ */ +%{ +open Fp_syntax +%} +%token INT +%token FLOAT +%token IDENT +%token EOF +%token COMMA SEMICOLON LP RP LC RC LB RB AND COLON +%token EQ GT ASSIGN GEQ NOT +%token PLUS MINUS +%token MULT DIV + + +%left EQ GT ASSIGN GEQ /* lowest precedence */ +%left PLUS MINUS +%left MULT DIV +%nonassoc NOT +%nonassoc UMINUS /* highest precedence */ + +%start expression /* the entry point */ +%type expression + +%% + +expression: + expression GT expression { Call (">",[$1;$3]) } + | expression GEQ expression { Call (">=",[$1;$3]) } + | expression EQ expression { Call ("==",[$1;$3]) } + | expression AND expression { Call ("&&",[$1;$3]) } + | expression PLUS expression { Call ("+",[$1;$3]) } + | expression MINUS expression { Call ("-",[$1;$3]) } + | expression MULT expression { Call ("*",[$1;$3]) } + | expression DIV expression { Call ("/",[$1;$3]) } + | MINUS expression %prec UMINUS { Call ("-",[$2]) } + | NOT expression { Call ("!",[$2]) } + | INT { Int $1 } + | FLOAT { Float $1 } + | IDENT { Ident $1 } + | IDENT LP Args RP { Call ($1, $3) } + | LP expression RP { $2 } + | IDENT LB expression RB { Index ($1, $3) } +; + +Args: { [] } + | expression NextArgs { $1::$2 } +; + +NextArgs: { [] } + | COMMA expression NextArgs { $2::$3 } +; diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml new file mode 100644 index 0000000000..46db0f3432 --- /dev/null +++ b/sw/tools/fp_proc.ml @@ -0,0 +1,389 @@ +(* + * $Id$ + * + * Flight plan preprocessing (procedure including) + * + * Copyright (C) 2004 CENA/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 Fp_syntax + + +let parse_expression = fun s -> + let lexbuf = Lexing.from_string s in + try + Fp_parser.expression Fp_lexer.token lexbuf + with + Failure("lexing: empty token") -> + Printf.fprintf stderr "Lexing error in '%s': unexpected char: '%c' \n" + s (Lexing.lexeme_char lexbuf 0); + exit 1 + | Parsing.Parse_error -> + Printf.fprintf stderr "Parsing error in '%s', token '%s' ?\n" + s (Lexing.lexeme lexbuf); + exit 1 + + +open Latlong + +let float_attrib = fun xml a -> float_of_string (ExtXml.attrib xml a) + +(* Translation and rotation *) +type affine = { dx : float; dy : float; angle : float (* Deg Clockwise *) } + +let dtd_error = fun f e -> + Printf.fprintf stderr "DTD error in '%s': %s\n" f e; + exit 1 + +(* Rotation. Would be better with a matrix multiplication ? *) +let rotate = fun angle (x, y) -> + let angle = -. (Deg>>Rad) angle in + let a = atan2 y x + and r = sqrt (x**2. +. y**2.) in + let a' = a +. angle in + (r*.cos a', r*.sin a') + +let rotate_expression = fun a expression -> + let rec rot = fun e -> + match e with + | Call("Qdr", [Float a']) -> Call("Qdr", [Float (a' +. a)]) + | Call("Qdr", [Int a']) -> Call("Qdr", [Float (float a' +. a)]) + | Call(op, [e1; e2]) when op = "And" || op ="Or" -> + Call(op, [rot e1; rot e2]) + | _ -> e in + rot expression + +let subst_expression = fun env e -> + let rec sub = fun e -> + match e with + Ident i -> Ident (try List.assoc i env with Not_found -> i) + | Int _ | Float _ -> e + | Call (i, es) -> Call (i, List.map sub es) + | Index (i,e) -> Index (i,sub e) in + sub e + + +let transform_expression = fun affine env e -> + let e = parse_expression e in + let e' = rotate_expression affine.angle e in + let e'' = subst_expression env e' in + Fp_syntax.sprint_expression e'' + + +let transform_values = fun attribs_not_modified affine env attribs -> + List.map + (fun (a, v) -> + let v' = + if List.mem (String.lowercase a) attribs_not_modified + then v + else transform_expression affine env v in + (a, v')) + attribs + +let transform_waypoint = fun prefix affine xml -> + let x = float_attrib xml "x" + and y = float_attrib xml "y" in + let (x, y) = rotate affine.angle (x, y) in + let (x, y) = (x+.affine.dx, y+.affine.dy) in + let alt = try ["alt", ExtXml.attrib xml "alt"] with ExtXml.Error _ -> [] in + Xml.Element (Xml.tag xml, + ["name", prefix (ExtXml.attrib xml "name"); + "x", string_of_float x; + "y", string_of_float y]@alt, + []) + + +let prefix_value = fun prefix name attribs -> + List.map + (fun (a, v) -> + let v' = if String.lowercase a = name then prefix v else v in + (a, v')) + attribs + +let prefix_or_deroute = fun prefix reroutes name attribs -> + List.map + (fun (a, v) -> + let v' = + if String.lowercase a = name then + try List.assoc v reroutes with + Not_found -> prefix v + else v in + (a, v')) + attribs + +let transform_attribs = fun affine attribs -> + List.map + (fun (a, v) -> + match String.lowercase a with + "wp_qdr" | "from_qdr" -> + (a, string_of_float (float_of_string v +. affine.angle)) + | _ -> (a, v) + ) + attribs + + +let transform_stage = fun prefix reroutes affine env xml -> + let rec tr = fun xml -> + match xml with + Xml.Element (tag, attribs, children) -> begin + match tag with + "exception" -> + assert (children=[]); + let attribs = prefix_or_deroute prefix reroutes "deroute" attribs in + let attribs = transform_values [] affine env attribs in + Xml.Element (tag, attribs, children) + | "while" -> + let attribs = transform_values [] affine env attribs in + Xml.Element (tag, attribs, List.map tr children) + | "heading" -> + assert (children=[]); + let attribs = transform_values ["vmode"] affine env attribs in + Xml.Element (tag, attribs, children) + | "go" -> + assert (children=[]); + let attribs = transform_values ["wp";"from";"hmode";"vmode"] affine env attribs in + let attribs = prefix_value prefix "wp" attribs in + let attribs = prefix_value prefix "from" attribs in + let attribs = transform_attribs affine attribs in + Xml.Element (tag, attribs, children) + | "xyz" -> + assert (children=[]); + let attribs = transform_values [] affine env attribs in + Xml.Element (tag, attribs, children) + | "circle" -> + assert (children=[]); + let attribs = transform_values ["wp";"hmode";"vmode"] affine env attribs in + let attribs = prefix_value prefix "wp" attribs in + let attribs = transform_attribs affine attribs in + Xml.Element (tag, attribs, children) + | "deroute" -> + assert (children=[]); + let attribs = prefix_or_deroute prefix reroutes "block" attribs in + Xml.Element (tag, attribs, children) + | "stay" -> + assert (children=[]); + let attribs = transform_values ["wp"; "vmode"] affine env attribs in + let attribs = prefix_value prefix "wp" attribs in + let attribs = transform_attribs affine attribs in + Xml.Element (tag, attribs, children) + | _ -> failwith (Printf.sprintf "Fp_proc: Unexpected tag: '%s'" tag) + end + | _ -> failwith "Fp_proc: Xml.Element expected" + in + tr xml + +let transform_block = fun prefix reroutes affine env xml -> + Xml.Element (Xml.tag xml, + ["name", prefix (ExtXml.attrib xml "name")], + List.map (transform_stage prefix reroutes affine env) (Xml.children xml)) + + +let check_params = fun params env -> + List.iter + (fun p -> + if not (List.mem_assoc p env) then begin + Printf.fprintf stderr "Parameter '%s' is missing\n" p; + exit 1 + end) + params + +let parse_include = fun dir include_xml -> + let f = Filename.concat dir (ExtXml.attrib include_xml "procedure") in + let proc_name = ExtXml.attrib include_xml "name" in + let prefix = fun x -> proc_name ^ "." ^ x in + let affine = { + dx = float_attrib include_xml "x"; + dy = float_attrib include_xml "y"; + angle = float_attrib include_xml "rotate" + } in + let reroutes = + List.filter + (fun x -> String.lowercase (Xml.tag x) = "with") + (Xml.children include_xml) in + let reroutes = List.map + (fun xml -> (ExtXml.attrib xml "from", ExtXml.attrib xml "to")) + reroutes in + let args = + List.filter + (fun x -> String.lowercase (Xml.tag x) = "arg") + (Xml.children include_xml) in + let env = List.map + (fun xml -> (ExtXml.attrib xml "name", ExtXml.attrib xml "value")) + args in + try + let proc = Xml.parse_file f in + let params = List.filter + (fun x -> String.lowercase (Xml.tag x) = "param") + (Xml.children proc) in + let value = fun xml env -> + let name = ExtXml.attrib xml "name" in + try + (name, List.assoc name env) + with + Not_found -> + try + (name, Xml.attrib xml "default_value") + with + _ -> failwith (Printf.sprintf "Value required for param '%s' in %s" name (Xml.to_string include_xml)) in + (* Complete the environment with default values *) + let env = List.map (fun xml -> value xml env) params in + + let waypoints = Xml.children (ExtXml.child proc "waypoints") + and blocks = Xml.children (ExtXml.child proc "blocks") in + + let waypoints = List.map (transform_waypoint prefix affine) waypoints in + let blocks = List.map (transform_block prefix reroutes affine env) blocks in + (waypoints, blocks) + with + Dtd.Prove_error e -> dtd_error f (Dtd.prove_error e) + | Dtd.Check_error e -> dtd_error f (Dtd.check_error e) + + +(** Adds new children to a list of XML elements *) +let insert_children = fun xmls new_children_assoc -> + List.map + (fun x -> + try + let new_children = List.assoc (Xml.tag x) new_children_assoc + and old_children = Xml.children x in + Xml.Element (Xml.tag x, Xml.attribs x, old_children @ new_children) + with + Not_found -> x + ) + xmls + +let replace_children = fun xml new_children_assoc -> + Xml.Element (Xml.tag xml, Xml.attribs xml, + List.map + (fun x -> + try + let new_children = List.assoc (Xml.tag x) new_children_assoc in + new_children + with + Not_found -> x + ) + (Xml.children xml)) + + +let process_includes = fun dir xml -> + let includes, children = + List.partition (fun x -> Xml.tag x = "include") (Xml.children xml) in + + (* List of pairs of list (waypoints, blocks) *) + let waypoints_and_blocks = List.map (parse_include dir) includes in + + let (inc_waypoints, inc_blocks) = List.split waypoints_and_blocks in + let inc_waypoints = List.flatten inc_waypoints + and inc_blocks = List.flatten inc_blocks in + + let new_children = insert_children children + ["waypoints", inc_waypoints; "blocks", inc_blocks] in + + Xml.Element (Xml.tag xml, Xml.attribs xml, new_children) + +let remove_attribs = fun xml names -> + List.filter (fun (x,_) -> not (List.mem (String.lowercase x) names)) (Xml.attribs xml) + +let xml_assoc_attrib = fun a v xmls -> + match List.filter (fun x -> ExtXml.attrib x a = v) xmls with + p::_ -> p + | _ -> failwith "xml_assoc_attrib" + +let coords_of_waypoint = fun wp -> + (float_attrib wp "x", float_attrib wp "y") + + +let new_waypoint = fun wp qdr dist waypoints -> + let wp_xml = xml_assoc_attrib "name" wp !waypoints in + let wpx, wpy = coords_of_waypoint wp_xml in + let a = (Deg>>Rad)(90. -. qdr) in + let x = string_of_float (wpx +. dist *. cos a) + and y = string_of_float (wpy +. dist *. sin a) in + let name = Printf.sprintf "%s_%.0f_%.0f" wp qdr dist in + let alt = try ["alt", Xml.attrib wp_xml "alt"] with _ -> [] in + waypoints := Xml.Element("waypoint", ["name", name; "x", x; "y", y]@alt, []) :: !waypoints; + name + + +let replace_wp = fun stage waypoints -> + try + let qdr = float_attrib stage "wp_qdr" + and dist = float_attrib stage "wp_dist" in + let wp = ExtXml.attrib stage "wp" in + + let name = new_waypoint wp qdr dist waypoints in + + let other_attribs = remove_attribs stage ["wp";"wp_qdr";"wp_dist"] in + Xml.Element (Xml.tag stage, ("wp", name)::other_attribs, []) + with + _ -> stage + + +let replace_from = fun stage waypoints -> + try + let qdr = float_attrib stage "from_qdr" + and dist = float_attrib stage "from_dist" in + let wp = ExtXml.attrib stage "from" in + + let name = new_waypoint wp qdr dist waypoints in + + let other_attribs = remove_attribs stage ["from";"from_qdr";"from_dist"] in + Xml.Element (Xml.tag stage, ("from", name)::other_attribs, []) + with + _ -> stage + + +let process_stage = fun stage waypoints -> + let rec do_it = fun stage -> + match String.lowercase (Xml.tag stage) with + "go" | "stay" | "circle" -> + replace_from (replace_wp stage waypoints) waypoints + + | "while" -> + Xml.Element("while", Xml.attribs stage, List.map do_it (Xml.children stage)) + | _ -> stage in + do_it stage + + +let process_relative_waypoints = fun xml -> + let waypoints = (ExtXml.child xml "waypoints") + and blocks = ExtXml.child xml "blocks" in + + let blocks_list = Xml.children blocks in + + let waypoints_list = ref (Xml.children waypoints) in + + let blocks_list = + List.map + (fun block -> + let new_children = + List.map + (fun stage -> process_stage stage waypoints_list) + (Xml.children block) in + Xml.Element (Xml.tag block, Xml.attribs block, new_children) + ) + blocks_list in + + let new_waypoints = Xml.Element ("waypoints", Xml.attribs waypoints, !waypoints_list) + and blocks = Xml.Element ("blocks", Xml.attribs blocks, blocks_list) in + + replace_children xml ["waypoints", new_waypoints; "blocks", blocks] + diff --git a/sw/tools/fp_syntax.ml b/sw/tools/fp_syntax.ml new file mode 100644 index 0000000000..c5c3d0b4f4 --- /dev/null +++ b/sw/tools/fp_syntax.ml @@ -0,0 +1,77 @@ +(* + $Id$ + + Syntax of flight plan expressions +*) + +open Printf + +type ident = string + +type operator = string +type expression = + | Ident of ident + | Int of int + | Float of float + | Call of ident * (expression list) + | Index of ident * expression + +(* Valid unary and binary opetarors *) +let binary_operators = ["+"; ">"; "-"] +let unary_operators = ["!"; "-"] + +let is_binary = fun op -> List.mem op binary_operators +let is_unary = fun op -> List.mem op unary_operators + +let rec sprint_expression = function + Ident i -> sprintf "%s" i + | Int i -> sprintf "%d" i + | Float i -> sprintf "%f" i + | Call (op, [e1;e2]) when is_binary op -> + sprintf "(" ^ sprint_expression e1 ^ op ^ sprint_expression e2 ^ ")" + | Call (op, [e1]) when is_unary op -> + sprintf "%s(%s)" op (sprint_expression e1) + | Call (i, es) -> + let ses = List.map sprint_expression es in + sprintf "%s(" i ^ String.concat "," ses ^ ")" + | Index (i,e) -> sprintf "%s[" i ^ sprint_expression e ^ "]" + +(* Valid functions *) +let functions = [ + "Qdr"; + "And"; + "Or"; + "RcEvent1"; + "RcEvent2"] @ binary_operators @ unary_operators + +(* Valid identifiers *) +let variables = [ + "launch"; + "estimator_z"; + "estimator_flight_time"; + "stage_time"; + "block_time"; + "SECURITY_ALT"; + "GROUND_ALT"; + "TRUE"; + "QFU" +] + +exception Unknown_ident of string +exception Unknown_operator of string +exception Unknown_function of string + +let rec check_expression = fun e -> + match e with + Ident i -> + if not (List.mem i variables) then + raise (Unknown_ident i) + | Int _ | Float _ -> () + | Call (i, es) -> + if not (List.mem i functions) then + raise (Unknown_function i); + List.iter check_expression es + | Index (i,e) -> + if not (List.mem i variables) then + raise (Unknown_ident i); + check_expression e diff --git a/sw/tools/fp_syntax.mli b/sw/tools/fp_syntax.mli new file mode 100644 index 0000000000..e56dc161d7 --- /dev/null +++ b/sw/tools/fp_syntax.mli @@ -0,0 +1,44 @@ +(* + * $Id$ + * + * Syntax of flight plan parsed expressions + * + * Copyright (C) 2004 CENA/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. + * + *) + +type ident = string +type operator = string +type expression = + | Ident of ident + | Int of int + | Float of float + | Call of ident * expression list + | Index of ident * expression + +val sprint_expression : expression -> string + +exception Unknown_ident of string +exception Unknown_operator of string +exception Unknown_function of string + +val check_expression : expression -> unit +(** May raise [Unknown_ident], [Unknown_operator] or [Unknown_function] + exceptions *) diff --git a/sw/tools/gen_aircraft.ml b/sw/tools/gen_aircraft.ml new file mode 100644 index 0000000000..14dd098492 --- /dev/null +++ b/sw/tools/gen_aircraft.ml @@ -0,0 +1,33 @@ +open Printf + +let (//) = Filename.concat + +let paparazzi_home = Env.paparazzi_home +let conf_xml = paparazzi_home // "conf" // "conf.xml" + +let mkdir = fun d -> + if not (Sys.file_exists d) then + Unix.mkdir d 0o755 + +let _ = + let aircraft = Sys.argv.(1) in + let conf = Xml.parse_file conf_xml in + let aircraft_xml = + try + ExtXml.child conf ~select:(fun x -> Xml.attrib x "name" = aircraft) "aircraft" + with + Not_found -> failwith (sprintf "Aircraft '%s' not found in '%s'" aircraft conf_xml) + + in + let value = ExtXml.attrib aircraft_xml in + + let aircraft_dir = paparazzi_home // "var" // aircraft in + + mkdir aircraft_dir; + mkdir (aircraft_dir // "fbw"); + mkdir (aircraft_dir // "autopilot"); + mkdir (aircraft_dir // "sim"); + + let c = sprintf "make -f Makefile.ac AIRCRAFT=%s AIRFRAME=%s RADIO=%s FLIGHT_PLAN=%s" aircraft (value "airframe") (value "radio") (value "flight_plan") in + prerr_endline c; + exit (Sys.command c) diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml new file mode 100644 index 0000000000..9b565c39cc --- /dev/null +++ b/sw/tools/gen_airframe.ml @@ -0,0 +1,157 @@ +(* + * $Id$ + * + * XML preprocessing for airframe parameters + * + * 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. + * + *) + +let command_travel = 1200. (* !!!! From link_autopilot.h !!!! *) +let nb_servo_4017 = 10 (* From servo.h *) + +open Printf +open Xml2h + + +type channel = { min : float; max : float; neutral : float } + +let fos = float_of_string +let sof = fun x -> if mod_float x 1. = 0. then Printf.sprintf "%.0f" x else string_of_float x +let soi = string_of_int + +let define_macro name n x = + let a = fun s -> ExtXml.attrib x s in + printf "#define %s(" name; + match n with (* Do we really need more ??? *) + 1 -> printf "x1) (%s*(x1))\n" (a "coeff1") + | 2 -> printf "x1,x2) (%s*(x1)+ %s*(x2))\n" (a "coeff1") (a "coeff2") + | 3 -> printf "x1,x2,x3) (%s*(x1)+ %s*(x2)+%s*(x3))\n" (a "coeff1") (a "coeff2") (a "coeff3") + | _ -> failwith "define_macro" + +let parse_element = fun prefix s -> + match Xml.tag s with + "define" -> + define (prefix^ExtXml.attrib s "name") (ExtXml.attrib s "value") + | "linear" -> + let name = ExtXml.attrib s "name" + and n = int_of_string (ExtXml.attrib s "arity") in + define_macro (prefix^name) n s + | _ -> xml_error "define|linear" + + +let parse_servo = + fun default_min default_neutral default_max servo_params c -> + let name = "SERVO_"^ExtXml.attrib c "name" in + let no_servo = int_of_string (ExtXml.attrib c "no") in + define name (string_of_int no_servo); + let min = fos (ExtXml.attrib_or_default c "min" (sof default_min)) + and neutral = fos (ExtXml.attrib_or_default c "neutral" (sof default_neutral)) + and max = fos (ExtXml.attrib_or_default c "max" (sof default_max)) in + + let travel = (max-.min) /. command_travel in + define (name^"_TRAVEL") (sof travel); + define (sprintf "SERVOS_NEUTRALS_%d" no_servo) (sof neutral); + nl (); + + servo_params.(no_servo) <- { min = min; neutral = neutral; max = max } + + +let pprz_value = Str.regexp "@\\([A-Z]+\\)" +let var_value = Str.regexp "\\$\\([_a-z0-9]+\\)" +let preprocess_command = fun s -> + let s = Str.global_replace pprz_value "values[RADIO_\\1]" s in + Str.global_replace var_value "_var_\\1" s + +let parse_command = fun command -> + let a = fun s -> ExtXml.attrib command s in + match Xml.tag command with + "set" -> + let servo = a "servo" + and value = a "value" in + let v = preprocess_command value in + printf " servo_value = SERVO_NEUTRAL(SERVO_%s) + (int16_t)((%s)*SERVO_%s_TRAVEL);\\\n" servo v servo; + printf " servo_widths[SERVO_%s] = ChopServo(servo_value);\\\n\\\n" servo + | "let" -> + let var = a "var" + and value = a "value" in + let v = preprocess_command value in + printf " int16_t _var_%s = %s;\\\n" var v + | _ -> xml_error "set|let" + +let parse_section = fun s -> + match Xml.tag s with + "section" -> + let prefix = ExtXml.attrib_or_default s "prefix" "" in + List.iter (parse_element prefix) (Xml.children s); + nl () + | "servos" -> + let get_float = fun x -> float_of_string (ExtXml.attrib s x) in + let min = get_float "min" + and neutral = get_float "neutral" + and max = get_float "max" in + + let servos = Xml.children s in + define "NB_SERVO" (string_of_int (List.length servos)); + nl (); + let servos_params = Array.create nb_servo_4017 { min = min; neutral = neutral; max = max } in + + List.iter (parse_servo min neutral max servos_params) servos; + + let servos_params = Array.to_list servos_params in + + nl (); + define "SERVOS_MINS" (sprint_float_array (List.map (fun x -> sof x.min) servos_params)); + define "SERVOS_NEUTRALS" (sprint_float_array (List.map (fun x -> sof x.neutral) servos_params)); + define "SERVOS_MAXS" (sprint_float_array (List.map (fun x -> sof x.max) servos_params)); + nl (); + + (* For compatibility *) + define "SERVO_MIN_US" (sprintf "%.0ful" min); + define "SERVO_MAX_US" (sprintf "%.0ful" max); + nl () + | "command" -> + printf "#define ServoSet(values) { \\\n"; + printf " uint16_t servo_value;\\\n"; + List.iter parse_command (Xml.children s); + printf "}\n" + | _ -> xml_error "param|servos|command" + + +let h_name = "AIRFRAME_H" + +let _ = + if Array.length Sys.argv <> 3 then + failwith (Printf.sprintf "Usage: %s A/C_ident xml_file" Sys.argv.(0)); + let xml_file = Sys.argv.(2) + and ac_name = Sys.argv.(1) in + try + let xml = start_and_begin xml_file h_name in + Xml2h.warning ("AIRFRAME MODEL: "^ ac_name); + define_string "AIRFRAME_NAME" ac_name; + nl (); + let v = ExtXml.attrib xml "ctl_board" in + define ("CTL_BRD_"^v) "1"; + nl (); + List.iter parse_section (Xml.children xml); + finish h_name + with + Xml.Error e -> prerr_endline (Xml.error e) + diff --git a/sw/tools/gen_calib.ml b/sw/tools/gen_calib.ml new file mode 100644 index 0000000000..3eaa05113d --- /dev/null +++ b/sw/tools/gen_calib.ml @@ -0,0 +1,101 @@ +(* + * $Id$ + * + * XML preprocessing for in flight calibration + * + * 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. + * + *) + +open Printf +open Xml2h + +let margin = ref 0 +let step = 2 +let tab () = printf "%s" (String.make !margin ' ') +let right () = margin := !margin + step +let left () = margin := !margin - step + +let lprintf = fun f -> tab (); printf f + + +let calib_mode_of_rc = function + "gain_1_up" -> 1, "up" + | "gain_1_down" -> 1, "down" + | "gain_2_up" -> 2, "up" + | "gain_2_down" -> 2, "down" + | x -> failwith (sprintf "Unknown rc: %s" x) + +let param_macro_of_type = fun x -> "ParamVal"^String.capitalize x + +let inttype = function + "int16" -> "int16_t" + | "float" -> "float" + | x -> failwith (sprintf "Gen_calib.inttype: unknown type '%s'" x) + +let parse_setting = fun xml -> + let cursor, cm = calib_mode_of_rc (ExtXml.attrib xml "rc") + and var = ExtXml.attrib xml "var" + and range = float_of_string (ExtXml.attrib xml "range") in + let t = (ExtXml.attrib xml "type") in + let param_macro = param_macro_of_type t in + let var_init = var ^ "_init" in + + lprintf "if (inflight_calib_mode == IF_CALIB_MODE_%s) {\n" (String.uppercase cm); + right (); + lprintf "static %s %s;\n" (inttype t) var_init; + lprintf "if (mode_changed) {\n"; + right (); + lprintf "%s = %s;\n" var_init var; + lprintf "slider%d_init = from_fbw.channels[RADIO_GAIN%d];\n" cursor cursor; + left (); lprintf "}\n"; + lprintf "%s = %s(%s, %f, from_fbw.channels[RADIO_GAIN%d], slider%d_init);\n" var param_macro var_init range cursor cursor; + lprintf "slider_%d_val = (float)%s;\n" cursor var; + left (); lprintf "}\n" + + +let parse_mode = fun xml -> + lprintf "if (pprz_mode == PPRZ_MODE_%s) {\n" (ExtXml.attrib xml "name"); + right (); + List.iter parse_setting (Xml.children xml); + left (); lprintf "}\n" + +let parse_modes = fun xml -> + List.iter parse_mode (Xml.children xml) + + +let _ = + if Array.length Sys.argv < 2 then + failwith (Printf.sprintf "Usage: %s xml_file" Sys.argv.(0)); + let xml_file = Sys.argv.(1) in + let h_name = "INFLIGHT_CALIB_H" in + try + let xml = start_and_begin xml_file h_name in + + let rc_control = try ExtXml.child xml "rc_control" with Not_found -> failwith (sprintf "Error: 'rc_control' child expected in %s" (Xml.to_string xml)) in + lprintf "void inflight_calib(bool_t mode_changed) {\n"; + right (); + parse_modes rc_control; + left (); lprintf "}\n"; + + finish h_name + with + Xml.Error e -> prerr_endline (Xml.error e) + | Dtd.Prove_error e -> prerr_endline (Dtd.prove_error e); exit 1 diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml new file mode 100644 index 0000000000..dc56388f09 --- /dev/null +++ b/sw/tools/gen_flight_plan.ml @@ -0,0 +1,450 @@ +(* + * $Id$ + * + * Flight plan preprocessing (from XML to C) + * + * Copyright (C) 2004 CENA/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 +open Latlong + +let check_expressions = ref true + +let parse_expression = Fp_proc.parse_expression + +let parse = fun s -> + if !check_expressions then + let e = parse_expression s in + let unexpected = fun kind x -> + fprintf stderr "Paring error in '%s': unexpected %s: '%s' \n" s kind x; + exit 1 in + begin + try + Fp_syntax.check_expression e + with + Fp_syntax.Unknown_operator x -> unexpected "operator" x + | Fp_syntax.Unknown_ident x -> unexpected "ident" x + | Fp_syntax.Unknown_function x -> unexpected "function" x + end; + Fp_syntax.sprint_expression e + else + s + +let parsed_attrib = fun xml a -> + parse (ExtXml.attrib xml a) + +let pi = atan 1. *. 4. + +let radE8_of_deg = fun d -> + d /. 180. *. pi *. 1e8 + +let rad_of_deg = fun d -> + d /. 180. *. pi + +let gen_label = + let x = ref 0 in + fun p -> incr x; sprintf "%s_%d" p !x + +let margin = ref 0 +let step = 2 + +let right () = margin := !margin + step +let left () = margin := !margin - step + +let lprintf = fun f -> + printf "%s" (String.make !margin ' '); + printf f + +let float_attrib = fun xml a -> float_of_string (Xml.attrib xml a) +let int_attrib = fun xml a -> int_of_string (Xml.attrib xml a) +let name_of = fun wp -> ExtXml.attrib wp "name" + + +let ground_alt = ref 0. +let security_height = ref 0. + +let check_altitude = fun a x -> + if a < !ground_alt +. !security_height then begin + fprintf stderr "\nWARNING: low altitude (%.0f<%.0f+%.0f) in %s\n\n" a !ground_alt !security_height (Xml.to_string x) + end + +let print_waypoint = fun rel_utm_of_wgs84 default_alt waypoint -> + let (x, y) = + try + rel_utm_of_wgs84 {posn_lat=(Deg>>Rad)(float_attrib waypoint "lat"); + posn_long=(Deg>>Rad)(float_attrib waypoint "lon") } + with + Xml.No_attribute "lat" | Xml.No_attribute "lon" -> + (float_attrib waypoint "x", float_attrib waypoint "y") + and alt = try Xml.attrib waypoint "alt" with _ -> default_alt in + check_altitude (float_of_string alt) waypoint; + printf " {%.1f, %.1f, %s},\\\n" x y alt + + +let blocks = Hashtbl.create 11 +let index_of_blocks = ref [] +let stages = Hashtbl.create 11 + +let get_index_block = fun x -> + try + string_of_int (List.assoc x !index_of_blocks) + with + Not_found -> failwith (sprintf "Unknown block: '%s'" x) + +let print_exception = fun x -> + let i = get_index_block (ExtXml.attrib x "deroute") in + let c = parsed_attrib x "cond" in + lprintf "if %s { GotoBlock(%s) }\n" c i + +let return_from_excpt l = Xml.Element ("return_from_excpt", ["name",l], []) +let goto l = Xml.Element ("goto", ["name",l], []) +let exit_block = Xml.Element ("exit_block", [], []) + +let stage = ref 0 + +let output_label l = lprintf "Label(%s)\n" l + +let output_vmode x wp last_wp = + let pitch = try parsed_attrib x "pitch" with _ -> "0.0" in + lprintf "nav_pitch = %s;\n" pitch; + let vmode = try ExtXml.attrib x "vmode" with _ -> "alt" in + begin + match vmode with + "climb" -> + lprintf "vertical_mode = VERTICAL_MODE_AUTO_CLIMB;\n"; + lprintf "desired_climb = %s;\n" (parsed_attrib x "climb") + | "alt" -> + lprintf "vertical_mode = VERTICAL_MODE_AUTO_ALT;\n"; + let alt = + try + let a = parsed_attrib x "alt" in + begin + try + check_altitude (float_of_string a) x + with + Failure "float_of_string" -> () + end; + a + with _ -> + if wp = "" then failwith "alt or waypoint required in alt vmode" else + sprintf "waypoints[%s].a" wp in + lprintf "desired_altitude = %s;\n" alt; + lprintf "pre_climb = 0.;\n" + | "glide" -> + lprintf "vertical_mode = VERTICAL_MODE_AUTO_ALT;\n"; + lprintf "glide_to(%s, %s);\n" last_wp wp + | "gaz" -> + lprintf "vertical_mode = VERTICAL_MODE_AUTO_GAZ;\n"; + lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz") + | x -> failwith (sprintf "Unknown vmode '%s'" x) + end; + vmode + +let output_hmode x wp last_wp = + try + let hmode = ExtXml.attrib x "hmode" in + begin + match hmode with + "route" -> + if last_wp = "last_wp" then + fprintf stderr "WARNING: Deprecated use of 'route' using last waypoint in %s\n"(Xml.to_string x); + lprintf "route_to(%s, %s);\n" last_wp wp + | "direct" -> lprintf "fly_to(%s);\n" wp + | x -> failwith (sprintf "Unknown hmode '%s'" x) + end; + hmode + with + ExtXml.Error _ -> lprintf "fly_to(%s);\n" wp; "direct" (* Default behaviour *) + + +let get_index_waypoint = fun x l -> + try + string_of_int (List.assoc x l) + with + Not_found -> failwith (sprintf "Unknown waypoint: %s\n" x) + + +let rec compile_stage = fun block x -> + incr stage; + Hashtbl.add stages x (block, !stage); + begin + match Xml.tag x with + "while" -> + List.iter (compile_stage block) (Xml.children x); + incr stage (* To count the loop stage *) + | "return_from_excpt" | "goto" | "deroute" | "exit_block" + | "heading" | "go" | "stay" | "xyz" | "circle" -> () + | s -> failwith (sprintf "Unknown stage: %s\n" s) + end + +let rec print_stage = fun index_of_waypoints x -> + incr stage; + let stage () = lprintf "Stage(%d)\n" !stage; right () in + begin + match String.lowercase (Xml.tag x) with + "return_from_excpt" -> + stage (); + lprintf "ReturnFromException(%s)\n" (name_of x) + | "goto" -> + stage (); + lprintf "Goto(%s)\n" (name_of x) + | "deroute" -> + stage (); + lprintf "GotoBlock(%s)\n" (get_index_block (ExtXml.attrib x "block")) + | "exit_block" -> + stage (); + lprintf "NextBlock()\n" + | "while" -> + let w = gen_label "while" in + let e = gen_label "endwhile" in + output_label w; + stage (); + let c = try parsed_attrib x "cond" with _ -> "TRUE" in + lprintf "if (! (%s)) Goto(%s) else NextStage();\n" c e; + List.iter (print_stage index_of_waypoints) (Xml.children x); + print_stage index_of_waypoints (goto w); + output_label e + | "heading" -> + stage (); + 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"); + ignore (output_vmode x "" ""); + left (); lprintf "}\n"; + lprintf "return;\n" + | "go" -> + stage (); + let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in + lprintf "if (approaching(%s)) NextStageFrom(%s) else {\n" wp wp; + right (); + let last_wp = + try + get_index_waypoint (ExtXml.attrib x "from") index_of_waypoints + with _ -> "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 + failwith "glide vmode requires route hmode"; + left (); lprintf "}\n"; + lprintf "return;\n" + | "stay" -> + stage (); + begin + try + let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in + ignore(output_hmode x wp ""); + ignore (output_vmode x wp ""); + with + Xml2h.Error _ -> + lprintf "fly_to_xy(last_x, last_y);\n"; + ignore(output_vmode x "" "") + end; + lprintf "return;\n" + | "xyz" -> + stage (); + let r = try parsed_attrib x "radius" with _ -> "100" in + lprintf "Goto3D(%s)\n" r; + lprintf "return;\n" + | "circle" -> + stage (); + let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in + let r = parsed_attrib x "radius" in + let vmode = output_vmode x wp "" in + lprintf "Circle(%s, %s);\n" wp r; + begin + try + let c = parsed_attrib x "until" in + lprintf "if (%s) NextStage();\n" c + with + ExtXml.Error _ -> () + end; + lprintf "return;\n" + | s -> failwith "Unreachable" + end; + left () + +let compile_block = fun block_num (b:Xml.xml) -> + Hashtbl.add blocks b block_num; + index_of_blocks := (name_of b, block_num) :: !index_of_blocks; + stage := (-1); + let stages = + List.filter (fun x -> Xml.tag x <> "exception") (Xml.children b) in + + List.iter (compile_stage block_num) stages; + + compile_stage block_num exit_block + +let compile_blocks = fun bs -> + let block = ref (-1) in + List.iter + (fun b -> + incr block; + compile_block !block b) + bs + + + +let print_block = fun index_of_waypoints (b:Xml.xml) block_num -> + let n = name_of b in + lprintf "Block(%d) // %s\n" block_num n; + + let excpts, stages = + List.partition (fun x -> Xml.tag x = "exception") (Xml.children b) in + + List.iter print_exception excpts; + + lprintf "switch(nav_stage) {\n"; + right (); + stage := (-1); + List.iter (print_stage index_of_waypoints) stages; + + print_stage index_of_waypoints exit_block; + + left (); lprintf "}\n\n" + + + +let print_blocks = fun index_of_waypoints bs -> + lprintf "#ifdef NAV_C\n"; + lprintf "\nstatic inline void auto_nav(void) {\n"; + right (); + lprintf "switch (nav_block) {\n"; + right (); + let block = ref (-1) in + List.iter (fun b -> incr block; print_block index_of_waypoints b !block) bs; + left (); lprintf "}\n"; + left (); lprintf "}\n"; + lprintf "#endif // NAV_C\n" + + +let define_home = fun waypoints -> + let rec loop i = function + [] -> failwith "Waypoint 'HOME' required" + | w::ws -> + if name_of w = "HOME" then begin + Xml2h.define "WP_HOME" (string_of_int i); + (float_attrib w "x", float_attrib w "y") + end else + loop (i+1) ws in + loop 0 waypoints + +let check_distance = fun (hx, hy) max_d wp -> + let x = float_attrib wp "x" + and y = float_attrib wp "y" in + let d = sqrt ((x-.hx)**2. +. (y-.hy)**2.) in + if d > max_d then + fprintf stderr "\nWARNING: Waypoint '%s' too far from HOME (%.0f>%.0f)\n\n" (name_of wp) d max_d + + + +let _ = + let xml_file = ref "fligh_plan.xml" + and dump = ref false in + Arg.parse [("-dump", Arg.Set dump, "Dump compile result"); + ("-nocheck", Arg.Clear check_expressions, "Disable expression checking")] + (fun f -> xml_file := f) + "Usage:"; + try + let xml = Xml.parse_file !xml_file in + let dir = Filename.dirname !xml_file in + let xml = Fp_proc.process_includes dir xml in + (*** prerr_endline (Xml.to_string_fmt xml); prerr_endline "\n\n\n"; ***) + let xml = Fp_proc.process_relative_waypoints xml in + let waypoints = ExtXml.child xml "waypoints" + and blocks = Xml.children (ExtXml.child xml "blocks") in + + compile_blocks blocks; + + if !dump then + let block_names = List.map (fun (x,y) -> (y, x)) !index_of_blocks in + let lstages = ref [] in + Hashtbl.iter + (fun xml (b,s) -> + lstages := + Xml.Element ("stage", [ "block", string_of_int b; + "block_name", List.assoc b block_names; + "stage", string_of_int s], [xml]) + :: !lstages) + stages; + let xml_stages = Xml.Element ("stages", [], !lstages) in + let dump_xml = Xml.Element ("dump", [], [xml; xml_stages]) in + printf "%s\n" (ExtXml.to_string_fmt dump_xml) + else begin + let h_name = "FLIGHT_PLAN_H" in + printf "/* This file has been generated from %s */\n" !xml_file; + printf "/* Please DO NOT EDIT */\n\n"; + + printf "#ifndef %s\n" h_name; + Xml2h.define h_name ""; + printf "\n"; + + + let name = ExtXml.attrib xml "name" in + Xml2h.warning ("FLIGHT PLAN: "^name); + Xml2h.define_string "FLIGHT_PLAN_NAME" name; + + let get_float = fun x -> float_attrib xml x in + let lat0_deg = get_float "lat0" + and lon0_deg = get_float "lon0" + and qfu = get_float "qfu" + and mdfh = get_float "max_dist_from_home" + and alt = ExtXml.attrib xml "alt" in + security_height := get_float "security_height"; + ground_alt := get_float "ground_alt"; + + check_altitude (float_of_string alt) xml; + + let utm0 = utm_of WGS84 {posn_lat=(Deg>>Rad)lat0_deg;posn_long=(Deg>>Rad)lon0_deg } in + let rel_utm_of_wgs84 = fun wgs84 -> + let utm = utm_of WGS84 wgs84 in + (utm.utm_x -. utm0.utm_x, utm.utm_y -. utm0.utm_y) in + + Xml2h.define "NAV_UTM_EAST0" (sprintf "%.0f" utm0.utm_x); + Xml2h.define "NAV_UTM_NORTH0" (sprintf "%.0f" utm0.utm_y); + Xml2h.define "QFU" (sprintf "%.1f" qfu); + + + let waypoints = Xml.children waypoints in + let (hx, hy) = define_home waypoints in + List.iter (check_distance (hx, hy) mdfh) waypoints; + + Xml2h.define "WAYPOINTS" "{ \\"; + List.iter (print_waypoint rel_utm_of_wgs84 alt) waypoints; + lprintf "};\n"; + Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); + + Xml2h.define "GROUND_ALT" (string_of_float !ground_alt); + Xml2h.define "SECURITY_ALT" (string_of_float (!security_height +. !ground_alt)); + Xml2h.define "MAX_DIST_FROM_HOME" (string_of_float mdfh); + + let index_of_waypoints = + let i = ref (-1) in + List.map (fun w -> incr i; (name_of w, !i)) waypoints in + + print_blocks index_of_waypoints blocks; + + Xml2h.finish h_name + end + with + Xml.Error e -> prerr_endline (Xml.error e); exit 1 + | Dtd.Prove_error e -> prerr_endline (Dtd.prove_error e); exit 1 diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml new file mode 100644 index 0000000000..d0e61e25b9 --- /dev/null +++ b/sw/tools/gen_messages.ml @@ -0,0 +1,254 @@ +(* + * $Id$ + * + * XML preprocessing for downlink protocol + * + * 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. + * + *) + +open Printf + + +module Syntax = struct + type format = string + + type type_name = string + + type _type = Basic of string | Array of string * int + + type field = _type * string * format option + + type fields = field list + + type message = { name : string ; id : int; period : float option; fields : fields } + + type messages = message list + + let lineno = ref 1 (* For syntax error messages *) + + let assoc_types t = + try + List.assoc t Pprz.types + with + Not_found -> fprintf stderr "Error: '%s' unknown type\n" t; exit 1 + + let rec sizeof = function + Basic t -> (assoc_types t).Pprz.size + | Array (t, i) -> i * sizeof (Basic t) + let glibof = fun t -> (assoc_types t).Pprz.glib_type + let formatof = fun t -> (assoc_types t).Pprz.format + + let print_format t = function + None -> printf "(%s)" (formatof t) + | Some f -> printf "(%s)" f + + let print_field = fun (t, s, f) -> + begin + match t with + Basic t -> printf "%s %s " t s; print_format t f + | Array(t, i) -> printf "%s %s[%d] " t s i; print_format t f + end; printf "\n" + + let print_message = fun (s, fields) -> + printf "%s {\n" s; + List.iter print_field fields; + printf "}\n" + + let print_messages = List.iter print_message + + open Xml + + let xml_error s = failwith ("Bad XML tag: "^s^ " expected") + + let fprint_fields = fun f l -> + fprintf f "<"; + List.iter (fun (a, b) -> fprintf f "%s=\"%s\" " a b) l; + fprintf f ">" + + + let assoc_or_fail x l = + let x = String.uppercase x + and l = List.map (fun (a, v) -> String.uppercase a, v) l in + try + List.assoc x l + with + Not_found -> + fprintf stderr "Error: Field '%s' expected in <%a>" x fprint_fields l; + exit 1 + + let of_xml = function + Element ("message", fields, l) -> + let name = assoc_or_fail "name" fields + and id = int_of_string (assoc_or_fail "id" fields) in + { id=id; name = name; + period = (try Some (float_of_string (List.assoc "period" fields)) with Not_found -> None); + fields=List.map (function + Element ("field", fields, []) -> + let id = assoc_or_fail "name" fields + and type_name = assoc_or_fail "type" fields + and fmt = try Some (List.assoc "format" fields) with _ -> None in + let _type = try Array(type_name, int_of_string (List.assoc "len" fields)) with Not_found -> Basic type_name in + + (_type, id, fmt) + | _ -> xml_error "field") + l} + | _ -> xml_error "message with id" + + + let read filename class_ = + let xml = + try Xml.parse_file filename with + Xml.Error (msg, pos) -> fprintf stderr "%s:%d : %s\n" filename (Xml.line pos) (Xml.error_msg msg); exit 1 + in + match List.filter (fun x -> assert(Xml.tag x="class"); Xml.attrib x "name" = class_) (Xml.children xml) with + [xml_class] -> List.map of_xml (Xml.children xml_class) + | [] -> failwith (sprintf "No class '%s' found" class_) + | _ -> failwith (sprintf "Several class '%s' found" class_) +end + +module Gen_onboard = struct + open Printf + open Syntax + + let print_avr_field = fun avr_h (t, name, (_f:format option)) -> + match t with + Basic _ -> + fprintf avr_h "\t MODEM_PUT_%d_BYTE_BY_ADDR((uint8_t*)(%s)); \\\n" (sizeof t) name + | Array (t, i) -> + let s = sizeof (Basic t) in + fprintf avr_h "\t {\\\n\t int i;\\\n\t for(i = 0; i < %d; i++) {\\\n" i; + fprintf avr_h "\t MODEM_PUT_%d_BYTE_BY_ADDR((uint8_t*)(&%s[i])); \\\n" s name; + fprintf avr_h "\t }\\\n"; + fprintf avr_h "\t }\\\n" + + let print_avr_macro_names avr_h = function + [] -> () + | (_, s, _)::fields -> + fprintf avr_h "%s" s; List.iter (fun (_, s, _) -> fprintf avr_h ", %s" s) fields + + let rec size_fields = fun fields size -> + match fields with + [] -> size + 4 + | (t, _, _)::fields -> size_fields fields (size + sizeof(t)) + + let size_of_message = fun m -> size_fields m.fields 0 + + let print_avr_macro = fun avr_h {name=s; fields = fields} -> + fprintf avr_h "#define DOWNLINK_SEND_%s(" s; + print_avr_macro_names avr_h fields; + fprintf avr_h "){ \\\n"; + fprintf avr_h "\tif (MODEM_CHECK_FREE_SPACE(%d)) {\\\n" (size_fields fields 0); + fprintf avr_h "\t ModemStartMessage(DL_%s) \\\n" s; + List.iter (print_avr_field avr_h) fields; + fprintf avr_h "\t ModemEndMessage() \\\n"; + fprintf avr_h "\t} \\\n"; + fprintf avr_h "\telse \\\n"; + fprintf avr_h "\t modem_nb_ovrn++; \\\n"; + fprintf avr_h "}\n\n" + + + + let print_enum = fun avr_h messages -> + List.iter (fun m -> fprintf avr_h "#define DL_%s %d\n" m.name m.id) messages; + fprintf avr_h "#define DL_MSG_NB %d\n\n" (List.length messages) + + let print_avr_macros = fun filename avr_h messages -> + print_enum avr_h messages; + List.iter (print_avr_macro avr_h) messages; + let md5sum = Digest.file filename in + fprintf avr_h "#define MESSAGES_MD5SUM \""; + for i = 0 to String.length md5sum - 1 do + fprintf avr_h "\\%03o" (Char.code md5sum.[i]) + done; + fprintf avr_h "\"\n" + + let freq = 10 + let buffer_length = 5 + let step = 1. /. float freq + let nb_steps = (256 / freq) * freq + + let is_periodic = fun m -> m.period <> None + let period_of = fun m -> + match m.period with Some p -> p | None -> failwith "period_of" + let morefrequent = fun m1 m2 -> compare (period_of m1) (period_of m2) + + let gen_periodic = fun avr_h messages -> + let periodic_messages = List.filter is_periodic messages in + let periodic_messages = List.sort morefrequent periodic_messages in + + let load = Array.create nb_steps 0 in + let buffer_load = Array.create nb_steps 0 in + + let scheduled_messages = + List.map + (fun m -> + let p = period_of m in + let period_steps = truncate (p /. step) in + let start_step = ref 0 in + for i = 1 to period_steps - 1 do + if (load.(i), buffer_load.(i)) < (load.(!start_step), buffer_load.(!start_step)) then start_step := i + done; + let s = size_of_message m in + for j = 0 to nb_steps/period_steps - 1 do + let i = !start_step+j*period_steps in + load.(i) <- load.(i) + s; + for k = i to i + buffer_length - 1 do + let k = (k + nb_steps) mod nb_steps in + buffer_load.(k) <- buffer_load.(k) + s + done + done; + (!start_step, period_steps, m)) + periodic_messages in + + fprintf avr_h "// Load: intant(buffer)"; + for i = 0 to nb_steps - 1 do + fprintf avr_h " %d(%d)" load.(i) buffer_load.(i) + done; + fprintf avr_h "\n"; + + fprintf avr_h "#define PeriodicSend() { /* %dHz */ \\\n" freq; + fprintf avr_h " static uint8_t i;\\\n"; + fprintf avr_h " i++; if (i == %d) i = 0;\\\n" nb_steps; + List.iter + (fun (s, p, m) -> + fprintf avr_h " if (i %% %d == %d) PERIODIC_SEND_%s();\\\n" p s m.name) + scheduled_messages; + fprintf avr_h "}\n" + + + +end + +let _ = + if Array.length Sys.argv <> 3 then begin + fprintf stderr "Usage: %s <.xml file> " Sys.argv.(0) + end; + let filename = Sys.argv.(1) in + let class_name = Sys.argv.(2) in + let base = Filename.basename (Filename.chop_extension filename) ^ class_name in + + let messages = Syntax.read filename class_name in + + let avr_h = stdout in + Printf.fprintf avr_h "/* Automatically generated from %s */\n" filename; + Printf.fprintf avr_h "/* Please DO NOT EDIT */\n"; + Gen_onboard.print_avr_macros filename avr_h messages; + Gen_onboard.gen_periodic avr_h messages diff --git a/sw/tools/gen_radio.ml b/sw/tools/gen_radio.ml new file mode 100644 index 0000000000..e296c13ffc --- /dev/null +++ b/sw/tools/gen_radio.ml @@ -0,0 +1,88 @@ +(* + * $Id$ + * + * XML preprocessing for radio-control parameters + * + * 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. + * + *) + +open Printf +open Xml2h + +let h_name = "RADIO_H" + +let fos = float_of_string + +type channel = { min : string; max : string; neutral : string; averaged : string } + +let default_neutral = "1600" +let default_min = "1000" +let default_max = "2200" + +let parse_channel = + let no_channel = ref 0 in + fun c -> + let ctl = "RADIO_CTL_"^ExtXml.attrib c "ctl" + and fct = "RADIO_" ^ ExtXml.attrib c "function" in + define ctl (string_of_int !no_channel); + define fct ctl; + no_channel := !no_channel + 1; + { min = ExtXml.attrib_or_default c "min" default_min; + neutral = ExtXml.attrib_or_default c "neutral" default_neutral; + max = ExtXml.attrib_or_default c "max" default_max; + averaged = ExtXml.attrib_or_default c "average" "0" + } + + +let _ = + if Array.length Sys.argv < 2 then + failwith "Usage: gen_radio xml_file"; + let xml_file = Sys.argv.(1) in + let xml = Xml.parse_file xml_file in + + printf "/* This file has been generated from %s */\n" xml_file; + printf "/* Please DO NOT EDIT */\n\n"; + printf "#ifndef %s\n" h_name; + define h_name ""; + nl (); + let channels = Xml.children xml in + let n = ExtXml.attrib xml "name" in + Xml2h.warning ("RADIO MODEL: "^n); + define_string "RADIO_NAME" n; + nl (); + define "RADIO_CTL_NB" (string_of_int (List.length channels)); + nl (); + + (* For compatibility *) + define "PPM_PULSE_NEUTRAL_US" default_neutral; + nl (); + let channels_params = List.map parse_channel channels in + nl (); + define "RADIO_MINS_US" (sprint_float_array (List.map (fun x -> x.min) channels_params)); + define "RADIO_NEUTRALS_US" (sprint_float_array (List.map (fun x -> x.neutral) channels_params)); + define "RADIO_MAXS_US" (sprint_float_array (List.map (fun x -> x.max) channels_params)); + define "RADIO_AVERAGED" (sprint_float_array (List.map (fun x -> x.averaged) channels_params)); + + nl (); + define "AveragedChannel(ch)" "(((int[])RADIO_AVERAGED)[ch])"; + + printf "\n#endif // %s\n" h_name + diff --git a/sw/tools/gen_ubx.ml b/sw/tools/gen_ubx.ml new file mode 100644 index 0000000000..4251a77007 --- /dev/null +++ b/sw/tools/gen_ubx.ml @@ -0,0 +1,126 @@ +(* + * $Id$ + * + * XML preprocessing for UBX protocol + * + * 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. + * + *) + +open Printf + +let out = stdout + +let sizeof = function + "U4" | "I4" -> 4 + | "U2" | "I2" -> 2 + | "U1" | "I1" -> 1 + | x -> failwith (sprintf "sizeof: unknown format '%s'" x) + +let (+=) = fun r x -> r := !r + x + +let get_at = fun offset format block_size -> + let t = + match format with + "I2" -> "int16_t" + | "I4" -> "int32_t" + | "U2" -> "uint16_t" + | "U4" -> "uint32_t" + | "U1" -> "uint8_t" + | "I1" -> "int8_t" + | _ -> failwith (sprintf "get_at: unknown format '%s'" format) in + let block_offset = + if block_size = 0 then "" else sprintf "+%d*_ubx_block" block_size in + sprintf "(*((%s*)(_ubx_payload+%d%s)))" t offset block_offset + +let define = fun x y -> + fprintf out "#define %s %s\n" x y + +exception Length_error of Xml.xml*int*int + + +let parse_class = fun c -> + let class_id = int_of_string (Xml.attrib c "id") + and class_name = Xml.attrib c "name" in + + fprintf out "\n"; + define (sprintf "UBX_%s_ID" class_name) (Xml.attrib c "ID"); + + let parse_message = fun m -> + let msg_name = Xml.attrib m "name" in + + fprintf out "\n"; + define (sprintf "UBX_%s_%s_ID" class_name msg_name) (Xml.attrib m "ID"); + + let offset = ref 0 in + let rec parse_field = fun block_size f -> + match Xml.tag f with + "field" -> + let field_name = Xml.attrib f "name" + and format = Xml.attrib f "format" in + let block_no = if block_size = 0 then "" else ",_ubx_block" in + define (sprintf "UBX_%s_%s_%s(_ubx_payload%s)" class_name msg_name field_name block_no) (get_at !offset format block_size); + offset += sizeof format + | "block" -> + let s = int_of_string (Xml.attrib f "length") in + let o = !offset in + List.iter (parse_field s) (Xml.children f); + let s' = !offset - o in + if s <> s' then raise (Length_error (f, s, s')) + | x -> failwith ("Unexpected field: " ^ x) + in + + List.iter (parse_field 0) (Xml.children m); + try + let l = int_of_string (Xml.attrib m "length") in + if l <> !offset then raise (Length_error (m, l, !offset)) + with + Xml.No_attribute("length") -> () + in + + + List.iter parse_message (Xml.children c) + + +let _ = + let xml_file = Sys.argv.(1) in + try + let xml = Xml.parse_file xml_file in + fprintf out "/* Generated from %s */\n" xml_file; + fprintf out "/* Please DO NOT EDIT */\n\n"; + + define "UBX_SYNC1" "0xB5"; + define "UBX_SYNC2" "0x62"; + + List.iter parse_class (Xml.children xml) + with + Xml.Error (em, ep) -> + let l = Xml.line ep + and c1, c2 = Xml.range ep in + fprintf stderr "File \"%s\", line %d, characters %d-%d:\n" xml_file l c1 c2; + fprintf stderr "%s\n" (Xml.error_msg em); + exit 1 + | Length_error (m, l1, l2) -> + fprintf stderr "File \"%s\", inconsistent length: %d expected, %d found from fields in message:\n %s\n" xml_file l1 l2 (Xml.to_string_fmt m); + exit 1 + | Dtd.Check_error e -> + fprintf stderr "File \"%s\", DTD check error: %s\n" xml_file (Dtd.check_error e) + | Dtd.Prove_error e -> + fprintf stderr "\nFile \"%s\", DTD check error: %s\n\n" xml_file (Dtd.prove_error e)