Add wasm tacle-bench targets
This commit is contained in:
8
targets/wasm-tacle/parallel/PapaBench/AUTHORS
Normal file
8
targets/wasm-tacle/parallel/PapaBench/AUTHORS
Normal file
@ -0,0 +1,8 @@
|
||||
Authors of PapaBench
|
||||
Fadia Nemer <nemer_at_irit_dot_fr>
|
||||
Hugues Cassé <casse_at_irit_dot_fr>
|
||||
|
||||
Authors of the initial project "Paparazzi"
|
||||
Project website <www.recherche.enac.fr/paparazzi>
|
||||
Pascal Brisset <pascal_dot_brisset_at_free_dot_fr>
|
||||
Antoine Drouin <antoine_dot_drouin_at_free_dot_fr>
|
||||
339
targets/wasm-tacle/parallel/PapaBench/COPYING
Normal file
339
targets/wasm-tacle/parallel/PapaBench/COPYING
Normal file
@ -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.
|
||||
|
||||
<one line to give the program's name and a brief idea of what it does.>
|
||||
Copyright (C) 19yy <name of author>
|
||||
|
||||
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.
|
||||
|
||||
<signature of Ty Coon>, 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.
|
||||
78
targets/wasm-tacle/parallel/PapaBench/Loops_Bounds.txt
Normal file
78
targets/wasm-tacle/parallel/PapaBench/Loops_Bounds.txt
Normal file
@ -0,0 +1,78 @@
|
||||
Fly_By_wire
|
||||
----------------------------------------------------
|
||||
adc_fbw.c
|
||||
---------
|
||||
function adc_init
|
||||
Loop for : 8 iterations
|
||||
|
||||
|
||||
servo.c
|
||||
-------
|
||||
function servo_init
|
||||
Loop for : 10 iterations
|
||||
function servo_transmit
|
||||
Loop for : 10 iterations
|
||||
|
||||
|
||||
uart.c
|
||||
------
|
||||
function uart_print_string
|
||||
Loop while used once in the main function, its iterations number is estimated to 100.
|
||||
|
||||
main.c
|
||||
------
|
||||
function to_autopilot_from_last_radio
|
||||
Loop for : 9 iterations
|
||||
|
||||
-----------------------------------------------------------------
|
||||
Autopilot
|
||||
-----------------------------------------------------------------
|
||||
adc.c
|
||||
-----
|
||||
function adc_init
|
||||
Loop for : 8 iterations
|
||||
|
||||
estimator.c
|
||||
-----------
|
||||
function estimator_update_ir_estim
|
||||
(NORM_RAD_ANGLE)
|
||||
while 1 while 2
|
||||
case 1 0 0
|
||||
case 2 0 1
|
||||
case 3 1 0
|
||||
(NORM_RAD_ANGLE)
|
||||
while 1 while 2
|
||||
case 1 0 0
|
||||
case 2 0 1
|
||||
case 3 1 0
|
||||
|
||||
mainloop.c
|
||||
----------
|
||||
function main
|
||||
the first while Loop has at most 31 iterations
|
||||
|
||||
nav.c
|
||||
-----
|
||||
|
||||
function nav_home
|
||||
(Macro NormCourse)
|
||||
while 1 while 2
|
||||
case 1 0 0
|
||||
case 2 0 1-2
|
||||
case 3 1-2 0
|
||||
|
||||
function auto_nav
|
||||
(Macro Goto3D / CircleXY / NormCourse)
|
||||
while 1 while 2
|
||||
case 1 0 0
|
||||
case 2 0 1
|
||||
case 3 1 0
|
||||
|
||||
pid.c
|
||||
-----
|
||||
function course_pid_run
|
||||
(macro NORM_RAD_ANGLE)
|
||||
while 1 while 2
|
||||
case 1 0 0
|
||||
case 2 0 1
|
||||
case 3 1 0
|
||||
68
targets/wasm-tacle/parallel/PapaBench/PapaBench_for_wcet.txt
Normal file
68
targets/wasm-tacle/parallel/PapaBench/PapaBench_for_wcet.txt
Normal file
@ -0,0 +1,68 @@
|
||||
PapaBench for the WCET computation
|
||||
==================================
|
||||
|
||||
The Paparazzi Real-Time Application
|
||||
-----------------------------------
|
||||
|
||||
PapaBench was extracted from an actual embedded real-time system, called Paparazzi, which drives an UAV (Unmanned Aerial Vehicle). To stay closer to the embedded running environment (for example, to preserve the memory mapping), we have kept the code organization almost unchanged compared to the original project.
|
||||
|
||||
There are two programs, called fbw (for Fly By Wire) and autopilot, each running on its own processor with its own local separated memory space and its own input / output peripherals. The communication between both processors is performed thanks to a Serial Peripheral Interconnect link with no straight effect on the memory space of each other.
|
||||
|
||||
Both programs have the same overall code structure. First, the initialization of the program is performed using classical non-real-time code. Then, the main control loop starts and runs uninterrupted until the shutdown of the system. This loop is in charge of calling tasks according to their time constraints (mainly their execution frequency) and according to some externals events.
|
||||
|
||||
To prove the real-time behaviour of the system, we have to measure the Worst-Case Execution Time of each task and to check that the sum of the different tasks WCETs fits in the control loop period. In the Paparazzi implementation, both applications have a main loop period of about 30 ms.
|
||||
|
||||
|
||||
|
||||
Papabench: 19 tasks = 19 functions = 19 WCETs
|
||||
---------------------------------------------
|
||||
|
||||
As presented in the previous section, PapaBench is built around two programs representing an actual real-time system running on two different processors. Each program implements an infinite control loop that calls the tasks performing the UAV drive.
|
||||
|
||||
Consequently, the running time of the "main" functions of each Papabench program cannot be computed. However, Papabench is still useful for WCET experimentation considering the execution time of the tasks called from the control loop.
|
||||
|
||||
In Papabench, the entry point of a task is a C function which is called from the control loop. In turn, each task function may develop its own program call tree to achieve its work. Usually, WCET analysis is applied on a whole program. With Papabench, WCET analysis must be made on the functions which are the 13 tasks of the application.
|
||||
|
||||
The tables below shows the list of tasks and their matching C function entry point.
|
||||
|
||||
* FBW
|
||||
|
||||
Identifier C Entry Point Function Description
|
||||
T1 check_failsafe_task Checks if the system must switch to Failsafe mode
|
||||
T2 check_mega128_values_task Receives MCU0 values
|
||||
T3 send_data_to_autopilot_task Send information to MCU0
|
||||
T4 servo_transmit Transmits the Servos\u2019 commands
|
||||
T5 test_ppm_task Decodes the Radio-Command orders
|
||||
|
||||
* Autopilot
|
||||
|
||||
Identifier C Entry Point Function Description
|
||||
T5 altitude_control_task Controls the airframe altitude Control
|
||||
T6 climb_control_task Controls the airframe Climb
|
||||
T7 link_fbw_send Sends the flight commands to MCU1
|
||||
T8 navigation_task Computes the next position of the airframe
|
||||
T9 radio_control_task Manages the Radio orders
|
||||
T10 receive_gps_data_task Decodes the GPS Signals
|
||||
T11 reporting_task Reports the flight information to the ground station
|
||||
T12 stabilisation_task Ensure the airframe Stabilization
|
||||
|
||||
In addition, both applications uses several interruption routines listed in the following tables.
|
||||
|
||||
* FBW
|
||||
|
||||
Identifier C Entry Point Function Description
|
||||
I1 __vector_5 Radio interrupt
|
||||
I2 __vector_6 Servo interrupt
|
||||
I3 __vector_10 SPI interrupt
|
||||
|
||||
* Autopilot
|
||||
|
||||
Identifier C Entry Point Function Description
|
||||
I4 __vector_5 Modem interrupt
|
||||
I5 __vector_12 SPI interrupt
|
||||
I6 __vector_17 SPI interrupt
|
||||
I7 __vector_30 GPS interrupt
|
||||
|
||||
One may also consider calculating the WCET of these routines.
|
||||
|
||||
For more details on the Papabench benchmark, please refer to PapaBench : A Free Real-Time Benchmark. In: 6th Workshop on Worst-Case Execution Time Analysis, Dresden, 04/07/2006.
|
||||
134
targets/wasm-tacle/parallel/PapaBench/README
Normal file
134
targets/wasm-tacle/parallel/PapaBench/README
Normal file
@ -0,0 +1,134 @@
|
||||
# Paparazzi $Id: README,v 1.1 2011-01-18 12:48:16 moellmer Exp $
|
||||
# Copyright (C) 2003 Pascal Brisset Antoine Drouin
|
||||
#
|
||||
# This file is part of PapaBench.
|
||||
#
|
||||
# PapaBench 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.
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
|
||||
Introduction
|
||||
------------
|
||||
|
||||
Paparazzi is an attempt to develop a cheap fixed wing UAV (Unmanned Air Vehicle). It develops a complete system hardware and software that may be installed on a variety of aircrafts. It comprises an embedded system and a ground station.
|
||||
The Paparazzi distribution is only available for a Linux environment but may be configured for several aircraft configurations.
|
||||
PapaBench is bound only to the embedded part of Paparazzi and to the default aircraft configuration. This configuration includes a Twinstar3 model-making aircraft, the flight plan used during the first European MAV Flight Competition held in Braunschweig Germany and a classic ground station consisting of the MC3030 radio-command, a radio receiver and a laptop.
|
||||
The first generation of the embedded system enables us to save the C header files found in sw/var/include directory. These headers are generated from XML files containing the configuration of the airframes, the radio commands and the flight plan.
|
||||
Then we analyzed the static sources and the generated headers in order to create PapaBench. We have excluded the sources of the ground station in charge of monitoring the flight, display statistics, programming the mission and generating the embedded system sources.
|
||||
|
||||
Top Directoriy description
|
||||
--------------------------
|
||||
aadl_sources :contains the aadl description of PapaBench. Their is four descriptions of the benchmark
|
||||
since we analyzed four cases for the periodicity of tasks and interruptions.
|
||||
- Version 1: Tasks and Interruptions are periodic non preemptive (PnP)
|
||||
- Version 2: Tasks are periodic, Interruptions are aperiodic but both non preemptive (PnP, AnP)
|
||||
- Version 3: Tasks and interruptions are periodic preemptive (PP)
|
||||
- Version 4: Tasks are periodic , interruptions are aperiodic but both preemptive (PP, AP)
|
||||
|
||||
avr : holds the header files of the AVR C libc project
|
||||
|
||||
conf : is the configuration directory
|
||||
|
||||
sw : has two sub-directories one for Fly_by_wire source code and the other for autopilot source code
|
||||
|
||||
Loops_Bounds.txt : gives for each loop encountered in the sources its maximum iteration number.
|
||||
|
||||
pcg_autopilot.txt and pcg_fbw.txt :provide the Program Call Graph for the autopilot and the fly_by_wire code respectively
|
||||
|
||||
|
||||
Compilation details
|
||||
-------------------
|
||||
Our objective was to enable a user to compile the benchmark without the requirement of the whole Paparazzi building environment and for different architectures. Until now, we have experimented the compilation for PowerPC, x86 and ARM architectures using the gcc compiler suite but it should be easy to adapt PapaBench to other configurations.
|
||||
|
||||
***********************************************
|
||||
* IMPORTANT *
|
||||
***********************************************
|
||||
It is important to mention that PapaBench includes header files from the AVR C libc library project containing macros providing access to the AVR hardware registers like IO ports, timers, and so on. As the benchmark does not target hardware simulation, either the hardware registers only matter by their temporal properties, or they may be simply considered as simple memory accesses.
|
||||
As they are mapped to low addresses (between 0x20 and 0x100), they might be impeding for some platforms where the addresses of interruptions vectors appear at this location. Fortunately, we can get rid of this problem by assigning in the compilation flags a compatible value to the SFR_OFFSET definition, which is the base of the hardware register addresses.
|
||||
|
||||
|
||||
To Compile PapaBench
|
||||
--------------------
|
||||
|
||||
1) Extract the archive
|
||||
|
||||
2) HAVE A LOOK at conf/Makefile.local
|
||||
- select or configure the ARCH (architecture) and MACH (machine) variables in "conf/Makefile.local",
|
||||
- configure your compiler tools,
|
||||
- configure your CFLAGS and LDFLAGS variable for compilation and linkage options.
|
||||
|
||||
3) "make" in top directory should compile everything
|
||||
|
||||
Multi-processor application
|
||||
---------------------------
|
||||
|
||||
In the default configuration, Papabench produces executable files representing two programs running on two processors. They may be found here:
|
||||
- Fly-By-Wire: PAPABENCH/sw/airborne/fly_by_wire/fbw.elf
|
||||
- Autpilot: PAPABENCH/sw/airborne/autopilot/autopilot.elf
|
||||
|
||||
Each application application starts by calling the main function that contains a static scheduling invoking the different tasks with the different frequencies. Each task has an entry point represented as a C function. The table below show the different tasks and their frequencies.
|
||||
|
||||
FLY-BY-WIRE
|
||||
- check_failsafe_task (20Hz)
|
||||
- check_mega128_values_task (20Hz)
|
||||
- send_data_to_autopilot_task (40Hz)
|
||||
- servo_transmit (20Hz)
|
||||
- test_ppm_task (40Hz)
|
||||
- __vector_5 (40Hz) -- interruption
|
||||
- __vector_6 (20Hz) -- interruption (5 subsequent invocation each 2ms)
|
||||
- __vector_10 (40Hz) -- interruption
|
||||
|
||||
AUTOPILOT
|
||||
- altitude_control_task (4Hz)
|
||||
- climb_control_task (4Hz)
|
||||
- link_fbw_send (20Hz)
|
||||
- navigation_task (4Hz)
|
||||
- radio_control_task (40Hz)
|
||||
- receive_gps_data_task (4Hz)
|
||||
- reporting_task (10Hz)
|
||||
- stabilisation_task (20Hz)
|
||||
- __vector_5 (10Hz) -- interruption
|
||||
- __vector_12 (20Hz) -- interruption
|
||||
- __vector_17 (20Hz) -- interruption
|
||||
- __vector_30 (4Hz) -- interruption
|
||||
|
||||
Mono-processor application
|
||||
--------------------------
|
||||
|
||||
To compile papabench to only one executable file for only one processor, just
|
||||
add the option PAPABENCH_SINGLE to the "make" command.
|
||||
"make PAPABENCH_SINGLE=yes"
|
||||
|
||||
We obtain now a lonely executable:
|
||||
PAPABENCH/sw/airborne/autopilot/autopilot.elf
|
||||
|
||||
The tasks becomes:
|
||||
- altitude_control_task (4Hz)
|
||||
- check_failsafe_task (20Hz)
|
||||
- check_mega128_values_task (20Hz)
|
||||
- climb_control_task (4Hz)
|
||||
- link_fbw_send (20Hz)
|
||||
- navigation_task (4Hz)
|
||||
- radio_control_task (40Hz)
|
||||
- receive_gps_data_task (4Hz)
|
||||
- reporting_task (10Hz)
|
||||
- send_data_to_autopilot_task (40Hz)
|
||||
- servo_transmit (20Hz)
|
||||
- stabilisation_task (20Hz)
|
||||
- test_ppm_task (40Hz)
|
||||
- __vector_5 (10Hz) -- interruption
|
||||
- __vector_6 (20Hz) -- interruption (5 subsequent invocation each 2ms)
|
||||
- __vector_10 (40Hz) -- interruption
|
||||
- __vector_12 (20Hz) -- interruption
|
||||
- __vector_17 (20Hz) -- interruption
|
||||
- __vector_18 (40Hz) -- interruption (previously __vector_5 in fly-by-wire)
|
||||
- __vector_30 (4Hz) -- interruption
|
||||
|
||||
A more formal description of the tasks and their temporal properties expressed in AADL is given in directory "aadl_sources".
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,677 @@
|
||||
-- le projet paprazzi s'interesse a la realisation
|
||||
-- d'un micro-drone autonome a bas cout.
|
||||
-- le systeme embarque est constitue de
|
||||
-- 1.recepteurs (HF(pour radio commande)
|
||||
-- 2.capteurs infrarouge et GPS
|
||||
-- 3.camera video
|
||||
-- 4.modem
|
||||
-- 5.emetteur
|
||||
-- 6.deux microcontroleurs (MCU0, MCU1)
|
||||
|
||||
--voir le fichier listes des taches pour corriger ce fichier AADL
|
||||
|
||||
---------------------------------------------------------------
|
||||
-- We model a bus that connects our devices to the processor
|
||||
bus Device_Bus
|
||||
end Device_Bus;
|
||||
|
||||
bus implementation Device_Bus.MCU0
|
||||
end Device_Bus.MCU0;
|
||||
|
||||
bus implementation Device_Bus.MCU1
|
||||
end Device_Bus.MCU1;
|
||||
|
||||
device Compteur
|
||||
features
|
||||
Cde_Input : in data port Data_Types::Tableau.Generic;
|
||||
Data_Gaz_Output : out data port Data_Types::Integer;
|
||||
Data_Elev_Output : out data port Data_Types::Integer;
|
||||
Data_AilL_Output : out data port Data_Types::Integer;
|
||||
Data_AilR_Output : out data port Data_Types::Integer;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU1;
|
||||
end Compteur;
|
||||
|
||||
device Servo
|
||||
features
|
||||
Data_Input : in data port Data_Types::Integer;
|
||||
--Cde_Output : out event data port; -- new data type?
|
||||
Dev_bus : requires bus access Device_Bus.MCU1;
|
||||
end Servo;
|
||||
|
||||
device Recepteur
|
||||
features
|
||||
--RC_Input : in event data port;
|
||||
--RC_Output : out event data port;
|
||||
RC_Data_Input : in data port ; -- new data type?
|
||||
RC_Evt_Input : in event port; -- -- -- -- -- -- -- -- -- new
|
||||
Cdes_Output : out data port; -- new data type?
|
||||
Error_Evt_Output : out event port;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU1;
|
||||
end Recepteur;
|
||||
|
||||
|
||||
--this thread gets the data from the Radio Commande (RC)
|
||||
--il definit les interruptions provenant de la RC
|
||||
thread Reception_Decodage
|
||||
features
|
||||
Data_RC_Input : in data port;-- -- -- new data type?
|
||||
Data_Decoded_Output: out data port Data_Types::Tableau.Generic;
|
||||
properties
|
||||
Dispatch_Protocol => periodic;
|
||||
end Reception_Decodage;
|
||||
|
||||
thread implementation Reception_Decodage.Impl
|
||||
--properties
|
||||
--Period =>
|
||||
end Reception_Decodage.Impl;
|
||||
|
||||
--this thread gets the flight parameters and transmit them to the Counter
|
||||
thread Pilotage_Servo_Cdes
|
||||
features
|
||||
Data_Cp_Output : out data port Data_Types::Tableau.Generic;--Cp :Compteur
|
||||
Param_Vol_Input : in data port Data_Types::Tableau.Generic;
|
||||
end Pilotage_Servo_Cdes;
|
||||
|
||||
thread implementation Pilotage_Servo_Cdes.Impl
|
||||
--properties
|
||||
--Period => ??????????????????
|
||||
end Pilotage_Servo_Cdes.Impl;
|
||||
|
||||
--this thread sends data to and receives data from MCU0
|
||||
thread Receive_Send_Data_MCU0
|
||||
features
|
||||
MCU1_Data_Input : in data port Data_Types::Tableau.Generic;
|
||||
MCU1_Send_Output : out event data port Data_Types::Inter_mcu_msg.Generic;--updated
|
||||
AP_Data_Input : in event data port Data_Types::Inter_mcu_msg.Generic;--updated
|
||||
AP_Evt_Output : out event port;--updated
|
||||
AP_Rec_Output : out data port Data_Types::Tableau.Generic;
|
||||
end Receive_Send_Data_MCU0;
|
||||
|
||||
thread implementation Receive_Send_Data_MCU0.Impl
|
||||
--properties
|
||||
--Period =>
|
||||
end Receive_Send_Data_MCU0.Impl;
|
||||
|
||||
process RecepteurCde_PilotageServo
|
||||
features
|
||||
Radio_Cdes_Input : in data port;
|
||||
AP_Cdes_Input : in event data port Data_Types::Inter_mcu_msg.Generic;--Autopilot Input --updated
|
||||
Radio_Evt_Input : in event port;--RC erreur
|
||||
--AP_Evt_Input : in event port;
|
||||
Cdes_Vol_Output : out event data port Data_Types::Inter_mcu_msg.Generic;--updated --data transmission to MCU0
|
||||
Cde_Serv_Output : out data port Data_Types::Tableau.Generic;
|
||||
end RecepteurCde_PilotageServo;
|
||||
|
||||
process implementation RecepteurCde_PilotageServo.Impl
|
||||
subcomponents
|
||||
Rec_Dec_Th : thread Reception_Decodage.Impl;
|
||||
Rec_Send_Th : thread Receive_Send_Data_MCU0.Impl;
|
||||
Pilot_Serv_Th : thread Pilotage_Servo_Cdes.Impl;
|
||||
|
||||
connections
|
||||
data port Radio_Cdes_Input->Rec_Dec_Th.Data_RC_Input in modes(manual);
|
||||
event data port AP_Cdes_Input->Rec_Send_Th.AP_Data_Input in modes (auto,manual);--new --updated
|
||||
data port Rec_Send_Th.AP_Rec_Output->Pilot_Serv_Th.Param_Vol_Input in modes(auto,manual);--new
|
||||
--on annule ce lien car les parametres de la Radio commande
|
||||
--doivent etre transmis a MCU0 pour obtenir en retour les
|
||||
--parametres de vol
|
||||
--data port Rec_Dec_Th.Data_Decoded_Output->Pilot_Serv_Th.Param_Vol_Input in modes(manual);
|
||||
data port Pilot_Serv_Th.Data_Cp_Output->Cde_SerV_Output in modes(manual,auto);
|
||||
data port Rec_Dec_Th.Data_Decoded_Output->Rec_Send_Th.MCU1_Data_Input in modes(manual);
|
||||
event data port Rec_Send_Th.MCU1_Send_Output->Cdes_Vol_Output in modes(manual); --updated
|
||||
modes
|
||||
manual : initial mode;
|
||||
auto : mode;
|
||||
failsafe: mode;
|
||||
-- Transitions
|
||||
-- Specify the transitions between modes, use the following syntax:
|
||||
-- <INITIAL_MODE> -[ <EVENT> ]-> <FINAL_MODE>
|
||||
manual-[Radio_Evt_Input]->auto;
|
||||
auto-[Rec_Send_Th.AP_Evt_Output]->manual;
|
||||
manual-[Radio_Evt_Input,Rec_Send_Th.AP_Evt_Output]->Failsafe;
|
||||
end RecepteurCde_PilotageServo.Impl;
|
||||
|
||||
processor Proc_MCU1
|
||||
features
|
||||
Mem_Bus : requires bus access Mem_Bus_MCU1;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU1;
|
||||
end Proc_MCU1;
|
||||
|
||||
-- AADL processor = microprocessor + scheduler
|
||||
processor implementation Proc_MCU1.Impl
|
||||
properties
|
||||
Scheduling_Protocol => RMS;
|
||||
--RMS (Rate Monotonic Scheduling) est un FPS
|
||||
--(Fixed Priority Scheduling)
|
||||
--chaque tache a une priorite fixe
|
||||
end Proc_MCU1.Impl;
|
||||
|
||||
-- We model the RAM to bind our processes and threads
|
||||
memory Ram_MCU1
|
||||
features
|
||||
Mem_Bus : requires bus access Mem_Bus_MCU1;
|
||||
end Ram_MCU1;
|
||||
|
||||
-- We model a data bus that links processors and memories
|
||||
bus Mem_Bus_MCU1
|
||||
end Mem_Bus_MCU1;
|
||||
|
||||
-- MCU1 est charge de la Reception des ordres de la radio-commande,
|
||||
-- la communication avec MCU0 et le pilotage des servos commandes
|
||||
|
||||
system MCU1
|
||||
features
|
||||
RC_Data_Input : in data port;---------------new
|
||||
RC_Evt_Input : in event port; --------new
|
||||
AP_Data_Input : in event data port Data_Types::Inter_mcu_msg.Generic;--AP: autopilot --updated
|
||||
--AP_Evt_Input : in event port;--Erreur Autopilot --updated
|
||||
--RC_Evt_Output : out event port;--Erreur Radio Cde --updated
|
||||
D_MCU1_Output : out event data port Data_Types::Inter_mcu_msg.Generic;-- updated
|
||||
Bus_Con : requires bus access SPI;
|
||||
end MCU1;
|
||||
|
||||
system implementation MCU1.Impl
|
||||
subcomponents
|
||||
--declare all the needed subcomponents to the system
|
||||
MCU1_P : processor Proc_MCU1.Impl;
|
||||
Ram : memory Ram_MCU1;
|
||||
Mem_Bus : bus Mem_Bus_MCU1;
|
||||
Dev_Bus : bus Device_Bus.MCU1;
|
||||
Recpt_dv : device Recepteur;
|
||||
Cpt_dv : device Compteur;
|
||||
Ser_Gaz_dv : device Servo;
|
||||
Ser_Elevator_dv : device Servo;
|
||||
Ser_AileronL_dv : device Servo;--Aileron Left
|
||||
Ser_AileronR_dv : device Servo;--Aileron Right
|
||||
Proc_RcptCde_PilotServ : process RecepteurCde_PilotageServo.Impl;
|
||||
connections
|
||||
--bus connections
|
||||
bus access Mem_Bus -> MCU1_P.Mem_Bus;
|
||||
bus access Dev_Bus -> MCU1_P.Dev_Bus;
|
||||
bus access Mem_Bus -> Ram.Mem_Bus;
|
||||
bus access Dev_Bus -> Recpt_dv.Dev_Bus;
|
||||
bus access Dev_Bus -> Cpt_dv.Dev_Bus;
|
||||
bus access Dev_Bus -> Ser_Gaz_dv.Dev_Bus;
|
||||
bus access Dev_Bus -> Ser_Elevator_dv.Dev_Bus;
|
||||
bus access Dev_Bus -> Ser_AileronL_dv.Dev_Bus;
|
||||
bus access Dev_Bus -> Ser_AileronR_dv.Dev_Bus;
|
||||
|
||||
--port connections
|
||||
data port RC_Data_Input->Recpt_dv.RC_Data_Input;
|
||||
event port RC_Evt_Input->Recpt_dv.RC_Evt_Input;
|
||||
data port Recpt_dv.Cdes_Output->Proc_RcptCde_PilotServ.Radio_Cdes_Input;
|
||||
event data port AP_Data_Input->Proc_RcptCde_PilotServ.AP_Cdes_Input; --updated
|
||||
event port Recpt_dv.Error_Evt_Output->Proc_RcptCde_PilotServ.Radio_Evt_Input;
|
||||
--event port AP_Evt_Input->Proc_RcptCde_PilotServ.AP_Evt_Input;--updated
|
||||
data port Proc_RcptCde_PilotServ.Cde_Serv_Output->Cpt_dv.Cde_Input;
|
||||
data port Cpt_dv.Data_Gaz_Output->Ser_Gaz_dv.Data_Input;
|
||||
data port Cpt_dv.Data_Elev_Output->Ser_Elevator_dv.Data_Input;
|
||||
data port Cpt_dv.Data_AilL_Output->Ser_AileronL_dv.Data_Input;
|
||||
data port Cpt_dv.Data_AilR_Output->Ser_AileronR_dv.Data_Input;
|
||||
--event port Recpt_dv.Error_Evt_Output->RC_Evt_Output;--updated
|
||||
|
||||
properties
|
||||
--to bind the Proc_RcptCde_PilotServ to the memory Ram and the processor MCU1_P
|
||||
--using predeclared properties
|
||||
Actual_Processor_Binding =>reference MCU1_P applies to Proc_RcptCde_PilotServ;
|
||||
Actual_Memory_Binding =>reference Ram applies to Proc_RcptCde_PilotServ;
|
||||
end MCU1.Impl;
|
||||
-----------------------------------------------------------------
|
||||
--determine la pression
|
||||
device Pression
|
||||
features
|
||||
P_Output : out data port Data_Types::Integer;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end Pression;
|
||||
|
||||
--determine la tension
|
||||
device Tension -- Alimentation
|
||||
features
|
||||
T_Output : out data port Data_Types::Integer;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end Tension;
|
||||
|
||||
device Compas
|
||||
features
|
||||
Compas_Output : out data port Data_Types::Float;-- new data type?
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end compas;
|
||||
|
||||
--determine la vitesse de l'air
|
||||
device Vitesse_Air
|
||||
features
|
||||
V_Air_Output : out data port Data_Types::Integer;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end Vitesse_Air;
|
||||
|
||||
--filme le trajet de la mission
|
||||
device Camera_Video
|
||||
features
|
||||
Video_Output : out data port; -- new data type?
|
||||
Dev_Bus : requires bus access Device_bus.MCU0;
|
||||
end Camera_Video;
|
||||
|
||||
-- transmet les donnees a la station sol
|
||||
device Emetteur
|
||||
features
|
||||
Video_Input : in data port;--data type?
|
||||
Trans_Data_Input : in event data port Data_Types::Inter_mcu_msg.Generic; --new --updated
|
||||
Trans_Data_Output : out data port;
|
||||
Video_Output : out data port;
|
||||
Modem_Bus : requires bus access Modem;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end Emetteur;
|
||||
|
||||
-- determine la position du drone
|
||||
device GPS
|
||||
features
|
||||
Error_Output : out event port;
|
||||
GPS_Output : out data port Data_Types::Stream_Element.Generic;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end GPS;
|
||||
|
||||
device InfraRed
|
||||
features
|
||||
Error_Output : out event port;
|
||||
IR_Output : out data port Data_Types::Stream_Element.Generic;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end InfraRed;
|
||||
|
||||
-- relie le systeme MCU0 a la station sol
|
||||
--ce qui permit la transmission de donnes
|
||||
bus Modem
|
||||
end Modem;
|
||||
|
||||
-- Ce thread recoit les sigaux GPS et les transforme
|
||||
-- en donnees utilisables
|
||||
thread Data_Acquisition_GPS
|
||||
features
|
||||
GPS_Input : in data port Data_Types::Stream_Element.Generic;
|
||||
GPS_Output : out data port Data_Types::Position.GPS;
|
||||
end Data_Acquisition_GPS;
|
||||
|
||||
thread implementation Data_Acquisition_GPS.Impl
|
||||
properties
|
||||
period => 250ms; --4hz
|
||||
end Data_Acquisition_GPS.Impl;
|
||||
|
||||
-- recoit les parametres
|
||||
-- des differents composants et prepare la mission
|
||||
thread Simple_Mission
|
||||
features
|
||||
--new
|
||||
--RC_Data_Input : in data port Data_Types::Tableau.Generic;--RC : Radio Commande
|
||||
GPS_Input : in data port Data_Types::Position.GPS;
|
||||
T_Input : in data port Data_Types::Integer;-- T : Tension
|
||||
P_Input : in data port Data_Types::Integer;-- P: Pression
|
||||
Compas_Input : in data port Data_Types::Float;
|
||||
V_Air_Input : in data port Data_Types::Integer;--V : Vitesse
|
||||
Cap_Output : out data port Data_Types::Integer;
|
||||
Alt_Output : out data port Data_Types::Integer;-- Att : Attitude
|
||||
--RC_Data_Output : out data port Data_Types::Tableau.Generic;--utilisee en mode manual voir p.34 article
|
||||
end Simple_Mission;
|
||||
|
||||
thread implementation Simple_Mission.Impl
|
||||
properties
|
||||
Period => 50ms; --20 hertz;
|
||||
end Simple_Mission.Impl;
|
||||
|
||||
--Controle l'altitude
|
||||
thread Altitude_Control
|
||||
features
|
||||
S_Miss_Alt_Input : in data port Data_Types::Integer;--Attitude transmie par Simple mission pour controle
|
||||
GPS_Input : in data port Data_Types::Position.GPS;
|
||||
S_Miss_Cap_Input : in data port Data_Types::Integer;
|
||||
Cap_Output : out data port Data_Types::Integer;
|
||||
Alt_Output : out data port Data_Types::Integer;
|
||||
end Altitude_Control;
|
||||
|
||||
--controle la montee
|
||||
thread Climb_Control
|
||||
features
|
||||
--new
|
||||
GPS_Input : in data port Data_Types::Position.GPS;
|
||||
Montee_Input : in data port Data_Types::Float;-- data fournie par le thread Navigation en mode auto et par Control by RC en mode manuel
|
||||
Montee_Output : out data port Data_Types::Float;
|
||||
end Climb_Control;
|
||||
|
||||
--controle la navigation de l'avion
|
||||
thread Navigation
|
||||
features
|
||||
AC_Cap_Input: in data port Data_Types::Integer;--AC: Attitude_Control
|
||||
AC_Alt_Input: in data port Data_Types::Integer;--AC: Attitude_Control
|
||||
Angle_Roulis_Output : out data port Data_Types::Float;
|
||||
Angle_Tangage_Output : out data port Data_Types::Float;
|
||||
Taux_Montee_Output: out data port Data_Types::Float; --fourni par le GPS
|
||||
end Navigation;
|
||||
|
||||
--Ce thread est declenche a la frequence des informations delivrees
|
||||
--par le GPS
|
||||
thread implementation Navigation.Impl
|
||||
properties
|
||||
Period => 250 ms;--4 hertz;
|
||||
end Navigation.Impl;
|
||||
|
||||
-- Controle la stabilisation et transmet les donnees a MCU1
|
||||
thread Stabilisation -- + Data transmission to MCU1
|
||||
features
|
||||
--new
|
||||
Roulis_Input : in data port Data_Types::Float;--(fourni par la nav en mode auto et par Ctrl_By_RC en mode manuel)
|
||||
Tangage_Input : in data port Data_Types::Float;--fourni par la nav en mode auto et par Ctrl_By_RC en mode manuel)
|
||||
CC_Montee_Input : in data port Data_Types::Float;--Taux de montee fourni par le Th Climb Control
|
||||
IR_Input : in data port;-- IR: Infra red
|
||||
-- Param_Vol : Gaz_Output ,Prof_Output,AileronL_Output,AileronR_Output
|
||||
Param_Vol_Output : out data port Data_Types::Tableau.Generic;
|
||||
end Stabilisation;
|
||||
|
||||
thread implementation Stabilisation.Impl
|
||||
properties
|
||||
Period => 50 ms;--20 hertz;
|
||||
end Stabilisation.Impl;
|
||||
|
||||
--this thread recoit les donnees de MCU1 en mode manuel
|
||||
thread Receive_Send_Data_MCU1
|
||||
features
|
||||
RC_Data_Input : in event data port Data_Types::Inter_mcu_msg.Generic;-- updated
|
||||
RC_Rec_Output : out data port Data_Types::Tableau.Generic;
|
||||
RC_Evt_Output : out event port;--updated (juste pour le changement de mode)
|
||||
MCU0_Data_Input : in data port Data_Types::Tableau.Generic;
|
||||
MCU0_Send_Output : out event data port Data_Types::Inter_mcu_msg.Generic;-- updated
|
||||
end Receive_Send_Data_MCU1;
|
||||
|
||||
thread implementation Receive_Send_Data_MCU1.Impl
|
||||
--properties
|
||||
--Period => ??????
|
||||
end Receive_Send_Data_MCU1.Impl;
|
||||
|
||||
thread Control_By_Radio_Cde
|
||||
features
|
||||
RC_Data_Input : in data port Data_Types::Tableau.Generic;
|
||||
Angle_Roulis_Output : out data port Data_Types::Float;
|
||||
Angle_Tangage_Output : out data port Data_Types::Float;
|
||||
Gaz_Output : out data port Data_Types::Float;
|
||||
end Control_By_Radio_Cde;
|
||||
|
||||
process Nav_Stab_Control_Proc
|
||||
features
|
||||
--MCU1_Evt_Input : in event port;--Erreur Radio Cde --updated
|
||||
MCU1_Data_Input : in event data port Data_Types::Inter_mcu_msg.Generic;--updated --Param Radio_Cde (mode manuel)
|
||||
GPS_Input : in data port Data_Types::Stream_Element.Generic;
|
||||
GPS_Evt_Input : in event port;
|
||||
IR_Input : in data port; -- new data type
|
||||
P_Input : in data port Data_Types::Integer;
|
||||
T_Input : in data port Data_Types :: Integer;
|
||||
Compas_Input : in data port Data_Types::Float; -- new data types
|
||||
V_Air_Input : in data port Data_Types:: Integer;
|
||||
-- donnees transmies a MCU1
|
||||
Trans_MCU0_Output: out event data port Data_Types::Inter_mcu_msg.Generic;-- updated
|
||||
-- donnees transmies a la station sol
|
||||
Trans_Sol_Output : out event data port Data_Types::Inter_mcu_msg.Generic;--updated
|
||||
end Nav_Stab_Control_Proc;
|
||||
|
||||
process implementation Nav_Stab_Control_Proc.Impl
|
||||
--new
|
||||
subcomponents
|
||||
Data_Acq_Filt_Th : thread Data_Acquisition_GPS;
|
||||
S_Mission_Th : thread Simple_Mission.Impl;
|
||||
Alt_Ctrl_Th : thread Altitude_Control;
|
||||
Nav_Th : thread Navigation.Impl;
|
||||
Climb_Ctrl_Th : thread Climb_Control;
|
||||
Stab_Th : thread Stabilisation.Impl;
|
||||
Rec_Send_Th : thread Receive_Send_Data_MCU1.Impl;
|
||||
Ctrl_By_RC_Th: thread Control_By_Radio_Cde;
|
||||
|
||||
--the internal data that will store the autopilot settings
|
||||
D_AP_Destination : data Data_Types::Position.Simple;
|
||||
D_AP_Airspeed : data Data_Types::Integer;
|
||||
D_AP_Altitude : data Data_Types::Integer;
|
||||
connections
|
||||
data port Rec_Send_Th.RC_Rec_Output->Ctrl_By_RC_Th.RC_Data_Input in modes (manual);
|
||||
data port Ctrl_By_RC_Th.Angle_Roulis_Output->Stab_Th.Roulis_Input in modes (manual);
|
||||
data port Ctrl_By_RC_Th.Angle_Tangage_Output->Stab_Th.Tangage_Input in modes (manual);
|
||||
data port Ctrl_By_RC_Th.Gaz_Output->Climb_Ctrl_Th.Montee_Input in modes (manual);
|
||||
|
||||
data port Data_Acq_Filt_Th.GPS_Output->S_Mission_Th.GPS_Input in modes(auto,home);
|
||||
data port Data_Acq_Filt_Th.GPS_Output->Alt_Ctrl_Th.GPS_Input in modes(auto,home);
|
||||
data port Data_Acq_Filt_Th.GPS_Output->Climb_Ctrl_Th.GPS_Input in modes(auto,home);
|
||||
|
||||
data port S_Mission_Th.Cap_Output->Alt_Ctrl_Th.S_Miss_Cap_Input in modes(auto,home);
|
||||
data port S_Mission_Th.Alt_Output->Alt_Ctrl_Th.S_Miss_Alt_Input in modes(auto,home);
|
||||
--en mode manuel c'est le thread Control by radio commande qui fournit les donnees
|
||||
--au thread stabilisation
|
||||
--data port S_Mission_Th.RC_Data_Output->Stab_Th.RC_Data_Input in modes(manual);
|
||||
--data port Rec_Send_Th.RC_Rec_Output->S_Mission_Th.RC_Data_Input in modes(manual);
|
||||
|
||||
data port Alt_Ctrl_Th.Cap_Output->Nav_Th.AC_Cap_Input in modes(auto,home);
|
||||
data port Alt_Ctrl_Th.Alt_Output->Nav_Th.AC_Alt_Input in modes(auto,home);
|
||||
|
||||
data port Nav_Th.Taux_Montee_Output->Climb_Ctrl_Th.Montee_Input in modes(auto,home);
|
||||
data port Climb_Ctrl_Th.Montee_Output->Stab_Th.CC_Montee_Input in modes (auto, home, manual);
|
||||
data port Nav_Th.Angle_Roulis_Output->Stab_Th.Roulis_Input in modes(auto, home);
|
||||
data port Nav_Th.Angle_Tangage_Output->Stab_Th.Tangage_Input in modes(auto, home);
|
||||
data port IR_Input->Stab_Th.IR_Input in modes(manual, auto, home);
|
||||
|
||||
data port Stab_Th.Param_Vol_Output->Rec_Send_Th.MCU0_Data_Input in modes(auto,home,manual);
|
||||
event data port Rec_Send_Th.MCU0_Send_Output->Trans_MCU0_Output in modes(auto,home,manual);--updated
|
||||
event data port Rec_Send_Th.MCU0_Send_Output->Trans_Sol_Output in modes(auto,home,manual);--new--updated
|
||||
event data port MCU1_Data_Input->Rec_Send_Th.RC_Data_Input in modes(manual);--updated
|
||||
|
||||
data port P_Input->S_Mission_Th.P_Input in modes(manual, auto, home);
|
||||
data port T_Input->S_Mission_Th.T_input in modes(manual, auto, home);
|
||||
data port Compas_Input->S_Mission_Th.Compas_Input in modes(manual, auto, home);
|
||||
data port V_Air_Input->S_Mission_Th.V_Air_Input in modes(manual, auto, home);
|
||||
data port GPS_Input->Data_Acq_Filt_Th.GPS_Input in modes(manual, auto, home);
|
||||
modes
|
||||
manual : initial mode;
|
||||
auto : mode;
|
||||
home : mode;
|
||||
--Transition entre les modes
|
||||
manual-[Rec_Send_Th.RC_Evt_Output]->auto;--updated
|
||||
auto-[GPS_Evt_Input]->manual;
|
||||
manual-[Rec_Send_Th.RC_Evt_Output,GPS_Evt_Input]->home;--updated
|
||||
end Nav_Stab_Control_Proc.Impl;
|
||||
|
||||
processor Proc_MCU0
|
||||
features
|
||||
Mem_Bus : requires bus access Mem_Bus_MCU0;
|
||||
Dev_Bus : requires bus access Device_Bus.MCU0;
|
||||
end Proc_MCU0;
|
||||
|
||||
processor implementation Proc_MCU0.Impl
|
||||
properties
|
||||
Scheduling_Protocol => RMS;
|
||||
--RMS (Rate Monotonic Scheduling) est un FPS (Fixed Priority Scheduling)
|
||||
--chaque tache a une priorite fixe
|
||||
end Proc_MCU0.Impl;
|
||||
|
||||
memory Ram_MCU0
|
||||
features
|
||||
Mem_Bus : requires bus access Mem_Bus_MCU0;
|
||||
end Ram_MCU0;
|
||||
|
||||
bus Mem_Bus_MCU0
|
||||
end Mem_Bus_MCU0;
|
||||
|
||||
-- MCU0 est charge de la Stabilisation,
|
||||
-- la navigation, la communication avec MCU1
|
||||
-- et la transmission des parametres de vol
|
||||
|
||||
system MCU0
|
||||
features
|
||||
D_MCU1_Input : in event data port Data_Types::Inter_mcu_msg.Generic;--updated
|
||||
--E_MCU1_Input : in event port;--updated
|
||||
D_MCU0_Output : out event data port Data_Types::Inter_mcu_msg.Generic;--updated
|
||||
--E_MCU0_Output : out event port;--updated
|
||||
Bus_Con : requires bus access SPI;
|
||||
end MCU0;
|
||||
|
||||
system implementation MCU0.Impl
|
||||
subcomponents
|
||||
Proc_0 : processor Proc_MCU0.Impl;
|
||||
Ram : memory Ram_MCU0;
|
||||
Mem_Bus: bus Mem_Bus_MCU0;
|
||||
P_dv : device Pression;
|
||||
T_dv : device Tension;
|
||||
Compas_dv : device Compas;
|
||||
V_Air_dv : device Vitesse_Air;
|
||||
Cam_dv : device Camera_Video;
|
||||
Emet_dv : device Emetteur;
|
||||
GPS_dv : device GPS;
|
||||
IR_dv : device InfraRed;
|
||||
Dev_Bus : bus Device_Bus.MCU0;
|
||||
Modem_Bus : bus Modem;
|
||||
N_S_C_proc : process Nav_Stab_Control_Proc.Impl;
|
||||
connections
|
||||
-- bus connections
|
||||
bus access Mem_Bus->Proc_0.Mem_Bus;
|
||||
bus access Mem_Bus->Ram.Mem_Bus;
|
||||
|
||||
bus access Dev_Bus->Proc_0.Dev_Bus;
|
||||
bus access Dev_Bus->P_dv.Dev_Bus;
|
||||
bus access Dev_Bus->T_dv.Dev_Bus;
|
||||
bus access Dev_Bus->Compas_dv.Dev_Bus;
|
||||
bus access Dev_Bus->V_Air_dv.Dev_Bus;
|
||||
bus access Dev_Bus->Cam_dv.Dev_Bus;
|
||||
bus access Dev_Bus->Emet_dv.Dev_Bus;
|
||||
bus access Modem_Bus->Emet_dv.Modem_Bus;
|
||||
bus access Dev_Bus->GPS_dv.Dev_Bus;
|
||||
bus access Dev_Bus->IR_dv.Dev_Bus;
|
||||
|
||||
--ports connections
|
||||
data port P_dv.P_Output->N_S_C_proc.P_Input;
|
||||
data port T_dv.T_Output->N_S_C_proc.T_Input;
|
||||
data port Compas_dv.Compas_Output->N_S_C_proc.Compas_Input;
|
||||
data port V_Air_dv.V_Air_Output->N_S_C_proc.V_Air_Input;
|
||||
data port GPS_dv.GPS_Output->N_S_C_proc.GPS_Input;
|
||||
event port GPS_dv.Error_Output->N_S_C_proc.GPS_Evt_Input;
|
||||
|
||||
event data port N_S_C_proc.Trans_Sol_Output->Emet_dv.Trans_Data_Input;--updated
|
||||
data port Cam_dv.Video_Output->Emet_dv.Video_Input;
|
||||
|
||||
event data port D_MCU1_Input->N_S_C_proc.MCU1_Data_Input;--updated
|
||||
--event port E_MCU1_Input->N_S_C_proc.MCU1_Evt_Input; --updated
|
||||
|
||||
event data port N_S_C_proc.Trans_MCU0_Output->D_MCU0_Output;--updated
|
||||
--event port GPS_dv.Error_Output->E_MCU0_Output; --updated
|
||||
|
||||
properties
|
||||
-- pour relier le processus a la memeoire et au processeur
|
||||
Actual_Processor_Binding => reference Proc_0 applies to N_S_C_Proc;
|
||||
Actual_Memory_Binding => reference Ram applies to N_S_C_Proc;
|
||||
end MCU0.Impl;
|
||||
----------------------------------------------------------------
|
||||
-- we model a bus to link the systems
|
||||
bus SPI
|
||||
end SPI;
|
||||
|
||||
--the embedded system
|
||||
system Systeme_Paparazzi
|
||||
features
|
||||
RC_Data_Input : in data port;
|
||||
RC_Evt_Input : in event port;
|
||||
end Systeme_Paparazzi;
|
||||
|
||||
system implementation Systeme_Paparazzi.Impl
|
||||
subcomponents
|
||||
MCU0_sys : system MCU0.Impl;
|
||||
MCU1_sys : system MCU1.Impl;
|
||||
Dev_Bus : bus SPI;
|
||||
connections
|
||||
bus access Dev_Bus->MCU0_Sys.Bus_Con;
|
||||
bus access Dev_Bus->MCU1_Sys.Bus_Con;
|
||||
|
||||
data port RC_Data_Input->MCU1_sys.RC_Data_Input;
|
||||
event port RC_Evt_Input->MCU1_sys.RC_Evt_Input;
|
||||
|
||||
event data port MCU0_sys.D_MCU0_Output->MCU1_sys.AP_Data_Input;--updated
|
||||
--event port MCU0_sys.E_MCU0_Output->MCU1_sys.AP_Evt_Input;--updated
|
||||
|
||||
event data port MCU1_sys.D_MCU1_Output->MCU0_sys.D_MCU1_Input;--updated
|
||||
--event port MCU1_sys.RC_Evt_Output->MCU0_sys.E_MCU1_Input;--updated
|
||||
end Systeme_Paparazzi.Impl;
|
||||
|
||||
------------------------------------------------------------
|
||||
------------------------------------------------------------
|
||||
------------------------------------------------------------
|
||||
------------------------------------------------------------
|
||||
-- Satation Sol
|
||||
--bus Device_Bus_Sol
|
||||
--end Device_Bus_Sol;
|
||||
|
||||
--device Radio_Cde
|
||||
--features
|
||||
-- PA_Evt_Output : out event port;
|
||||
-- Cde_Output : out data port; -- le type de donnees??
|
||||
-- Connecteur_Bus : requires bus access Device_Bus_Sol; -- ? juste ? faux?
|
||||
--end Radio_Cde;
|
||||
|
||||
-- Magnetoscope
|
||||
--device Magnetoscope
|
||||
--features
|
||||
-- Video_Input : in data port;
|
||||
-- Connection_Bus : requires bus access Device_Bus_Sol;
|
||||
--end Magnetoscope;
|
||||
|
||||
-- Laptop
|
||||
--processor Laptop_Proc
|
||||
--end Laptop_Proc;
|
||||
|
||||
--thread Affichage_Param_Vol_Th
|
||||
--end Affichage_Param_Vol_Th;
|
||||
|
||||
--thread Affichage_Mission_Trajet_Th
|
||||
--end Affichage_Mission_Trajet_Th;
|
||||
|
||||
--thread Affichage_Mes_Aeronef_Th
|
||||
--end Affichage_Mes_Aeronef_Th;
|
||||
|
||||
--process Affichage_Resultats
|
||||
--end Affichage_Resultats;
|
||||
|
||||
--processor Proc_Lap
|
||||
--end Proc_Lap;
|
||||
|
||||
--memory Ram_Lap
|
||||
--end Ram_Lap;
|
||||
|
||||
--bus Mem_Bus_Lap
|
||||
--end Mem_Bus_Lap;
|
||||
|
||||
---process implementation Affichage_Resultats.Impl
|
||||
--end Affichage_Resultats.Impl;
|
||||
|
||||
--system Laptop_Sys
|
||||
--end Laptop_Sys;
|
||||
|
||||
--system implementation Laptop_Sys.Impl
|
||||
--end Laptop_Sys.Impl;
|
||||
|
||||
-- Satation Sol
|
||||
--system Station_Sol_Sys
|
||||
--features
|
||||
-- PA_Evt_Output : out event port;
|
||||
-- Radio_Cde_Output : out data port;
|
||||
-- Mission_Trajet_Input : in data port;
|
||||
-- Mes_Aeronef_Input : in data port;
|
||||
-- Param_Vol_Input : in data port;
|
||||
-- Video_Input : in data port;
|
||||
-- Connection_Network : requires bus access Network;
|
||||
--end Station_Sol_Sys;
|
||||
|
||||
--system implementation Station_Sol_Sys.Impl
|
||||
--subcomponents
|
||||
-- Lap_Sys : system Laptop_Sys;
|
||||
-- Mag_dv : device Magnetoscope;
|
||||
-- Radio_dv : device Radio_Cde;
|
||||
-- Dev_Bus : bus Device_Bus_Sol;
|
||||
|
||||
--end Station_Sol_Sys.Impl;
|
||||
|
||||
-- Systeme complet
|
||||
--bus Network
|
||||
--end Network;
|
||||
|
||||
--system Paparazzi_System
|
||||
|
||||
--end Paparazzi_System;
|
||||
|
||||
@ -0,0 +1,10 @@
|
||||
property set Thread_Properties is
|
||||
|
||||
Non_Preemptive : constant aadlinteger =>0;
|
||||
System_Preemption : constant aadlinteger =>1;
|
||||
Time_Sharing_Preemption : constant aadlinteger => 2;
|
||||
|
||||
Preemption : enumeration (Non_Preemptive,System_Preemption,Time_Sharing_Preemption)
|
||||
applies to (thread);
|
||||
Entrypoints: list of Compute_Entrypoint applies to (thread);
|
||||
end Thread_Properties;
|
||||
@ -0,0 +1,185 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<core:AadlSpec xmi:version="2.0" xmlns:xmi="http://www.omg.org/XMI" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:core="http://core" xmlns:property="http://property">
|
||||
<packageSpec name="Data_Types">
|
||||
<aadlPublic>
|
||||
<dataType name="Integer">
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Source_Data_Size]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Size_Units]/unitLiteral[@name=Bits]" valueString="8" value="8" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</dataType>
|
||||
<dataType name="Boolean">
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Source_Data_Size]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Size_Units]/unitLiteral[@name=Bits]" valueString="8" value="8" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</dataType>
|
||||
<dataType name="Integer_16">
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Source_Data_Size]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Size_Units]/unitLiteral[@name=Bits]" valueString="16" value="16" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</dataType>
|
||||
<dataType name="Float">
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Source_Data_Size]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Size_Units]/unitLiteral[@name=Bits]" valueString="32" value="32" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</dataType>
|
||||
<subprogramType name="Get_Integer">
|
||||
<features>
|
||||
<parameter name="Result" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</features>
|
||||
</subprogramType>
|
||||
<subprogramType name="Set_Integer">
|
||||
<features>
|
||||
<parameter name="Int" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</features>
|
||||
</subprogramType>
|
||||
<subprogramType name="Get_Integer16">
|
||||
<features>
|
||||
<parameter name="Result" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
</features>
|
||||
</subprogramType>
|
||||
<subprogramType name="Set_Integer16">
|
||||
<features>
|
||||
<parameter name="Int" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
</features>
|
||||
</subprogramType>
|
||||
<dataType name="Lat_Long">
|
||||
<comment>-- le type Lat_long est une structure de donnees pour stocker les </comment>
|
||||
<comment>-- longitudes et latitudes</comment>
|
||||
<features>
|
||||
<subprogram name="Get_Degs" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer]"/>
|
||||
<subprogram name="Set_Degs" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer]"/>
|
||||
<subprogram name="Get_Mins" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer]"/>
|
||||
<subprogram name="Set_Mins" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer]"/>
|
||||
<subprogram name="Get_Secs" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer]"/>
|
||||
<subprogram name="Set_Secs" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer]"/>
|
||||
</features>
|
||||
</dataType>
|
||||
<dataImpl name="Lat_Long.Generic" compType="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Lat_Long]">
|
||||
<subcomponents>
|
||||
<dataSubcomponent name="Degs" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataSubcomponent name="Mins" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataSubcomponent name="Secs" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</subcomponents>
|
||||
</dataImpl>
|
||||
<dataType name="Position">
|
||||
<comment>-- The Position type specifies a position</comment>
|
||||
</dataType>
|
||||
<dataImpl name="Position.simple" compType="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Position]">
|
||||
<comment>-- This data type represents the position</comment>
|
||||
<subcomponents>
|
||||
<dataSubcomponent name="Latitude" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Lat_Long.Generic]"/>
|
||||
<dataSubcomponent name="Longitude" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Lat_Long.Generic]"/>
|
||||
</subcomponents>
|
||||
</dataImpl>
|
||||
<dataImpl name="Position.GPS" compType="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Position]" extend="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Position.simple]">
|
||||
<comment>-- The data type that represents GPS sensor data</comment>
|
||||
<subcomponents>
|
||||
<dataSubcomponent name="Altitude" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataSubcomponent name="Cap" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</subcomponents>
|
||||
</dataImpl>
|
||||
<dataType name="Stream_Element"/>
|
||||
<dataImpl name="Stream_Element.Generic" compType="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Stream_Element]">
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Source_Data_Size]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Size_Units]/unitLiteral[@name=Bits]" valueString="8" value="8" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</dataImpl>
|
||||
<dataType name="Tab"/>
|
||||
<dataType name="Tableau" extend="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Tab]">
|
||||
<features>
|
||||
<subprogram name="Get_Data0" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data0" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data1" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data1" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data2" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data2" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data3" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data3" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data4" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data4" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data5" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data5" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data6" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data6" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data7" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data7" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Data8" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Data8" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
</features>
|
||||
</dataType>
|
||||
<dataImpl name="Tableau.Generic" compType="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Tableau]">
|
||||
<comment>--etc selon le parametre RADIO_CTL_NB = 9 du fichier link_autopilot.h</comment>
|
||||
<comment>--Valeur de RADIO_CTL_NB se trouve dans radio.h</comment>
|
||||
<comment>--qui est egale a PPM_NB_OF_PULSE de ppm.h</comment>
|
||||
<subcomponents>
|
||||
<dataSubcomponent name="data_0" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_1" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_2" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_3" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_4" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_5" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_6" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_7" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="data_8" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
</subcomponents>
|
||||
</dataImpl>
|
||||
<dataType name="TabServos" extend="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Tab]">
|
||||
<features>
|
||||
<subprogram name="Get_MotorL" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_MotorL" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_MotorR" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_MotorR" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_AileronL" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_AileronL" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_AileronR" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_AileronR" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Rudder" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Rudder" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
<subprogram name="Get_Elevator" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer16]"/>
|
||||
<subprogram name="Set_Elevator" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer16]"/>
|
||||
</features>
|
||||
</dataType>
|
||||
<dataImpl name="TabServos.Impl" compType="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=TabServos]">
|
||||
<subcomponents>
|
||||
<dataSubcomponent name="MotorL" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="MotorR" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="AileronL" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="AileronR" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="Rudder" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
<dataSubcomponent name="Elevator" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer_16]"/>
|
||||
</subcomponents>
|
||||
</dataImpl>
|
||||
<dataType name="inter_mcu_msg">
|
||||
<features>
|
||||
<subprogram name="Set_Ppm" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer]"/>
|
||||
<subprogram name="Get_Ppm" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer]"/>
|
||||
<subprogram name="Set_Status" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer]"/>
|
||||
<subprogram name="Get_Status" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer]"/>
|
||||
<subprogram name="Set_NbErr" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer]"/>
|
||||
<subprogram name="Get_NbErr" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer]"/>
|
||||
<subprogram name="Set_Vsupply" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Set_Integer]"/>
|
||||
<subprogram name="Get_Vsupply" subprogramClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/subprogramType[@name=Get_Integer]"/>
|
||||
</features>
|
||||
</dataType>
|
||||
<dataImpl name="Inter_mcu_msg.Generic" compType="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=inter_mcu_msg]">
|
||||
<subcomponents>
|
||||
<dataSubcomponent name="channels" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]"/>
|
||||
<dataSubcomponent name="ppm_cpt" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataSubcomponent name="satus" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataSubcomponent name="nb_err" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataSubcomponent name="vsupply" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</subcomponents>
|
||||
</dataImpl>
|
||||
</aadlPublic>
|
||||
</packageSpec>
|
||||
</core:AadlSpec>
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,722 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<core:AadlSpec xmi:version="2.0" xmlns:xmi="http://www.omg.org/XMI" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:core="http://core" xmlns:property="http://property" name="paparazzi_Old Version">
|
||||
<busType name="Device_Bus">
|
||||
<comment>-- le projet paprazzi s'interesse a la realisation </comment>
|
||||
<comment>-- d'un micro-drone autonome a bas cout.</comment>
|
||||
<comment>-- le systeme embarque est constitue de</comment>
|
||||
<comment>-- 1.recepteurs (HF(pour radio commande)</comment>
|
||||
<comment>-- 2.capteurs infrarouge et GPS</comment>
|
||||
<comment>-- 3.camera video</comment>
|
||||
<comment>-- 4.modem</comment>
|
||||
<comment>-- 5.emetteur</comment>
|
||||
<comment>-- 6.deux microcontroleurs (MCU0, MCU1)</comment>
|
||||
<comment>--voir le fichier listes des taches pour corriger ce fichier AADL</comment>
|
||||
<comment>---------------------------------------------------------------</comment>
|
||||
<comment>-- We model a bus that connects our devices to the processor</comment>
|
||||
</busType>
|
||||
<busImpl name="Device_Bus.MCU0" compType="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Device_Bus]"/>
|
||||
<busImpl name="Device_Bus.MCU1" compType="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Device_Bus]"/>
|
||||
<deviceType name="Compteur">
|
||||
<features>
|
||||
<dataPort name="Cde_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]"/>
|
||||
<dataPort name="Data_Gaz_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="Data_Elev_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="Data_AilL_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="Data_AilR_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU1]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="Servo">
|
||||
<features>
|
||||
<dataPort name="Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<busAccess name="Dev_bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU1]">
|
||||
<comment>--Cde_Output : out event data port; -- new data type?</comment>
|
||||
</busAccess>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="Recepteur">
|
||||
<features>
|
||||
<dataPort name="RC_Data_Input">
|
||||
<comment>--RC_Input : in event data port;</comment>
|
||||
<comment>--RC_Output : out event data port;</comment>
|
||||
</dataPort>
|
||||
<eventPort name="RC_Evt_Input">
|
||||
<comment>-- new data type?</comment>
|
||||
</eventPort>
|
||||
<dataPort name="Cdes_Output" direction="out">
|
||||
<comment>-- -- -- -- -- -- -- -- -- new</comment>
|
||||
</dataPort>
|
||||
<eventPort name="Error_Evt_Output" direction="out">
|
||||
<comment>-- new data type?</comment>
|
||||
</eventPort>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU1]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<threadType name="Reception_Decodage">
|
||||
<comment>--this thread gets the data from the Radio Commande (RC)</comment>
|
||||
<comment>--il definit les interruptions provenant de la RC</comment>
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Dispatch_Protocol]">
|
||||
<propertyValue xsi:type="property:EnumValue" enumLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Supported_Dispatch_Protocols]/enumLiteral[@name=Periodic]"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
<features>
|
||||
<dataPort name="Data_RC_Input"/>
|
||||
<dataPort name="Data_Decoded_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]">
|
||||
<comment>-- -- -- new data type?</comment>
|
||||
</dataPort>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Reception_Decodage.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Reception_Decodage]">
|
||||
<comment>--properties</comment>
|
||||
<comment>--Period =></comment>
|
||||
</threadImpl>
|
||||
<threadType name="Pilotage_Servo_Cdes">
|
||||
<comment>--this thread gets the flight parameters and transmit them to the Counter </comment>
|
||||
<features>
|
||||
<dataPort name="Data_Cp_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]"/>
|
||||
<dataPort name="Param_Vol_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]">
|
||||
<comment>--Cp :Compteur</comment>
|
||||
</dataPort>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Pilotage_Servo_Cdes.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Pilotage_Servo_Cdes]">
|
||||
<comment>--properties</comment>
|
||||
<comment>--Period => ??????????????????</comment>
|
||||
</threadImpl>
|
||||
<threadType name="Receive_Send_Data_MCU0">
|
||||
<comment>--this thread sends data to and receives data from MCU0</comment>
|
||||
<features>
|
||||
<dataPort name="MCU1_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]"/>
|
||||
<eventDataPort name="MCU1_Send_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]"/>
|
||||
<eventDataPort name="AP_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>--updated</comment>
|
||||
</eventDataPort>
|
||||
<eventPort name="AP_Evt_Output" direction="out">
|
||||
<comment>--updated</comment>
|
||||
</eventPort>
|
||||
<dataPort name="AP_Rec_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]">
|
||||
<comment>--updated</comment>
|
||||
</dataPort>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Receive_Send_Data_MCU0.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU0]">
|
||||
<comment>--properties </comment>
|
||||
<comment>--Period =></comment>
|
||||
</threadImpl>
|
||||
<processType name="RecepteurCde_PilotageServo">
|
||||
<features>
|
||||
<dataPort name="Radio_Cdes_Input"/>
|
||||
<eventDataPort name="AP_Cdes_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]"/>
|
||||
<eventPort name="Radio_Evt_Input">
|
||||
<comment>--Autopilot Input --updated</comment>
|
||||
</eventPort>
|
||||
<eventDataPort name="Cdes_Vol_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>--RC erreur</comment>
|
||||
<comment>--AP_Evt_Input : in event port;</comment>
|
||||
</eventDataPort>
|
||||
<dataPort name="Cde_Serv_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]">
|
||||
<comment>--updated --data transmission to MCU0</comment>
|
||||
</dataPort>
|
||||
</features>
|
||||
</processType>
|
||||
<processImpl name="RecepteurCde_PilotageServo.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]">
|
||||
<connections>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Dec_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/dataPort[@name=Radio_Cdes_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Reception_Decodage]/features/dataPort[@name=Data_RC_Input]"/>
|
||||
<eventDataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU0]/features/eventDataPort[@name=AP_Data_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/eventDataPort[@name=AP_Cdes_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Pilot_Serv_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU0]/features/dataPort[@name=AP_Rec_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Pilotage_Servo_Cdes]/features/dataPort[@name=Param_Vol_Input]">
|
||||
<comment>--new --updated</comment>
|
||||
</dataConnection>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=auto]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Pilot_Serv_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Pilotage_Servo_Cdes]/features/dataPort[@name=Data_Cp_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/dataPort[@name=Cde_Serv_Output]">
|
||||
<comment>--new</comment>
|
||||
<comment>--on annule ce lien car les parametres de la Radio commande</comment>
|
||||
<comment>--doivent etre transmis a MCU0 pour obtenir en retour les </comment>
|
||||
<comment>--parametres de vol</comment>
|
||||
<comment>--data port Rec_Dec_Th.Data_Decoded_Output->Pilot_Serv_Th.Param_Vol_Input in modes(manual);</comment>
|
||||
</dataConnection>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Dec_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Reception_Decodage]/features/dataPort[@name=Data_Decoded_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU0]/features/dataPort[@name=MCU1_Data_Input]"/>
|
||||
<eventDataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/eventDataPort[@name=Cdes_Vol_Output]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU0]/features/eventDataPort[@name=MCU1_Send_Output]"/>
|
||||
</connections>
|
||||
<modes>
|
||||
<comment>--updated</comment>
|
||||
<mode name="manual" state="INITIAL"/>
|
||||
<mode name="auto"/>
|
||||
<mode name="failsafe"/>
|
||||
<modeTransition srcMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" dstMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=auto]" triggerEventPort="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/eventPort[@name=Radio_Evt_Input]" triggerEventPortContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]">
|
||||
<comment>-- Transitions</comment>
|
||||
<comment>-- Specify the transitions between modes, use the following syntax:</comment>
|
||||
<comment>-- <INITIAL_MODE> -[ <EVENT> ]-> <FINAL_MODE></comment>
|
||||
</modeTransition>
|
||||
<modeTransition srcMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=auto]" dstMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" triggerEventPort="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU0]/features/eventPort[@name=AP_Evt_Output]" triggerEventPortContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]"/>
|
||||
<modeTransition srcMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=manual]" dstMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/modes/mode[@name=failsafe]" triggerEventPort="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/eventPort[@name=Radio_Evt_Input] /aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU0]/features/eventPort[@name=AP_Evt_Output]" triggerEventPortContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]"/>
|
||||
</modes>
|
||||
<subcomponents>
|
||||
<threadSubcomponent name="Rec_Dec_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadImpl[@name=Reception_Decodage.Impl]"/>
|
||||
<threadSubcomponent name="Rec_Send_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadImpl[@name=Receive_Send_Data_MCU0.Impl]"/>
|
||||
<threadSubcomponent name="Pilot_Serv_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadImpl[@name=Pilotage_Servo_Cdes.Impl]"/>
|
||||
</subcomponents>
|
||||
</processImpl>
|
||||
<processorType name="Proc_MCU1">
|
||||
<features>
|
||||
<busAccess name="Mem_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Mem_Bus_MCU1]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU1]"/>
|
||||
</features>
|
||||
</processorType>
|
||||
<processorImpl name="Proc_MCU1.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/processorType[@name=Proc_MCU1]">
|
||||
<comment>-- AADL processor = microprocessor + scheduler</comment>
|
||||
<comment>--RMS (Rate Monotonic Scheduling) est un FPS </comment>
|
||||
<comment>--(Fixed Priority Scheduling) </comment>
|
||||
<comment>--chaque tache a une priorite fixe</comment>
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Scheduling_Protocol]">
|
||||
<propertyValue xsi:type="property:EnumValue" enumLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Supported_Scheduling_Protocols]/enumLiteral[@name=RMS]"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</processorImpl>
|
||||
<memoryType name="Ram_MCU1">
|
||||
<comment>-- We model the RAM to bind our processes and threads</comment>
|
||||
<features>
|
||||
<busAccess name="Mem_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Mem_Bus_MCU1]"/>
|
||||
</features>
|
||||
</memoryType>
|
||||
<busType name="Mem_Bus_MCU1">
|
||||
<comment>-- We model a data bus that links processors and memories</comment>
|
||||
</busType>
|
||||
<systemType name="MCU1">
|
||||
<comment>-- MCU1 est charge de la Reception des ordres de la radio-commande,</comment>
|
||||
<comment>-- la communication avec MCU0 et le pilotage des servos commandes</comment>
|
||||
<features>
|
||||
<dataPort name="RC_Data_Input"/>
|
||||
<eventPort name="RC_Evt_Input">
|
||||
<comment>---------------new</comment>
|
||||
</eventPort>
|
||||
<eventDataPort name="AP_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>--------new</comment>
|
||||
</eventDataPort>
|
||||
<eventDataPort name="D_MCU1_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>--AP: autopilot --updated</comment>
|
||||
<comment>--AP_Evt_Input : in event port;--Erreur Autopilot --updated</comment>
|
||||
<comment>--RC_Evt_Output : out event port;--Erreur Radio Cde --updated</comment>
|
||||
</eventDataPort>
|
||||
<busAccess name="Bus_Con" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=SPI]">
|
||||
<comment>-- updated</comment>
|
||||
</busAccess>
|
||||
</features>
|
||||
</systemType>
|
||||
<systemImpl name="MCU1.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]">
|
||||
<properties>
|
||||
<comment>--event port Recpt_dv.Error_Evt_Output->RC_Evt_Output;--updated</comment>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Actual_Processor_Binding]" appliesTo="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processSubcomponent[@name=Proc_RcptCde_PilotServ]">
|
||||
<comment>--to bind the Proc_RcptCde_PilotServ to the memory Ram and the processor MCU1_P</comment>
|
||||
<comment>--using predeclared properties</comment>
|
||||
<propertyValue xsi:type="property:ReferenceValue" referenceElement="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processorSubcomponent[@name=MCU1_P]"/>
|
||||
</propertyAssociation>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Actual_Memory_Binding]" appliesTo="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processSubcomponent[@name=Proc_RcptCde_PilotServ]">
|
||||
<propertyValue xsi:type="property:ReferenceValue" referenceElement="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/memorySubcomponent[@name=Ram]"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
<connections>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processorSubcomponent[@name=MCU1_P]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/processorType[@name=Proc_MCU1]/features/busAccess[@name=Mem_Bus]">
|
||||
<comment>--bus connections </comment>
|
||||
</busAccessConnection>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processorSubcomponent[@name=MCU1_P]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/processorType[@name=Proc_MCU1]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/memorySubcomponent[@name=Ram]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/memoryType[@name=Ram_MCU1]/features/busAccess[@name=Mem_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Recpt_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Recepteur]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Cpt_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compteur]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_Gaz_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/busAccess[@name=Dev_bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_Elevator_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/busAccess[@name=Dev_bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_AileronL_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/busAccess[@name=Dev_bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_AileronR_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/busAccess[@name=Dev_bus]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Recpt_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/dataPort[@name=RC_Data_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Recepteur]/features/dataPort[@name=RC_Data_Input]">
|
||||
<comment>--port connections</comment>
|
||||
</dataConnection>
|
||||
<eventConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Recpt_dv]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Recepteur]/features/eventPort[@name=RC_Evt_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/eventPort[@name=RC_Evt_Input]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Recpt_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processSubcomponent[@name=Proc_RcptCde_PilotServ]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Recepteur]/features/dataPort[@name=Cdes_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/dataPort[@name=Radio_Cdes_Input]"/>
|
||||
<eventDataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processSubcomponent[@name=Proc_RcptCde_PilotServ]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/eventDataPort[@name=AP_Cdes_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/eventDataPort[@name=AP_Data_Input]"/>
|
||||
<eventConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Recpt_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processSubcomponent[@name=Proc_RcptCde_PilotServ]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/eventPort[@name=Radio_Evt_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Recepteur]/features/eventPort[@name=Error_Evt_Output]">
|
||||
<comment>--updated</comment>
|
||||
</eventConnection>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/processSubcomponent[@name=Proc_RcptCde_PilotServ]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Cpt_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=RecepteurCde_PilotageServo]/features/dataPort[@name=Cde_Serv_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compteur]/features/dataPort[@name=Cde_Input]">
|
||||
<comment>--event port AP_Evt_Input->Proc_RcptCde_PilotServ.AP_Evt_Input;--updated</comment>
|
||||
</dataConnection>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Cpt_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_Gaz_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compteur]/features/dataPort[@name=Data_Gaz_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/dataPort[@name=Data_Input]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Cpt_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_Elevator_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compteur]/features/dataPort[@name=Data_Elev_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/dataPort[@name=Data_Input]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Cpt_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_AileronL_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compteur]/features/dataPort[@name=Data_AilL_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/dataPort[@name=Data_Input]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Cpt_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]/subcomponents/deviceSubcomponent[@name=Ser_AileronR_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compteur]/features/dataPort[@name=Data_AilR_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]/features/dataPort[@name=Data_Input]"/>
|
||||
</connections>
|
||||
<subcomponents>
|
||||
<processorSubcomponent name="MCU1_P" classifier="/aadlSpec[@name=paparazzi_Old Version]/processorImpl[@name=Proc_MCU1.Impl]">
|
||||
<comment>--declare all the needed subcomponents to the system</comment>
|
||||
</processorSubcomponent>
|
||||
<memorySubcomponent name="Ram" classifier="/aadlSpec[@name=paparazzi_Old Version]/memoryType[@name=Ram_MCU1]"/>
|
||||
<busSubcomponent name="Mem_Bus" classifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Mem_Bus_MCU1]"/>
|
||||
<busSubcomponent name="Dev_Bus" classifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU1]"/>
|
||||
<deviceSubcomponent name="Recpt_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Recepteur]"/>
|
||||
<deviceSubcomponent name="Cpt_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compteur]"/>
|
||||
<deviceSubcomponent name="Ser_Gaz_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]"/>
|
||||
<deviceSubcomponent name="Ser_Elevator_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]"/>
|
||||
<deviceSubcomponent name="Ser_AileronL_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]"/>
|
||||
<deviceSubcomponent name="Ser_AileronR_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Servo]">
|
||||
<comment>--Aileron Left</comment>
|
||||
</deviceSubcomponent>
|
||||
<processSubcomponent name="Proc_RcptCde_PilotServ" classifier="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=RecepteurCde_PilotageServo.Impl]">
|
||||
<comment>--Aileron Right</comment>
|
||||
</processSubcomponent>
|
||||
</subcomponents>
|
||||
</systemImpl>
|
||||
<deviceType name="Pression">
|
||||
<comment>-----------------------------------------------------------------</comment>
|
||||
<comment>--determine la pression </comment>
|
||||
<features>
|
||||
<dataPort name="P_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="Tension">
|
||||
<comment>--determine la tension</comment>
|
||||
<features>
|
||||
<comment>-- Alimentation</comment>
|
||||
<dataPort name="T_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="Compas">
|
||||
<features>
|
||||
<dataPort name="Compas_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]">
|
||||
<comment>-- new data type?</comment>
|
||||
</busAccess>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="Vitesse_Air">
|
||||
<comment>--determine la vitesse de l'air</comment>
|
||||
<features>
|
||||
<dataPort name="V_Air_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="Camera_Video">
|
||||
<comment>--filme le trajet de la mission</comment>
|
||||
<features>
|
||||
<dataPort name="Video_Output" direction="out"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]">
|
||||
<comment>-- new data type?</comment>
|
||||
</busAccess>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="Emetteur">
|
||||
<comment>-- transmet les donnees a la station sol</comment>
|
||||
<features>
|
||||
<dataPort name="Video_Input"/>
|
||||
<eventDataPort name="Trans_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>--data type?</comment>
|
||||
</eventDataPort>
|
||||
<dataPort name="Trans_Data_Output" direction="out">
|
||||
<comment>--new --updated</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Video_Output" direction="out"/>
|
||||
<busAccess name="Modem_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Modem]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="GPS">
|
||||
<comment>-- determine la position du drone </comment>
|
||||
<features>
|
||||
<eventPort name="Error_Output" direction="out"/>
|
||||
<dataPort name="GPS_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Stream_Element.Generic]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<deviceType name="InfraRed">
|
||||
<features>
|
||||
<eventPort name="Error_Output" direction="out"/>
|
||||
<dataPort name="IR_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Stream_Element.Generic]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
</features>
|
||||
</deviceType>
|
||||
<busType name="Modem">
|
||||
<comment>-- relie le systeme MCU0 a la station sol</comment>
|
||||
<comment>--ce qui permit la transmission de donnes</comment>
|
||||
</busType>
|
||||
<threadType name="Data_Acquisition_GPS">
|
||||
<comment>-- Ce thread recoit les sigaux GPS et les transforme </comment>
|
||||
<comment>-- en donnees utilisables</comment>
|
||||
<features>
|
||||
<dataPort name="GPS_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Stream_Element.Generic]"/>
|
||||
<dataPort name="GPS_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Position.GPS]"/>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Data_Acquisition_GPS.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Data_Acquisition_GPS]">
|
||||
<comment>--4hz </comment>
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Period]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Time_Units]/unitLiteral[@name=Ms]" valueString="250" value="250" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</threadImpl>
|
||||
<threadType name="Simple_Mission">
|
||||
<comment>-- recoit les parametres </comment>
|
||||
<comment>-- des differents composants et prepare la mission</comment>
|
||||
<comment>-- Att : Attitude</comment>
|
||||
<comment>--RC_Data_Output : out data port Data_Types::Tableau.Generic;--utilisee en mode manual voir p.34 article</comment>
|
||||
<features>
|
||||
<dataPort name="GPS_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Position.GPS]">
|
||||
<comment>--new</comment>
|
||||
<comment>--RC_Data_Input : in data port Data_Types::Tableau.Generic;--RC : Radio Commande</comment>
|
||||
</dataPort>
|
||||
<dataPort name="T_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="P_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]">
|
||||
<comment>-- T : Tension</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Compas_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]">
|
||||
<comment>-- P: Pression</comment>
|
||||
</dataPort>
|
||||
<dataPort name="V_Air_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="Cap_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]">
|
||||
<comment>--V : Vitesse</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Alt_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Simple_Mission.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]">
|
||||
<comment>--20 hertz;</comment>
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Period]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Time_Units]/unitLiteral[@name=Ms]" valueString="50" value="50" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</threadImpl>
|
||||
<threadType name="Altitude_Control">
|
||||
<comment>--Controle l'altitude</comment>
|
||||
<features>
|
||||
<dataPort name="S_Miss_Alt_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="GPS_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Position.GPS]">
|
||||
<comment>--Attitude transmie par Simple mission pour controle</comment>
|
||||
</dataPort>
|
||||
<dataPort name="S_Miss_Cap_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="Cap_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="Alt_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadType name="Climb_Control">
|
||||
<comment>--controle la montee</comment>
|
||||
<features>
|
||||
<dataPort name="GPS_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Position.GPS]">
|
||||
<comment>--new</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Montee_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
<dataPort name="Montee_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]">
|
||||
<comment>-- data fournie par le thread Navigation en mode auto et par Control by RC en mode manuel</comment>
|
||||
</dataPort>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadType name="Navigation">
|
||||
<comment>--controle la navigation de l'avion</comment>
|
||||
<comment>--fourni par le GPS</comment>
|
||||
<features>
|
||||
<dataPort name="AC_Cap_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="AC_Alt_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]">
|
||||
<comment>--AC: Attitude_Control</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Angle_Roulis_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]">
|
||||
<comment>--AC: Attitude_Control</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Angle_Tangage_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
<dataPort name="Taux_Montee_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Navigation.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Navigation]">
|
||||
<comment>--Ce thread est declenche a la frequence des informations delivrees </comment>
|
||||
<comment>--par le GPS</comment>
|
||||
<comment>--4 hertz; </comment>
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Period]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Time_Units]/unitLiteral[@name=Ms]" valueString="250" value="250" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</threadImpl>
|
||||
<threadType name="Stabilisation">
|
||||
<comment>-- Controle la stabilisation et transmet les donnees a MCU1</comment>
|
||||
<features>
|
||||
<comment>-- + Data transmission to MCU1</comment>
|
||||
<dataPort name="Roulis_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]">
|
||||
<comment>--new</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Tangage_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]">
|
||||
<comment>--(fourni par la nav en mode auto et par Ctrl_By_RC en mode manuel)</comment>
|
||||
</dataPort>
|
||||
<dataPort name="CC_Montee_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]">
|
||||
<comment>--fourni par la nav en mode auto et par Ctrl_By_RC en mode manuel)</comment>
|
||||
</dataPort>
|
||||
<dataPort name="IR_Input">
|
||||
<comment>--Taux de montee fourni par le Th Climb Control</comment>
|
||||
</dataPort>
|
||||
<dataPort name="Param_Vol_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]">
|
||||
<comment>-- IR: Infra red</comment>
|
||||
<comment>-- Param_Vol : Gaz_Output ,Prof_Output,AileronL_Output,AileronR_Output</comment>
|
||||
</dataPort>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Stabilisation.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]">
|
||||
<comment>--20 hertz; </comment>
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Period]">
|
||||
<propertyValue xsi:type="property:IntegerValue" unitLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Time_Units]/unitLiteral[@name=Ms]" valueString="50" value="50" base="10"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</threadImpl>
|
||||
<threadType name="Receive_Send_Data_MCU1">
|
||||
<comment>--this thread recoit les donnees de MCU1 en mode manuel </comment>
|
||||
<comment>-- updated</comment>
|
||||
<features>
|
||||
<eventDataPort name="RC_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]"/>
|
||||
<dataPort name="RC_Rec_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]">
|
||||
<comment>-- updated</comment>
|
||||
</dataPort>
|
||||
<eventPort name="RC_Evt_Output" direction="out"/>
|
||||
<dataPort name="MCU0_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]">
|
||||
<comment>--updated (juste pour le changement de mode)</comment>
|
||||
</dataPort>
|
||||
<eventDataPort name="MCU0_Send_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]"/>
|
||||
</features>
|
||||
</threadType>
|
||||
<threadImpl name="Receive_Send_Data_MCU1.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]">
|
||||
<comment>--properties</comment>
|
||||
<comment>--Period => ??????</comment>
|
||||
</threadImpl>
|
||||
<threadType name="Control_By_Radio_Cde">
|
||||
<features>
|
||||
<dataPort name="RC_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Tableau.Generic]"/>
|
||||
<dataPort name="Angle_Roulis_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
<dataPort name="Angle_Tangage_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
<dataPort name="Gaz_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
</features>
|
||||
</threadType>
|
||||
<processType name="Nav_Stab_Control_Proc">
|
||||
<comment>--updated</comment>
|
||||
<features>
|
||||
<eventDataPort name="MCU1_Data_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>--MCU1_Evt_Input : in event port;--Erreur Radio Cde --updated</comment>
|
||||
</eventDataPort>
|
||||
<dataPort name="GPS_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Stream_Element.Generic]">
|
||||
<comment>--updated --Param Radio_Cde (mode manuel)</comment>
|
||||
</dataPort>
|
||||
<eventPort name="GPS_Evt_Input"/>
|
||||
<dataPort name="IR_Input"/>
|
||||
<dataPort name="P_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]">
|
||||
<comment>-- new data type</comment>
|
||||
</dataPort>
|
||||
<dataPort name="T_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataPort name="Compas_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Float]"/>
|
||||
<dataPort name="V_Air_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]">
|
||||
<comment>-- new data types</comment>
|
||||
</dataPort>
|
||||
<eventDataPort name="Trans_MCU0_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>-- donnees transmies a MCU1</comment>
|
||||
</eventDataPort>
|
||||
<eventDataPort name="Trans_Sol_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>-- updated</comment>
|
||||
<comment>-- donnees transmies a la station sol</comment>
|
||||
</eventDataPort>
|
||||
</features>
|
||||
</processType>
|
||||
<processImpl name="Nav_Stab_Control_Proc.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]">
|
||||
<comment>--updated</comment>
|
||||
<connections>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Ctrl_By_RC_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]/features/dataPort[@name=RC_Rec_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Control_By_Radio_Cde]/features/dataPort[@name=RC_Data_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Ctrl_By_RC_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Stab_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Control_By_Radio_Cde]/features/dataPort[@name=Angle_Roulis_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]/features/dataPort[@name=Roulis_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Ctrl_By_RC_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Stab_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Control_By_Radio_Cde]/features/dataPort[@name=Angle_Tangage_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]/features/dataPort[@name=Tangage_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Ctrl_By_RC_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Climb_Ctrl_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Control_By_Radio_Cde]/features/dataPort[@name=Gaz_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Climb_Control]/features/dataPort[@name=Montee_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Data_Acq_Filt_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=S_Mission_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Data_Acquisition_GPS]/features/dataPort[@name=GPS_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]/features/dataPort[@name=GPS_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Data_Acq_Filt_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Alt_Ctrl_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Data_Acquisition_GPS]/features/dataPort[@name=GPS_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Altitude_Control]/features/dataPort[@name=GPS_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Data_Acq_Filt_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Climb_Ctrl_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Data_Acquisition_GPS]/features/dataPort[@name=GPS_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Climb_Control]/features/dataPort[@name=GPS_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=S_Mission_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Alt_Ctrl_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]/features/dataPort[@name=Cap_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Altitude_Control]/features/dataPort[@name=S_Miss_Cap_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=S_Mission_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Alt_Ctrl_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]/features/dataPort[@name=Alt_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Altitude_Control]/features/dataPort[@name=S_Miss_Alt_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Alt_Ctrl_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Nav_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Altitude_Control]/features/dataPort[@name=Cap_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Navigation]/features/dataPort[@name=AC_Cap_Input]">
|
||||
<comment>--en mode manuel c'est le thread Control by radio commande qui fournit les donnees</comment>
|
||||
<comment>--au thread stabilisation</comment>
|
||||
<comment>--data port S_Mission_Th.RC_Data_Output->Stab_Th.RC_Data_Input in modes(manual);</comment>
|
||||
<comment>--data port Rec_Send_Th.RC_Rec_Output->S_Mission_Th.RC_Data_Input in modes(manual);</comment>
|
||||
</dataConnection>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Alt_Ctrl_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Nav_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Altitude_Control]/features/dataPort[@name=Alt_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Navigation]/features/dataPort[@name=AC_Alt_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Nav_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Climb_Ctrl_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Navigation]/features/dataPort[@name=Taux_Montee_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Climb_Control]/features/dataPort[@name=Montee_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Climb_Ctrl_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Stab_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Climb_Control]/features/dataPort[@name=Montee_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]/features/dataPort[@name=CC_Montee_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Nav_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Stab_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Navigation]/features/dataPort[@name=Angle_Roulis_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]/features/dataPort[@name=Roulis_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Nav_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Stab_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Navigation]/features/dataPort[@name=Angle_Tangage_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]/features/dataPort[@name=Tangage_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Stab_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=IR_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]/features/dataPort[@name=IR_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Stab_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Stabilisation]/features/dataPort[@name=Param_Vol_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]/features/dataPort[@name=MCU0_Data_Input]"/>
|
||||
<eventDataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventDataPort[@name=Trans_MCU0_Output]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]/features/eventDataPort[@name=MCU0_Send_Output]"/>
|
||||
<eventDataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventDataPort[@name=Trans_Sol_Output]" src="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]/features/eventDataPort[@name=MCU0_Send_Output]">
|
||||
<comment>--updated</comment>
|
||||
</eventDataConnection>
|
||||
<eventDataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]/features/eventDataPort[@name=RC_Data_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventDataPort[@name=MCU1_Data_Input]">
|
||||
<comment>--new--updated</comment>
|
||||
</eventDataConnection>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=S_Mission_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=P_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]/features/dataPort[@name=P_Input]">
|
||||
<comment>--updated</comment>
|
||||
</dataConnection>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=S_Mission_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=T_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]/features/dataPort[@name=T_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=S_Mission_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=Compas_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]/features/dataPort[@name=Compas_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=S_Mission_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=V_Air_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Simple_Mission]/features/dataPort[@name=V_Air_Input]"/>
|
||||
<dataConnection inModes="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" srcContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Data_Acq_Filt_Th]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=GPS_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Data_Acquisition_GPS]/features/dataPort[@name=GPS_Input]"/>
|
||||
</connections>
|
||||
<modes>
|
||||
<mode name="manual" state="INITIAL"/>
|
||||
<mode name="auto"/>
|
||||
<mode name="home"/>
|
||||
<modeTransition srcMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" dstMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto]" triggerEventPort="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]/features/eventPort[@name=RC_Evt_Output]" triggerEventPortContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th]">
|
||||
<comment>--Transition entre les modes</comment>
|
||||
</modeTransition>
|
||||
<modeTransition srcMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=auto]" dstMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" triggerEventPort="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventPort[@name=GPS_Evt_Input]" triggerEventPortContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]">
|
||||
<comment>--updated</comment>
|
||||
</modeTransition>
|
||||
<modeTransition srcMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=manual]" dstMode="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/modes/mode[@name=home]" triggerEventPort="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Receive_Send_Data_MCU1]/features/eventPort[@name=RC_Evt_Output] /aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventPort[@name=GPS_Evt_Input]" triggerEventPortContext="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]/subcomponents/threadSubcomponent[@name=Rec_Send_Th] /aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]"/>
|
||||
</modes>
|
||||
<subcomponents>
|
||||
<comment>--new</comment>
|
||||
<threadSubcomponent name="Data_Acq_Filt_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Data_Acquisition_GPS]"/>
|
||||
<threadSubcomponent name="S_Mission_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadImpl[@name=Simple_Mission.Impl]"/>
|
||||
<threadSubcomponent name="Alt_Ctrl_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Altitude_Control]"/>
|
||||
<threadSubcomponent name="Nav_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadImpl[@name=Navigation.Impl]"/>
|
||||
<threadSubcomponent name="Climb_Ctrl_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Climb_Control]"/>
|
||||
<threadSubcomponent name="Stab_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadImpl[@name=Stabilisation.Impl]"/>
|
||||
<threadSubcomponent name="Rec_Send_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadImpl[@name=Receive_Send_Data_MCU1.Impl]"/>
|
||||
<threadSubcomponent name="Ctrl_By_RC_Th" classifier="/aadlSpec[@name=paparazzi_Old Version]/threadType[@name=Control_By_Radio_Cde]"/>
|
||||
<dataSubcomponent name="D_AP_Destination" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Position.simple]">
|
||||
<comment>--the internal data that will store the autopilot settings</comment>
|
||||
</dataSubcomponent>
|
||||
<dataSubcomponent name="D_AP_Airspeed" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
<dataSubcomponent name="D_AP_Altitude" classifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataType[@name=Integer]"/>
|
||||
</subcomponents>
|
||||
</processImpl>
|
||||
<processorType name="Proc_MCU0">
|
||||
<features>
|
||||
<busAccess name="Mem_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Mem_Bus_MCU0]"/>
|
||||
<busAccess name="Dev_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
</features>
|
||||
</processorType>
|
||||
<processorImpl name="Proc_MCU0.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/processorType[@name=Proc_MCU0]">
|
||||
<comment>--RMS (Rate Monotonic Scheduling) est un FPS (Fixed Priority Scheduling) </comment>
|
||||
<comment>--chaque tache a une priorite fixe</comment>
|
||||
<properties>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Scheduling_Protocol]">
|
||||
<propertyValue xsi:type="property:EnumValue" enumLiteral="/aadlSpec[@name=AADL_Project]/propertySet[@name=AADL_Project]/propertyType[@name=Supported_Scheduling_Protocols]/enumLiteral[@name=RMS]"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
</processorImpl>
|
||||
<memoryType name="Ram_MCU0">
|
||||
<features>
|
||||
<busAccess name="Mem_Bus" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Mem_Bus_MCU0]"/>
|
||||
</features>
|
||||
</memoryType>
|
||||
<busType name="Mem_Bus_MCU0"/>
|
||||
<systemType name="MCU0">
|
||||
<comment>-- MCU0 est charge de la Stabilisation, </comment>
|
||||
<comment>-- la navigation, la communication avec MCU1</comment>
|
||||
<comment>-- et la transmission des parametres de vol</comment>
|
||||
<features>
|
||||
<eventDataPort name="D_MCU1_Input" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]"/>
|
||||
<eventDataPort name="D_MCU0_Output" direction="out" dataClassifier="/aadlSpec[@name=Data_Types]/packageSpec[@name=Data_Types]/aadlPublic/dataImpl[@name=Inter_mcu_msg.Generic]">
|
||||
<comment>--updated</comment>
|
||||
<comment>--E_MCU1_Input : in event port;--updated</comment>
|
||||
</eventDataPort>
|
||||
<busAccess name="Bus_Con" direction="required" busClassifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=SPI]">
|
||||
<comment>--updated</comment>
|
||||
<comment>--E_MCU0_Output : out event port;--updated</comment>
|
||||
</busAccess>
|
||||
</features>
|
||||
</systemType>
|
||||
<systemImpl name="MCU0.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU0]">
|
||||
<properties>
|
||||
<comment>--updated</comment>
|
||||
<comment>--event port GPS_dv.Error_Output->E_MCU0_Output; --updated</comment>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Actual_Processor_Binding]" appliesTo="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]">
|
||||
<comment>-- pour relier le processus a la memeoire et au processeur</comment>
|
||||
<propertyValue xsi:type="property:ReferenceValue" referenceElement="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processorSubcomponent[@name=Proc_0]"/>
|
||||
</propertyAssociation>
|
||||
<propertyAssociation propertyDefinition="/aadlSpec[@name=AADL_Properties]/propertySet[@name=AADL_Properties]/propertyDefinition[@name=Actual_Memory_Binding]" appliesTo="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]">
|
||||
<propertyValue xsi:type="property:ReferenceValue" referenceElement="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/memorySubcomponent[@name=Ram]"/>
|
||||
</propertyAssociation>
|
||||
</properties>
|
||||
<connections>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processorSubcomponent[@name=Proc_0]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/processorType[@name=Proc_MCU0]/features/busAccess[@name=Mem_Bus]">
|
||||
<comment>-- bus connections</comment>
|
||||
</busAccessConnection>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/memorySubcomponent[@name=Ram]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Mem_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/memoryType[@name=Ram_MCU0]/features/busAccess[@name=Mem_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processorSubcomponent[@name=Proc_0]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/processorType[@name=Proc_MCU0]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=P_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Pression]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=T_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Tension]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Compas_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compas]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=V_Air_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Vitesse_Air]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Cam_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Camera_Video]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Emet_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Emetteur]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Modem_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Emet_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Modem_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Emetteur]/features/busAccess[@name=Modem_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=GPS_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=GPS]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=IR_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=InfraRed]/features/busAccess[@name=Dev_Bus]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=P_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Pression]/features/dataPort[@name=P_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=P_Input]">
|
||||
<comment>--ports connections</comment>
|
||||
</dataConnection>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=T_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Tension]/features/dataPort[@name=T_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=T_Input]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Compas_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compas]/features/dataPort[@name=Compas_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=Compas_Input]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=V_Air_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Vitesse_Air]/features/dataPort[@name=V_Air_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=V_Air_Input]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=GPS_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=GPS]/features/dataPort[@name=GPS_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/dataPort[@name=GPS_Input]"/>
|
||||
<eventConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=GPS_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventPort[@name=GPS_Evt_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=GPS]/features/eventPort[@name=Error_Output]"/>
|
||||
<eventDataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Emet_dv]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Emetteur]/features/eventDataPort[@name=Trans_Data_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventDataPort[@name=Trans_Sol_Output]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Cam_dv]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/deviceSubcomponent[@name=Emet_dv]" src="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Camera_Video]/features/dataPort[@name=Video_Output]" dst="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Emetteur]/features/dataPort[@name=Video_Input]">
|
||||
<comment>--updated</comment>
|
||||
</dataConnection>
|
||||
<eventDataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" dst="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventDataPort[@name=MCU1_Data_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU0]/features/eventDataPort[@name=D_MCU1_Input]"/>
|
||||
<eventDataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]/subcomponents/processSubcomponent[@name=N_S_C_proc]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]" dst="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU0]/features/eventDataPort[@name=D_MCU0_Output]" src="/aadlSpec[@name=paparazzi_Old Version]/processType[@name=Nav_Stab_Control_Proc]/features/eventDataPort[@name=Trans_MCU0_Output]">
|
||||
<comment>--updated</comment>
|
||||
<comment>--event port E_MCU1_Input->N_S_C_proc.MCU1_Evt_Input; --updated</comment>
|
||||
</eventDataConnection>
|
||||
</connections>
|
||||
<subcomponents>
|
||||
<processorSubcomponent name="Proc_0" classifier="/aadlSpec[@name=paparazzi_Old Version]/processorImpl[@name=Proc_MCU0.Impl]"/>
|
||||
<memorySubcomponent name="Ram" classifier="/aadlSpec[@name=paparazzi_Old Version]/memoryType[@name=Ram_MCU0]"/>
|
||||
<busSubcomponent name="Mem_Bus" classifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Mem_Bus_MCU0]"/>
|
||||
<deviceSubcomponent name="P_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Pression]"/>
|
||||
<deviceSubcomponent name="T_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Tension]"/>
|
||||
<deviceSubcomponent name="Compas_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Compas]"/>
|
||||
<deviceSubcomponent name="V_Air_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Vitesse_Air]"/>
|
||||
<deviceSubcomponent name="Cam_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Camera_Video]"/>
|
||||
<deviceSubcomponent name="Emet_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=Emetteur]"/>
|
||||
<deviceSubcomponent name="GPS_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=GPS]"/>
|
||||
<deviceSubcomponent name="IR_dv" classifier="/aadlSpec[@name=paparazzi_Old Version]/deviceType[@name=InfraRed]"/>
|
||||
<busSubcomponent name="Dev_Bus" classifier="/aadlSpec[@name=paparazzi_Old Version]/busImpl[@name=Device_Bus.MCU0]"/>
|
||||
<busSubcomponent name="Modem_Bus" classifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=Modem]"/>
|
||||
<processSubcomponent name="N_S_C_proc" classifier="/aadlSpec[@name=paparazzi_Old Version]/processImpl[@name=Nav_Stab_Control_Proc.Impl]"/>
|
||||
</subcomponents>
|
||||
</systemImpl>
|
||||
<busType name="SPI">
|
||||
<comment>----------------------------------------------------------------</comment>
|
||||
<comment>-- we model a bus to link the systems</comment>
|
||||
</busType>
|
||||
<systemType name="Systeme_Paparazzi">
|
||||
<comment>--the embedded system</comment>
|
||||
<features>
|
||||
<dataPort name="RC_Data_Input"/>
|
||||
<eventPort name="RC_Evt_Input"/>
|
||||
</features>
|
||||
</systemType>
|
||||
<systemImpl name="Systeme_Paparazzi.Impl" compType="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=Systeme_Paparazzi]">
|
||||
<comment>--updated</comment>
|
||||
<comment>--event port MCU1_sys.RC_Evt_Output->MCU0_sys.E_MCU1_Input;--updated</comment>
|
||||
<connections>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU0_sys]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU0]/features/busAccess[@name=Bus_Con]"/>
|
||||
<busAccessConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU1_sys]" src="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/busSubcomponent[@name=Dev_Bus]" dst="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/busAccess[@name=Bus_Con]"/>
|
||||
<dataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU1_sys]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=Systeme_Paparazzi]/features/dataPort[@name=RC_Data_Input]" dst="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/dataPort[@name=RC_Data_Input]"/>
|
||||
<eventConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU1_sys]" dst="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/eventPort[@name=RC_Evt_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=Systeme_Paparazzi]/features/eventPort[@name=RC_Evt_Input]"/>
|
||||
<eventDataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU0_sys]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU1_sys]" dst="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/eventDataPort[@name=AP_Data_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU0]/features/eventDataPort[@name=D_MCU0_Output]"/>
|
||||
<eventDataConnection srcContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU1_sys]" dstContext="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=Systeme_Paparazzi.Impl]/subcomponents/systemSubcomponent[@name=MCU0_sys]" dst="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU0]/features/eventDataPort[@name=D_MCU1_Input]" src="/aadlSpec[@name=paparazzi_Old Version]/systemType[@name=MCU1]/features/eventDataPort[@name=D_MCU1_Output]">
|
||||
<comment>--updated</comment>
|
||||
<comment>--event port MCU0_sys.E_MCU0_Output->MCU1_sys.AP_Evt_Input;--updated</comment>
|
||||
</eventDataConnection>
|
||||
</connections>
|
||||
<subcomponents>
|
||||
<systemSubcomponent name="MCU0_sys" classifier="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU0.Impl]"/>
|
||||
<systemSubcomponent name="MCU1_sys" classifier="/aadlSpec[@name=paparazzi_Old Version]/systemImpl[@name=MCU1.Impl]"/>
|
||||
<busSubcomponent name="Dev_Bus" classifier="/aadlSpec[@name=paparazzi_Old Version]/busType[@name=SPI]"/>
|
||||
</subcomponents>
|
||||
</systemImpl>
|
||||
</core:AadlSpec>
|
||||
@ -0,0 +1,28 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<core:AadlSpec xmi:version="2.0" xmlns:xmi="http://www.omg.org/XMI" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:core="http://core" xmlns:property="http://property">
|
||||
<propertySet name="Thread_Properties">
|
||||
<propertyConstant name="Non_Preemptive" propertyTypeReference="/aadlSpec[@name=Thread_Properties]/propertySet[@name=Thread_Properties]/propertyConstant[@name=Non_Preemptive]/propertyType">
|
||||
<propertyType xsi:type="property:Aadlinteger"/>
|
||||
<constantValue xsi:type="property:IntegerValue" valueString="0" base="10"/>
|
||||
</propertyConstant>
|
||||
<propertyConstant name="System_Preemption" propertyTypeReference="/aadlSpec[@name=Thread_Properties]/propertySet[@name=Thread_Properties]/propertyConstant[@name=System_Preemption]/propertyType">
|
||||
<propertyType xsi:type="property:Aadlinteger"/>
|
||||
<constantValue xsi:type="property:IntegerValue" valueString="1" value="1" base="10"/>
|
||||
</propertyConstant>
|
||||
<propertyConstant name="Time_Sharing_Preemption" propertyTypeReference="/aadlSpec[@name=Thread_Properties]/propertySet[@name=Thread_Properties]/propertyConstant[@name=Time_Sharing_Preemption]/propertyType">
|
||||
<propertyType xsi:type="property:Aadlinteger"/>
|
||||
<constantValue xsi:type="property:IntegerValue" valueString="2" value="2" base="10"/>
|
||||
</propertyConstant>
|
||||
<propertyDefinition name="Preemption">
|
||||
<propertyType xsi:type="property:EnumType">
|
||||
<enumLiteral name="Non_Preemptive"/>
|
||||
<enumLiteral name="System_Preemption"/>
|
||||
<enumLiteral name="Time_Sharing_Preemption"/>
|
||||
</propertyType>
|
||||
<appliesto>thread</appliesto>
|
||||
</propertyDefinition>
|
||||
<propertyDefinition name="Entrypoints" list="true">
|
||||
<appliesto>thread</appliesto>
|
||||
</propertyDefinition>
|
||||
</propertySet>
|
||||
</core:AadlSpec>
|
||||
@ -0,0 +1,149 @@
|
||||
/* Copyright (c) 2002, Marek Michalkiewicz
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
|
||||
Neither the name of the copyright holders nor the names of
|
||||
contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE. */
|
||||
|
||||
/* $Id: interrupt.h,v 1.1 2011-01-25 09:40:04 plazar Exp $ */
|
||||
|
||||
#ifndef _AVR_INTERRUPT_H_
|
||||
#define _AVR_INTERRUPT_H_
|
||||
|
||||
#include <arch/io.h>
|
||||
|
||||
/** \name Global manipulation of the interrupt flag
|
||||
|
||||
The global interrupt flag is maintained in the I bit of the status
|
||||
register (SREG). */
|
||||
|
||||
/*@{*/
|
||||
|
||||
/** \def sei()
|
||||
\ingroup avr_interrupts
|
||||
|
||||
\code#include <arch/interrupt.h>\endcode
|
||||
|
||||
Enables interrupts by clearing the global interrupt mask. This function
|
||||
actually compiles into a single line of assembly, so there is no function
|
||||
call overhead. */
|
||||
|
||||
#define sei() __asm__ __volatile__ ("sei" ::)
|
||||
|
||||
/** \def cli()
|
||||
\ingroup avr_interrupts
|
||||
|
||||
\code#include <arch/interrupt.h>\endcode
|
||||
|
||||
Disables all interrupts by clearing the global interrupt mask. This function
|
||||
actually compiles into a single line of assembly, so there is no function
|
||||
call overhead. */
|
||||
|
||||
#define cli() __asm__ __volatile__ ("cli" ::)
|
||||
|
||||
/*@}*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** \name Allowing specific system-wide interrupts
|
||||
|
||||
In addition to globally enabling interrupts, each device's particular
|
||||
interrupt needs to be enabled separately if interrupts for this device are
|
||||
desired. While some devices maintain their interrupt enable bit inside
|
||||
the device's register set, external and timer interrupts have system-wide
|
||||
configuration registers.
|
||||
|
||||
Example:
|
||||
|
||||
\code
|
||||
// Enable timer 1 overflow interrupts.
|
||||
timer_enable_int(_BV(TOIE1));
|
||||
|
||||
// Do some work...
|
||||
|
||||
// Disable all timer interrupts.
|
||||
timer_enable_int(0);
|
||||
\endcode
|
||||
|
||||
\note Be careful when you use these functions. If you already have a
|
||||
different interrupt enabled, you could inadvertantly disable it by
|
||||
enabling another intterupt. */
|
||||
|
||||
/*@{*/
|
||||
|
||||
/** \ingroup avr_interrupts
|
||||
\def enable_external_int(mask)
|
||||
\code#include <arch/interrupt.h>\endcode
|
||||
|
||||
This macro gives access to the \c GIMSK register (or \c EIMSK register
|
||||
if using an AVR Mega device or \c GICR register for others). Although this
|
||||
macro is essentially the same as assigning to the register, it does
|
||||
adapt slightly to the type of device being used. This macro is
|
||||
unavailable if none of the registers listed above are defined. */
|
||||
|
||||
/* Define common register definition if available. */
|
||||
|
||||
#if defined(EIMSK)
|
||||
#define __EICR EIMSK
|
||||
#endif
|
||||
#if defined(GIMSK)
|
||||
#define __EICR GIMSK
|
||||
#endif
|
||||
#if defined(GICR)
|
||||
#define __EICR GICR
|
||||
//printf("--EICR=GICR");//FN 5-1-06
|
||||
#endif
|
||||
|
||||
/* If common register defined, define macro. */
|
||||
#if defined(__EICR) || defined(DOXYGEN)
|
||||
#define enable_external_int(mask) (__EICR = mask)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/** \ingroup avr_interrupts
|
||||
|
||||
\code#include <arch/interrupt.h>\endcode
|
||||
|
||||
This function modifies the \c timsk register.
|
||||
The value you pass via \c ints is device specific. */
|
||||
#define __inline__ inline
|
||||
static __inline__ void timer_enable_int ( unsigned char ints )
|
||||
{
|
||||
#ifdef TIMSK
|
||||
TIMSK = ints;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*@}*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
285
targets/wasm-tacle/parallel/PapaBench/arch/include/avr/arch/io.h
Normal file
285
targets/wasm-tacle/parallel/PapaBench/arch/include/avr/arch/io.h
Normal file
@ -0,0 +1,285 @@
|
||||
/* Copyright (c) 2002,2003 Marek Michalkiewicz, Joerg Wunsch
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
|
||||
Neither the name of the copyright holders nor the names of
|
||||
contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE. */
|
||||
|
||||
/* $Id: io.h,v 1.1 2011-01-25 09:40:04 plazar Exp $ */
|
||||
|
||||
/** \defgroup avr_io AVR device-specific IO definitions
|
||||
\code #include <arch/io.h> \endcode
|
||||
|
||||
This header file includes the apropriate IO definitions for the
|
||||
device that has been specified by the <tt>-mmcu=</tt> compiler
|
||||
command-line switch. This is done by diverting to the appropriate
|
||||
file <tt><avr/io</tt><em>XXXX</em><tt>.h></tt> which should
|
||||
never be included directly. Some register names common to all
|
||||
AVR devices are defined directly within <tt><avr/io.h></tt>,
|
||||
but most of the details come from the respective include file.
|
||||
|
||||
Note that this file always includes
|
||||
\code #include <arch/sfr_defs.h> \endcode
|
||||
See \ref avr_sfr for the details.
|
||||
|
||||
Included are definitions of the IO register set and their
|
||||
respective bit values as specified in the Atmel documentation.
|
||||
Note that Atmel is not very consistent in its naming conventions,
|
||||
so even identical functions sometimes get different names on
|
||||
different devices.
|
||||
|
||||
Also included are the specific names useable for interrupt
|
||||
function definitions as documented
|
||||
\ref avr_signames "here".
|
||||
|
||||
Finally, the following macros are defined:
|
||||
|
||||
- \b RAMEND
|
||||
<br>
|
||||
A constant describing the last on-chip RAM location.
|
||||
<br>
|
||||
- \b XRAMEND
|
||||
<br>
|
||||
A constant describing the last possible location in RAM.
|
||||
This is equal to RAMEND for devices that do not allow for
|
||||
external RAM.
|
||||
<br>
|
||||
- \b E2END
|
||||
<br>
|
||||
A constant describing the address of the last EEPROM cell.
|
||||
<br>
|
||||
- \b FLASHEND
|
||||
<br>
|
||||
A constant describing the last byte address in flash ROM.
|
||||
<br>
|
||||
- \b SPM_PAGESIZE
|
||||
<br>
|
||||
For devices with bootloader support, the flash pagesize
|
||||
(in bytes) to be used for the \c SPM instruction. */
|
||||
|
||||
#ifndef _AVR_IO_H_
|
||||
#define _AVR_IO_H_
|
||||
|
||||
#include <arch/sfr_defs.h>
|
||||
|
||||
/*
|
||||
Registers common to all AVR devices.
|
||||
*/
|
||||
|
||||
#if __AVR_ARCH__ != 1
|
||||
/*
|
||||
AVR architecture 1 has no RAM, thus no stack pointer.
|
||||
|
||||
All other archs do have a stack pointer. Some devices have only
|
||||
less than 256 bytes of possible RAM locations (128 Bytes of SRAM
|
||||
and no option for external RAM), thus SPH is officially "reserved"
|
||||
for them. We catch this case below after including the
|
||||
device-specific ioXXXX.h file, by examining XRAMEND, and
|
||||
#undef-ining SP and SPH in that case.
|
||||
*/
|
||||
/* Stack Pointer */
|
||||
#define SP _SFR_IO16(0x3D)
|
||||
#define SPL _SFR_IO8(0x3D)
|
||||
#define SPH _SFR_IO8(0x3E)
|
||||
#endif /* #if __AVR_ARCH__ != 1 */
|
||||
|
||||
/* Status REGister */
|
||||
#define SREG _SFR_IO8(0x3F)
|
||||
|
||||
/* Status Register - SREG */
|
||||
#define SREG_I 7
|
||||
#define SREG_T 6
|
||||
#define SREG_H 5
|
||||
#define SREG_S 4
|
||||
#define SREG_V 3
|
||||
#define SREG_N 2
|
||||
#define SREG_Z 1
|
||||
#define SREG_C 0
|
||||
|
||||
/* Pointer definition */
|
||||
#if __AVR_ARCH__ != 1
|
||||
/* avr1 has only the Z pointer */
|
||||
#define XL r26
|
||||
#define XH r27
|
||||
#define YL r28
|
||||
#define YH r29
|
||||
#endif /* #if __AVR_ARCH__ != 1 */
|
||||
#define ZL r30
|
||||
#define ZH r31
|
||||
|
||||
/*
|
||||
Only few devices come without EEPROM. In order to assemble the
|
||||
EEPROM library components without defining a specific device, we
|
||||
keep the EEPROM-related definitions here, and catch the devices
|
||||
without EEPROM (E2END == 0) below. Obviously, the EEPROM library
|
||||
functions will not work for them. ;-)
|
||||
*/
|
||||
/* EEPROM Control Register */
|
||||
#define EECR _SFR_IO8(0x1C)
|
||||
|
||||
/* EEPROM Data Register */
|
||||
#define EEDR _SFR_IO8(0x1D)
|
||||
|
||||
/* EEPROM Address Register */
|
||||
#define EEAR _SFR_IO16(0x1E)
|
||||
#define EEARL _SFR_IO8(0x1E)
|
||||
#define EEARH _SFR_IO8(0x1F)
|
||||
|
||||
/* EEPROM Control Register */
|
||||
#define EERIE 3
|
||||
#define EEMWE 2
|
||||
#define EEWE 1
|
||||
#define EERE 0
|
||||
|
||||
#if defined (__AVR_AT94K__)
|
||||
# include <arch/ioat94k.h>
|
||||
#elif defined (__AVR_AT43USB320__)
|
||||
# include <arch/io43u32x.h>
|
||||
#elif defined (__AVR_AT43USB355__)
|
||||
# include <arch/io43u35x.h>
|
||||
#elif defined (__AVR_AT76C711__)
|
||||
# include <arch/io76c711.h>
|
||||
#elif defined (__AVR_AT86RF401__)
|
||||
# include <arch/io86r401.h>
|
||||
#elif defined (__AVR_ATmega128__)
|
||||
# include <arch/iom128.h>
|
||||
#elif defined (__AVR_AT90CAN128__)
|
||||
# include <arch/iocan128.h>
|
||||
#elif defined (__AVR_ATmega64__)
|
||||
# include <arch/iom64.h>
|
||||
#elif defined (__AVR_ATmega645__)
|
||||
# include <arch/iom645.h>
|
||||
#elif defined (__AVR_ATmega6450__)
|
||||
# include <arch/iom6450.h>
|
||||
#elif defined (__AVR_ATmega103__)
|
||||
# include <arch/iom103.h>
|
||||
#elif defined (__AVR_ATmega32__)
|
||||
# include <arch/iom32.h>
|
||||
#elif defined (__AVR_ATmega323__)
|
||||
# include <arch/iom323.h>
|
||||
#elif defined (__AVR_ATmega325__)
|
||||
# include <arch/iom325.h>
|
||||
#elif defined (__AVR_ATmega3250__)
|
||||
# include <arch/iom3250.h>
|
||||
#elif defined (__AVR_ATmega16__)
|
||||
# include <arch/iom16.h>
|
||||
#elif defined (__AVR_ATmega161__)
|
||||
# include <arch/iom161.h>
|
||||
#elif defined (__AVR_ATmega162__)
|
||||
# include <arch/iom162.h>
|
||||
#elif defined (__AVR_ATmega163__)
|
||||
# include <arch/iom163.h>
|
||||
#elif defined (__AVR_ATmega165__)
|
||||
# include <arch/iom165.h>
|
||||
#elif defined (__AVR_ATmega168__)
|
||||
# include <arch/iom168.h>
|
||||
#elif defined (__AVR_ATmega169__)
|
||||
# include <arch/iom169.h>
|
||||
#elif defined (__AVR_ATmega8__)
|
||||
# include <arch/iom8.h>
|
||||
#elif defined (__AVR_ATmega48__)
|
||||
# include <arch/iom48.h>
|
||||
#elif defined (__AVR_ATmega88__)
|
||||
# include <arch/iom88.h>
|
||||
#elif defined (__AVR_ATmega8515__)
|
||||
# include <arch/iom8515.h>
|
||||
#elif defined (__AVR_ATmega8535__)
|
||||
# include <arch/iom8535.h>
|
||||
#elif defined (__AVR_AT90S8535__)
|
||||
# include <arch/io8535.h>
|
||||
#elif defined (__AVR_AT90C8534__)
|
||||
# include <arch/io8534.h>
|
||||
#elif defined (__AVR_AT90S8515__)
|
||||
# include <arch/io8515.h>
|
||||
#elif defined (__AVR_AT90S4434__)
|
||||
# include <arch/io4434.h>
|
||||
#elif defined (__AVR_AT90S4433__)
|
||||
# include <arch/io4433.h>
|
||||
#elif defined (__AVR_AT90S4414__)
|
||||
# include <arch/io4414.h>
|
||||
#elif defined (__AVR_ATtiny22__)
|
||||
# include <arch/iotn22.h>
|
||||
#elif defined (__AVR_ATtiny26__)
|
||||
# include <arch/iotn26.h>
|
||||
#elif defined (__AVR_AT90S2343__)
|
||||
# include <arch/io2343.h>
|
||||
#elif defined (__AVR_AT90S2333__)
|
||||
# include <arch/io2333.h>
|
||||
#elif defined (__AVR_AT90S2323__)
|
||||
# include <arch/io2323.h>
|
||||
#elif defined (__AVR_AT90S2313__)
|
||||
# include <arch/io2313.h>
|
||||
#elif defined (__AVR_ATtiny2313__)
|
||||
# include <arch/iotn2313.h>
|
||||
#elif defined (__AVR_ATtiny13__)
|
||||
# include <arch/iotn13.h>
|
||||
/* avr1: the following only supported for assembler programs */
|
||||
#elif defined (__AVR_ATtiny28__)
|
||||
# include <arch/iotn28.h>
|
||||
#elif defined (__AVR_AT90S1200__)
|
||||
# include <arch/io1200.h>
|
||||
#elif defined (__AVR_ATtiny15__)
|
||||
# include <arch/iotn15.h>
|
||||
#elif defined (__AVR_ATtiny12__)
|
||||
# include <arch/iotn12.h>
|
||||
#elif defined (__AVR_ATtiny11__)
|
||||
# include <arch/iotn11.h>
|
||||
#else
|
||||
# if !defined(__COMPILING_AVR_LIBC__)
|
||||
# warning "device type not defined"
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#include <arch/portpins.h>
|
||||
|
||||
#if __AVR_ARCH__ != 1
|
||||
# if XRAMEND < 0x100 && !defined(__COMPILING_AVR_LIBC__)
|
||||
# undef SP
|
||||
# define SP _SFR_IO8(0x3D)
|
||||
# undef SPH
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#if E2END == 0 && !defined(__COMPILING_AVR_LIBC__)
|
||||
# undef EECR
|
||||
# undef EEDR
|
||||
# undef EEARL
|
||||
# undef EEMWE
|
||||
# undef EEWE
|
||||
# undef EERE
|
||||
#endif
|
||||
#if E2END < 0x100 && !defined(__COMPILING_AVR_LIBC__)
|
||||
# undef EEAR
|
||||
# if E2END > 0
|
||||
# define EEAR _SFR_IO8(0x1E)
|
||||
# endif
|
||||
# undef EEARH
|
||||
#endif
|
||||
#if !defined(SIG_EEPROM_READY)
|
||||
# undef EERIE
|
||||
#endif
|
||||
|
||||
#endif /* _AVR_IO_H_ */
|
||||
1035
targets/wasm-tacle/parallel/PapaBench/arch/include/avr/arch/iom128.h
Normal file
1035
targets/wasm-tacle/parallel/PapaBench/arch/include/avr/arch/iom128.h
Normal file
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,498 @@
|
||||
/* Copyright (c) 2002, Marek Michalkiewicz
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
|
||||
Neither the name of the copyright holders nor the names of
|
||||
contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE. */
|
||||
|
||||
/* $Id: iom8.h,v 1.1 2011-01-25 09:40:04 plazar Exp $ */
|
||||
|
||||
/* avr/iom8.h - definitions for ATmega8 */
|
||||
|
||||
#ifndef _AVR_IOM8_H_
|
||||
#define _AVR_IOM8_H_ 1
|
||||
|
||||
/* This file should only be included from <arch/io.h>, never directly. */
|
||||
|
||||
#ifndef _AVR_IO_H_
|
||||
# error "Include <arch/io.h> instead of this file."
|
||||
#endif
|
||||
|
||||
#ifndef _AVR_IOXXX_H_
|
||||
# define _AVR_IOXXX_H_ "iom8.h"
|
||||
#else
|
||||
# error "Attempt to include more than one <arch/ioXXX.h> file."
|
||||
#endif
|
||||
|
||||
/* I/O registers */
|
||||
|
||||
/* TWI stands for "Two Wire Interface" or "TWI Was I2C(tm)" */
|
||||
#define TWBR _SFR_IO8(0x00)
|
||||
#define TWSR _SFR_IO8(0x01)
|
||||
#define TWAR _SFR_IO8(0x02)
|
||||
#define TWDR _SFR_IO8(0x03)
|
||||
|
||||
/* ADC */
|
||||
#define ADCW _SFR_IO16(0x04)
|
||||
#ifndef __ASSEMBLER__
|
||||
#define ADC _SFR_IO16(0x04)
|
||||
#endif
|
||||
#define ADCL _SFR_IO8(0x04)
|
||||
#define ADCH _SFR_IO8(0x05)
|
||||
#define ADCSR _SFR_IO8(0x06)
|
||||
#define ADCSRA _SFR_IO8(0x06) /* Changed in 2486H-AVR-09/02 */
|
||||
#define ADMUX _SFR_IO8(0x07)
|
||||
|
||||
/* analog comparator */
|
||||
#define ACSR _SFR_IO8(0x08)
|
||||
|
||||
/* USART */
|
||||
#define UBRRL _SFR_IO8(0x09)
|
||||
#define UCSRB _SFR_IO8(0x0A)
|
||||
#define UCSRA _SFR_IO8(0x0B)
|
||||
#define UDR _SFR_IO8(0x0C)
|
||||
|
||||
/* SPI */
|
||||
#define SPCR _SFR_IO8(0x0D)
|
||||
#define SPSR _SFR_IO8(0x0E)
|
||||
#define SPDR _SFR_IO8(0x0F)
|
||||
|
||||
/* Port D */
|
||||
#define PIND _SFR_IO8(0x10)
|
||||
#define DDRD _SFR_IO8(0x11)
|
||||
#define PORTD _SFR_IO8(0x12)
|
||||
|
||||
/* Port C */
|
||||
#define PINC _SFR_IO8(0x13)
|
||||
#define DDRC _SFR_IO8(0x14)
|
||||
#define PORTC _SFR_IO8(0x15)
|
||||
|
||||
/* Port B */
|
||||
#define PINB _SFR_IO8(0x16)
|
||||
#define DDRB _SFR_IO8(0x17)
|
||||
#define PORTB _SFR_IO8(0x18)
|
||||
|
||||
/* 0x1C..0x1F EEPROM */
|
||||
|
||||
#define UCSRC _SFR_IO8(0x20)
|
||||
#define UBRRH _SFR_IO8(0x20)
|
||||
|
||||
#define WDTCR _SFR_IO8(0x21)
|
||||
#define ASSR _SFR_IO8(0x22)
|
||||
|
||||
/* Timer 2 */
|
||||
#define OCR2 _SFR_IO8(0x23)
|
||||
#define TCNT2 _SFR_IO8(0x24)
|
||||
#define TCCR2 _SFR_IO8(0x25)
|
||||
|
||||
/* Timer 1 */
|
||||
#define ICR1 _SFR_IO16(0x26)
|
||||
#define ICR1L _SFR_IO8(0x26)
|
||||
#define ICR1H _SFR_IO8(0x27)
|
||||
#define OCR1B _SFR_IO16(0x28)
|
||||
#define OCR1BL _SFR_IO8(0x28)
|
||||
#define OCR1BH _SFR_IO8(0x29)
|
||||
#define OCR1A _SFR_IO16(0x2A)
|
||||
#define OCR1AL _SFR_IO8(0x2A)
|
||||
#define OCR1AH _SFR_IO8(0x2B)
|
||||
#define TCNT1 _SFR_IO16(0x2C)
|
||||
#define TCNT1L _SFR_IO8(0x2C)
|
||||
#define TCNT1H _SFR_IO8(0x2D)
|
||||
#define TCCR1B _SFR_IO8(0x2E)
|
||||
#define TCCR1A _SFR_IO8(0x2F)
|
||||
|
||||
#define SFIOR _SFR_IO8(0x30)
|
||||
|
||||
#define OSCCAL _SFR_IO8(0x31)
|
||||
|
||||
/* Timer 0 */
|
||||
#define TCNT0 _SFR_IO8(0x32)
|
||||
#define TCCR0 _SFR_IO8(0x33)
|
||||
|
||||
#define MCUCSR _SFR_IO8(0x34)
|
||||
#define MCUCR _SFR_IO8(0x35)
|
||||
|
||||
#define TWCR _SFR_IO8(0x36)
|
||||
|
||||
#define SPMCR _SFR_IO8(0x37)
|
||||
|
||||
#define TIFR _SFR_IO8(0x38)
|
||||
#define TIMSK _SFR_IO8(0x39)
|
||||
|
||||
#define GIFR _SFR_IO8(0x3A)
|
||||
#define GIMSK _SFR_IO8(0x3B)
|
||||
#define GICR _SFR_IO8(0x3B) /* Changed in 2486H-AVR-09/02 */
|
||||
|
||||
/* 0x3C reserved (OCR0?) */
|
||||
|
||||
/* 0x3D..0x3E SP */
|
||||
|
||||
/* 0x3F SREG */
|
||||
|
||||
/* Interrupt vectors */
|
||||
|
||||
#define SIG_INTERRUPT0 _VECTOR(1)
|
||||
#define SIG_INTERRUPT1 _VECTOR(2)
|
||||
#define SIG_OUTPUT_COMPARE2 _VECTOR(3)
|
||||
#define SIG_OVERFLOW2 _VECTOR(4)
|
||||
#define SIG_INPUT_CAPTURE1 _VECTOR(5)
|
||||
#define SIG_OUTPUT_COMPARE1A _VECTOR(6)
|
||||
#define SIG_OUTPUT_COMPARE1B _VECTOR(7)
|
||||
#define SIG_OVERFLOW1 _VECTOR(8)
|
||||
#define SIG_OVERFLOW0 _VECTOR(9)
|
||||
#define SIG_SPI _VECTOR(10)
|
||||
#define SIG_UART_RECV _VECTOR(11)
|
||||
#define SIG_UART_DATA _VECTOR(12)
|
||||
#define SIG_UART_TRANS _VECTOR(13)
|
||||
#define SIG_ADC _VECTOR(14)
|
||||
#define SIG_EEPROM_READY _VECTOR(15)
|
||||
#define SIG_COMPARATOR _VECTOR(16)
|
||||
#define SIG_2WIRE_SERIAL _VECTOR(17)
|
||||
#define SIG_SPM_READY _VECTOR(18)
|
||||
|
||||
#define _VECTORS_SIZE 38
|
||||
|
||||
/* Bit numbers */
|
||||
|
||||
/* GIMSK / GICR */
|
||||
#define INT1 7
|
||||
#define INT0 6
|
||||
#define IVSEL 1
|
||||
#define IVCE 0
|
||||
|
||||
/* GIFR */
|
||||
#define INTF1 7
|
||||
#define INTF0 6
|
||||
|
||||
/* TIMSK */
|
||||
#define OCIE2 7
|
||||
#define TOIE2 6
|
||||
#define TICIE1 5
|
||||
#define OCIE1A 4
|
||||
#define OCIE1B 3
|
||||
#define TOIE1 2
|
||||
/* bit 1 reserved (OCIE0?) */
|
||||
#define TOIE0 0
|
||||
|
||||
/* TIFR */
|
||||
#define OCF2 7
|
||||
#define TOV2 6
|
||||
#define ICF1 5
|
||||
#define OCF1A 4
|
||||
#define OCF1B 3
|
||||
#define TOV1 2
|
||||
/* bit 1 reserved (OCF0?) */
|
||||
#define TOV0 0
|
||||
|
||||
/* SPMCR */
|
||||
#define SPMIE 7
|
||||
#define RWWSB 6
|
||||
/* bit 5 reserved */
|
||||
#define RWWSRE 4
|
||||
#define BLBSET 3
|
||||
#define PGWRT 2
|
||||
#define PGERS 1
|
||||
#define SPMEN 0
|
||||
|
||||
/* TWCR */
|
||||
#define TWINT 7
|
||||
#define TWEA 6
|
||||
#define TWSTA 5
|
||||
#define TWSTO 4
|
||||
#define TWWC 3
|
||||
#define TWEN 2
|
||||
/* bit 1 reserved (TWI_TST?) */
|
||||
#define TWIE 0
|
||||
|
||||
/* TWAR */
|
||||
#define TWA6 7
|
||||
#define TWA5 6
|
||||
#define TWA4 5
|
||||
#define TWA3 4
|
||||
#define TWA2 3
|
||||
#define TWA1 2
|
||||
#define TWA0 1
|
||||
#define TWGCE 0
|
||||
|
||||
/* TWSR */
|
||||
#define TWS7 7
|
||||
#define TWS6 6
|
||||
#define TWS5 5
|
||||
#define TWS4 4
|
||||
#define TWS3 3
|
||||
/* bit 2 reserved */
|
||||
#define TWPS1 1
|
||||
#define TWPS0 0
|
||||
|
||||
/* MCUCR */
|
||||
#define SE 7
|
||||
#define SM2 6
|
||||
#define SM1 5
|
||||
#define SM0 4
|
||||
#define ISC11 3
|
||||
#define ISC10 2
|
||||
#define ISC01 1
|
||||
#define ISC00 0
|
||||
|
||||
/* MCUCSR */
|
||||
/* bits 7-4 reserved */
|
||||
#define WDRF 3
|
||||
#define BORF 2
|
||||
#define EXTRF 1
|
||||
#define PORF 0
|
||||
|
||||
/* SFIOR */
|
||||
/* bits 7-5 reserved */
|
||||
#define ADHSM 4
|
||||
#define ACME 3
|
||||
#define PUD 2
|
||||
#define PSR2 1
|
||||
#define PSR10 0
|
||||
|
||||
/* TCCR0 */
|
||||
/* bits 7-3 reserved */
|
||||
#define CS02 2
|
||||
#define CS01 1
|
||||
#define CS00 0
|
||||
|
||||
/* TCCR2 */
|
||||
#define FOC2 7
|
||||
#define WGM20 6
|
||||
#define COM21 5
|
||||
#define COM20 4
|
||||
#define WGM21 3
|
||||
#define CS22 2
|
||||
#define CS21 1
|
||||
#define CS20 0
|
||||
|
||||
/* ASSR */
|
||||
/* bits 7-4 reserved */
|
||||
#define AS2 3
|
||||
#define TCN2UB 2
|
||||
#define OCR2UB 1
|
||||
#define TCR2UB 0
|
||||
|
||||
/* TCCR1A */
|
||||
#define COM1A1 7
|
||||
#define COM1A0 6
|
||||
#define COM1B1 5
|
||||
#define COM1B0 4
|
||||
#define FOC1A 3
|
||||
#define FOC1B 2
|
||||
#define WGM11 1
|
||||
#define WGM10 0
|
||||
|
||||
/* TCCR1B */
|
||||
#define ICNC1 7
|
||||
#define ICES1 6
|
||||
/* bit 5 reserved */
|
||||
#define WGM13 4
|
||||
#define WGM12 3
|
||||
#define CS12 2
|
||||
#define CS11 1
|
||||
#define CS10 0
|
||||
|
||||
/* WDTCR */
|
||||
/* bits 7-5 reserved */
|
||||
#define WDCE 4
|
||||
#define WDE 3
|
||||
#define WDP2 2
|
||||
#define WDP1 1
|
||||
#define WDP0 0
|
||||
|
||||
/* UBRRH */
|
||||
#define URSEL 7
|
||||
|
||||
/* UCSRC */
|
||||
#define URSEL 7
|
||||
#define UMSEL 6
|
||||
#define UPM1 5
|
||||
#define UPM0 4
|
||||
#define USBS 3
|
||||
#define UCSZ1 2
|
||||
#define UCSZ0 1
|
||||
#define UCPOL 0
|
||||
|
||||
/* PORTB */
|
||||
#define PB7 7
|
||||
#define PB6 6
|
||||
#define PB5 5
|
||||
#define PB4 4
|
||||
#define PB3 3
|
||||
#define PB2 2
|
||||
#define PB1 1
|
||||
#define PB0 0
|
||||
|
||||
/* DDRB */
|
||||
#define DDB7 7
|
||||
#define DDB6 6
|
||||
#define DDB5 5
|
||||
#define DDB4 4
|
||||
#define DDB3 3
|
||||
#define DDB2 2
|
||||
#define DDB1 1
|
||||
#define DDB0 0
|
||||
|
||||
/* PINB */
|
||||
#define PINB7 7
|
||||
#define PINB6 6
|
||||
#define PINB5 5
|
||||
#define PINB4 4
|
||||
#define PINB3 3
|
||||
#define PINB2 2
|
||||
#define PINB1 1
|
||||
#define PINB0 0
|
||||
|
||||
/* PORTC */
|
||||
#define PC6 6
|
||||
#define PC5 5
|
||||
#define PC4 4
|
||||
#define PC3 3
|
||||
#define PC2 2
|
||||
#define PC1 1
|
||||
#define PC0 0
|
||||
|
||||
/* DDRC */
|
||||
#define DDC6 6
|
||||
#define DDC5 5
|
||||
#define DDC4 4
|
||||
#define DDC3 3
|
||||
#define DDC2 2
|
||||
#define DDC1 1
|
||||
#define DDC0 0
|
||||
|
||||
/* PINC */
|
||||
#define PINC6 6
|
||||
#define PINC5 5
|
||||
#define PINC4 4
|
||||
#define PINC3 3
|
||||
#define PINC2 2
|
||||
#define PINC1 1
|
||||
#define PINC0 0
|
||||
|
||||
/* PORTD */
|
||||
#define PD7 7
|
||||
#define PD6 6
|
||||
#define PD5 5
|
||||
#define PD4 4
|
||||
#define PD3 3
|
||||
#define PD2 2
|
||||
#define PD1 1
|
||||
#define PD0 0
|
||||
|
||||
/* DDRD */
|
||||
#define DDD7 7
|
||||
#define DDD6 6
|
||||
#define DDD5 5
|
||||
#define DDD4 4
|
||||
#define DDD3 3
|
||||
#define DDD2 2
|
||||
#define DDD1 1
|
||||
#define DDD0 0
|
||||
|
||||
/* PIND */
|
||||
#define PIND7 7
|
||||
#define PIND6 6
|
||||
#define PIND5 5
|
||||
#define PIND4 4
|
||||
#define PIND3 3
|
||||
#define PIND2 2
|
||||
#define PIND1 1
|
||||
#define PIND0 0
|
||||
|
||||
/* SPSR */
|
||||
#define SPIF 7
|
||||
#define WCOL 6
|
||||
#define SPI2X 0
|
||||
|
||||
/* SPCR */
|
||||
#define SPIE 7
|
||||
#define SPE 6
|
||||
#define DORD 5
|
||||
#define MSTR 4
|
||||
#define CPOL 3
|
||||
#define CPHA 2
|
||||
#define SPR1 1
|
||||
#define SPR0 0
|
||||
|
||||
/* UCSRA */
|
||||
#define RXC 7
|
||||
#define TXC 6
|
||||
#define UDRE 5
|
||||
#define FE 4
|
||||
#define DOR 3
|
||||
#define PE 2
|
||||
#define U2X 1
|
||||
#define MPCM 0
|
||||
|
||||
/* UCSRB */
|
||||
#define RXCIE 7
|
||||
#define TXCIE 6
|
||||
#define UDRIE 5
|
||||
#define RXEN 4
|
||||
#define TXEN 3
|
||||
#define UCSZ2 2
|
||||
#define RXB8 1
|
||||
#define TXB8 0
|
||||
|
||||
/* ACSR */
|
||||
#define ACD 7
|
||||
#define ACBG 6
|
||||
#define ACO 5
|
||||
#define ACI 4
|
||||
#define ACIE 3
|
||||
#define ACIC 2
|
||||
#define ACIS1 1
|
||||
#define ACIS0 0
|
||||
|
||||
/* ADCSR / ADCSRA */
|
||||
#define ADEN 7
|
||||
#define ADSC 6
|
||||
#define ADFR 5
|
||||
#define ADIF 4
|
||||
#define ADIE 3
|
||||
#define ADPS2 2
|
||||
#define ADPS1 1
|
||||
#define ADPS0 0
|
||||
|
||||
/* ADMUX */
|
||||
#define REFS1 7
|
||||
#define REFS0 6
|
||||
#define ADLAR 5
|
||||
/* bit 4 reserved */
|
||||
#define MUX3 3
|
||||
#define MUX2 2
|
||||
#define MUX1 1
|
||||
#define MUX0 0
|
||||
|
||||
/* Constants */
|
||||
#define SPM_PAGESIZE 64
|
||||
#define RAMEND 0x45F
|
||||
#define XRAMEND 0x45F
|
||||
#define E2END 0x1FF
|
||||
#define FLASHEND 0x1FFF
|
||||
|
||||
#endif /* _AVR_IOM8_H_ */
|
||||
@ -0,0 +1,265 @@
|
||||
/* Copyright (c) 2003 Theodore A. Roth
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
|
||||
Neither the name of the copyright holders nor the names of
|
||||
contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE. */
|
||||
|
||||
/* $Id: portpins.h,v 1.1 2011-01-25 09:40:04 plazar Exp $ */
|
||||
|
||||
#ifndef _AVR_PORTPINS_H_
|
||||
#define _AVR_PORTPINS_H_ 1
|
||||
|
||||
/* This file should only be included from <arch/io.h>, never directly. */
|
||||
|
||||
#ifndef _AVR_IO_H_
|
||||
# error "Include <arch/io.h> instead of this file."
|
||||
#endif
|
||||
|
||||
/* Define Generic PORTn, DDn, and PINn values. */
|
||||
|
||||
/* Port Data Register (generic) */
|
||||
#define PORT7 7
|
||||
#define PORT6 6
|
||||
#define PORT5 5
|
||||
#define PORT4 4
|
||||
#define PORT3 3
|
||||
#define PORT2 2
|
||||
#define PORT1 1
|
||||
#define PORT0 0
|
||||
|
||||
/* Port Data Direction Register (generic) */
|
||||
#define DD7 7
|
||||
#define DD6 6
|
||||
#define DD5 5
|
||||
#define DD4 4
|
||||
#define DD3 3
|
||||
#define DD2 2
|
||||
#define DD1 1
|
||||
#define DD0 0
|
||||
|
||||
/* Port Input Pins (generic) */
|
||||
#define PIN7 7
|
||||
#define PIN6 6
|
||||
#define PIN5 5
|
||||
#define PIN4 4
|
||||
#define PIN3 3
|
||||
#define PIN2 2
|
||||
#define PIN1 1
|
||||
#define PIN0 0
|
||||
|
||||
/* Define PORTxn values for all possible port pins. */
|
||||
|
||||
/* PORT A */
|
||||
|
||||
#if defined(PA0)
|
||||
# define PORTA0 PA0
|
||||
#endif
|
||||
#if defined(PA1)
|
||||
# define PORTA1 PA1
|
||||
#endif
|
||||
#if defined(PA2)
|
||||
# define PORTA2 PA2
|
||||
#endif
|
||||
#if defined(PA3)
|
||||
# define PORTA3 PA3
|
||||
#endif
|
||||
#if defined(PA4)
|
||||
# define PORTA4 PA4
|
||||
#endif
|
||||
#if defined(PA5)
|
||||
# define PORTA5 PA5
|
||||
#endif
|
||||
#if defined(PA6)
|
||||
# define PORTA6 PA6
|
||||
#endif
|
||||
#if defined(PA7)
|
||||
# define PORTA7 PA7
|
||||
#endif
|
||||
|
||||
/* PORT B */
|
||||
|
||||
#if defined(PB0)
|
||||
# define PORTB0 PB0
|
||||
#endif
|
||||
#if defined(PB1)
|
||||
# define PORTB1 PB1
|
||||
#endif
|
||||
#if defined(PB2)
|
||||
# define PORTB2 PB2
|
||||
#endif
|
||||
#if defined(PB3)
|
||||
# define PORTB3 PB3
|
||||
#endif
|
||||
#if defined(PB4)
|
||||
# define PORTB4 PB4
|
||||
#endif
|
||||
#if defined(PB5)
|
||||
# define PORTB5 PB5
|
||||
#endif
|
||||
#if defined(PB6)
|
||||
# define PORTB6 PB6
|
||||
#endif
|
||||
#if defined(PB7)
|
||||
# define PORTB7 PB7
|
||||
#endif
|
||||
|
||||
/* PORT C */
|
||||
|
||||
#if defined(PC0)
|
||||
# define PORTC0 PC0
|
||||
#endif
|
||||
#if defined(PC1)
|
||||
# define PORTC1 PC1
|
||||
#endif
|
||||
#if defined(PC2)
|
||||
# define PORTC2 PC2
|
||||
#endif
|
||||
#if defined(PC3)
|
||||
# define PORTC3 PC3
|
||||
#endif
|
||||
#if defined(PC4)
|
||||
# define PORTC4 PC4
|
||||
#endif
|
||||
#if defined(PC5)
|
||||
# define PORTC5 PC5
|
||||
#endif
|
||||
#if defined(PC6)
|
||||
# define PORTC6 PC6
|
||||
#endif
|
||||
#if defined(PC7)
|
||||
# define PORTC7 PC7
|
||||
#endif
|
||||
|
||||
/* PORT D */
|
||||
|
||||
#if defined(PD0)
|
||||
# define PORTD0 PD0
|
||||
#endif
|
||||
#if defined(PD1)
|
||||
# define PORTD1 PD1
|
||||
#endif
|
||||
#if defined(PD2)
|
||||
# define PORTD2 PD2
|
||||
#endif
|
||||
#if defined(PD3)
|
||||
# define PORTD3 PD3
|
||||
#endif
|
||||
#if defined(PD4)
|
||||
# define PORTD4 PD4
|
||||
#endif
|
||||
#if defined(PD5)
|
||||
# define PORTD5 PD5
|
||||
#endif
|
||||
#if defined(PD6)
|
||||
# define PORTD6 PD6
|
||||
#endif
|
||||
#if defined(PD7)
|
||||
# define PORTD7 PD7
|
||||
#endif
|
||||
|
||||
/* PORT E */
|
||||
|
||||
#if defined(PE0)
|
||||
# define PORTE0 PE0
|
||||
#endif
|
||||
#if defined(PE1)
|
||||
# define PORTE1 PE1
|
||||
#endif
|
||||
#if defined(PE2)
|
||||
# define PORTE2 PE2
|
||||
#endif
|
||||
#if defined(PE3)
|
||||
# define PORTE3 PE3
|
||||
#endif
|
||||
#if defined(PE4)
|
||||
# define PORTE4 PE4
|
||||
#endif
|
||||
#if defined(PE5)
|
||||
# define PORTE5 PE5
|
||||
#endif
|
||||
#if defined(PE6)
|
||||
# define PORTE6 PE6
|
||||
#endif
|
||||
#if defined(PE7)
|
||||
# define PORTE7 PE7
|
||||
#endif
|
||||
|
||||
/* PORT F */
|
||||
|
||||
#if defined(PF0)
|
||||
# define PORTF0 PF0
|
||||
#endif
|
||||
#if defined(PF1)
|
||||
# define PORTF1 PF1
|
||||
#endif
|
||||
#if defined(PF2)
|
||||
# define PORTF2 PF2
|
||||
#endif
|
||||
#if defined(PF3)
|
||||
# define PORTF3 PF3
|
||||
#endif
|
||||
#if defined(PF4)
|
||||
# define PORTF4 PF4
|
||||
#endif
|
||||
#if defined(PF5)
|
||||
# define PORTF5 PF5
|
||||
#endif
|
||||
#if defined(PF6)
|
||||
# define PORTF6 PF6
|
||||
#endif
|
||||
#if defined(PF7)
|
||||
# define PORTF7 PF7
|
||||
#endif
|
||||
|
||||
/* PORT G */
|
||||
|
||||
#if defined(PG0)
|
||||
# define PORTG0 PG0
|
||||
#endif
|
||||
#if defined(PG1)
|
||||
# define PORTG1 PG1
|
||||
#endif
|
||||
#if defined(PG2)
|
||||
# define PORTG2 PG2
|
||||
#endif
|
||||
#if defined(PG3)
|
||||
# define PORTG3 PG3
|
||||
#endif
|
||||
#if defined(PG4)
|
||||
# define PORTG4 PG4
|
||||
#endif
|
||||
#if defined(PG5)
|
||||
# define PORTG5 PG5
|
||||
#endif
|
||||
#if defined(PG6)
|
||||
# define PORTG6 PG6
|
||||
#endif
|
||||
#if defined(PG7)
|
||||
# define PORTG7 PG7
|
||||
#endif
|
||||
|
||||
#endif /* _AVR_PORTPINS_H_ */
|
||||
@ -0,0 +1,250 @@
|
||||
/* Copyright (c) 2002, Marek Michalkiewicz <marekm@amelek.gda.pl>
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
|
||||
Neither the name of the copyright holders nor the names of
|
||||
contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE. */
|
||||
|
||||
/* avr/sfr_defs.h - macros for accessing AVR special function registers */
|
||||
|
||||
/* $Id: sfr_defs.h,v 1.1 2011-01-25 09:40:04 plazar Exp $ */
|
||||
|
||||
#ifndef _AVR_SFR_DEFS_H_
|
||||
#define _AVR_SFR_DEFS_H_ 1
|
||||
|
||||
/** \defgroup avr_sfr_notes Additional notes from <arch/sfr_defs.h>
|
||||
\ingroup avr_sfr
|
||||
|
||||
The \c <arch/sfr_defs.h> file is included by all of the \c <arch/ioXXXX.h>
|
||||
files, which use macros defined here to make the special function register
|
||||
definitions look like C variables or simple constants, depending on the
|
||||
<tt>_SFR_ASM_COMPAT</tt> define. Some examples from \c <arch/iom128.h> to
|
||||
show how to define such macros:
|
||||
|
||||
\code
|
||||
#define PORTA _SFR_IO8(0x1b)
|
||||
#define TCNT1 _SFR_IO16(0x2c)
|
||||
#define PORTF _SFR_MEM8(0x61)
|
||||
#define TCNT3 _SFR_MEM16(0x88)
|
||||
\endcode
|
||||
|
||||
If \c _SFR_ASM_COMPAT is not defined, C programs can use names like
|
||||
<tt>PORTA</tt> directly in C expressions (also on the left side of
|
||||
assignment operators) and GCC will do the right thing (use short I/O
|
||||
instructions if possible). The \c __SFR_OFFSET definition is not used in
|
||||
any way in this case.
|
||||
|
||||
Define \c _SFR_ASM_COMPAT as 1 to make these names work as simple constants
|
||||
(addresses of the I/O registers). This is necessary when included in
|
||||
preprocessed assembler (*.S) source files, so it is done automatically if
|
||||
\c __ASSEMBLER__ is defined. By default, all addresses are defined as if
|
||||
they were memory addresses (used in \c lds/sts instructions). To use these
|
||||
addresses in \c in/out instructions, you must subtract 0x20 from them.
|
||||
|
||||
For more backwards compatibility, insert the following at the start of your
|
||||
old assembler source file:
|
||||
|
||||
\code
|
||||
#define __SFR_OFFSET 0
|
||||
\endcode
|
||||
|
||||
This automatically subtracts 0x20 from I/O space addresses, but it's a
|
||||
hack, so it is recommended to change your source: wrap such addresses in
|
||||
macros defined here, as shown below. After this is done, the
|
||||
<tt>__SFR_OFFSET</tt> definition is no longer necessary and can be removed.
|
||||
|
||||
Real example - this code could be used in a boot loader that is portable
|
||||
between devices with \c SPMCR at different addresses.
|
||||
|
||||
\verbatim
|
||||
<arch/iom163.h>: #define SPMCR _SFR_IO8(0x37)
|
||||
<arch/iom128.h>: #define SPMCR _SFR_MEM8(0x68)
|
||||
\endverbatim
|
||||
|
||||
\code
|
||||
#if _SFR_IO_REG_P(SPMCR)
|
||||
out _SFR_IO_ADDR(SPMCR), r24
|
||||
#else
|
||||
sts _SFR_MEM_ADDR(SPMCR), r24
|
||||
#endif
|
||||
\endcode
|
||||
|
||||
You can use the \c in/out/cbi/sbi/sbic/sbis instructions, without the
|
||||
<tt>_SFR_IO_REG_P</tt> test, if you know that the register is in the I/O
|
||||
space (as with \c SREG, for example). If it isn't, the assembler will
|
||||
complain (I/O address out of range 0...0x3f), so this should be fairly
|
||||
safe.
|
||||
|
||||
If you do not define \c __SFR_OFFSET (so it will be 0x20 by default), all
|
||||
special register addresses are defined as memory addresses (so \c SREG is
|
||||
0x5f), and (if code size and speed are not important, and you don't like
|
||||
the ugly #if above) you can always use lds/sts to access them. But, this
|
||||
will not work if <tt>__SFR_OFFSET</tt> != 0x20, so use a different macro
|
||||
(defined only if <tt>__SFR_OFFSET</tt> == 0x20) for safety:
|
||||
|
||||
\code
|
||||
sts _SFR_ADDR(SPMCR), r24
|
||||
\endcode
|
||||
|
||||
In C programs, all 3 combinations of \c _SFR_ASM_COMPAT and
|
||||
<tt>__SFR_OFFSET</tt> are supported - the \c _SFR_ADDR(SPMCR) macro can be
|
||||
used to get the address of the \c SPMCR register (0x57 or 0x68 depending on
|
||||
device). */
|
||||
|
||||
#ifdef __ASSEMBLER__
|
||||
#define _SFR_ASM_COMPAT 1
|
||||
#endif
|
||||
|
||||
#ifndef __ASSEMBLER__
|
||||
/* These only work in C programs. */
|
||||
#include <inttypes.h>
|
||||
|
||||
#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr))
|
||||
#define _MMIO_WORD(mem_addr) (*(volatile uint16_t *)(mem_addr))
|
||||
#endif
|
||||
|
||||
#if _SFR_ASM_COMPAT
|
||||
|
||||
#ifndef __SFR_OFFSET
|
||||
/* Define as 0 before including this file for compatibility with old asm
|
||||
sources that don't subtract __SFR_OFFSET from symbolic I/O addresses. */
|
||||
#define __SFR_OFFSET 0x20
|
||||
#endif
|
||||
|
||||
#if (__SFR_OFFSET != 0) && (__SFR_OFFSET != 0x20)
|
||||
#error "__SFR_OFFSET must be 0 or 0x20"
|
||||
#endif
|
||||
|
||||
#define _SFR_MEM8(mem_addr) (mem_addr)
|
||||
#define _SFR_MEM16(mem_addr) (mem_addr)
|
||||
#define _SFR_IO8(io_addr) ((io_addr) + __SFR_OFFSET)
|
||||
#define _SFR_IO16(io_addr) ((io_addr) + __SFR_OFFSET)
|
||||
|
||||
#define _SFR_IO_ADDR(sfr) ((sfr) - __SFR_OFFSET)
|
||||
#define _SFR_MEM_ADDR(sfr) (sfr)
|
||||
#define _SFR_IO_REG_P(sfr) ((sfr) < 0x40 + __SFR_OFFSET)
|
||||
|
||||
#if (__SFR_OFFSET == 0x20)
|
||||
/* No need to use ?: operator, so works in assembler too. */
|
||||
#define _SFR_ADDR(sfr) _SFR_MEM_ADDR(sfr)
|
||||
#elif !defined(__ASSEMBLER__)
|
||||
#define _SFR_ADDR(sfr) (_SFR_IO_REG_P(sfr) ? (_SFR_IO_ADDR(sfr) + 0x20) : _SFR_MEM_ADDR(sfr))
|
||||
#endif
|
||||
|
||||
#else /* !_SFR_ASM_COMPAT */
|
||||
|
||||
#define _SFR_MEM8(mem_addr) _MMIO_BYTE(mem_addr)
|
||||
#define _SFR_MEM16(mem_addr) _MMIO_WORD(mem_addr)
|
||||
#define _SFR_IO8(io_addr) _MMIO_BYTE((io_addr) + 0x20)
|
||||
#define _SFR_IO16(io_addr) _MMIO_WORD((io_addr) + 0x20)
|
||||
|
||||
#define _SFR_MEM_ADDR(sfr) ((uint16_t) &(sfr))
|
||||
#define _SFR_IO_ADDR(sfr) (_SFR_MEM_ADDR(sfr) - 0x20)
|
||||
#define _SFR_IO_REG_P(sfr) (_SFR_MEM_ADDR(sfr) < 0x60)
|
||||
|
||||
#define _SFR_ADDR(sfr) _SFR_MEM_ADDR(sfr)
|
||||
|
||||
#endif /* !_SFR_ASM_COMPAT */
|
||||
|
||||
#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
|
||||
#define _SFR_WORD(sfr) _MMIO_WORD(_SFR_ADDR(sfr))
|
||||
|
||||
/** \name Bit manipulation */
|
||||
|
||||
/*@{*/
|
||||
/** \def _BV
|
||||
\ingroup avr_sfr
|
||||
|
||||
\code #include <arch/io.h>\endcode
|
||||
|
||||
Converts a bit number into a byte value.
|
||||
|
||||
\note The bit shift is performed by the compiler which then inserts the
|
||||
result into the code. Thus, there is no run-time overhead when using
|
||||
_BV(). */
|
||||
|
||||
#define _BV(bit) (1 << (bit))
|
||||
|
||||
/*@}*/
|
||||
|
||||
#ifndef _VECTOR
|
||||
#define _VECTOR(N) __vector_ ## N
|
||||
#endif
|
||||
|
||||
#ifndef __ASSEMBLER__
|
||||
|
||||
|
||||
/** \name IO register bit manipulation */
|
||||
|
||||
/*@{*/
|
||||
|
||||
|
||||
|
||||
/** \def bit_is_set
|
||||
\ingroup avr_sfr
|
||||
|
||||
\code #include <arch/io.h>\endcode
|
||||
|
||||
Test whether bit \c bit in IO register \c sfr is set.
|
||||
This will return a 0 if the bit is clear, and non-zero
|
||||
if the bit is set. */
|
||||
|
||||
#define bit_is_set(sfr, bit) (_SFR_BYTE(sfr) & _BV(bit))
|
||||
|
||||
/** \def bit_is_clear
|
||||
\ingroup avr_sfr
|
||||
|
||||
\code #include <arch/io.h>\endcode
|
||||
|
||||
Test whether bit \c bit in IO register \c sfr is clear.
|
||||
This will return non-zero if the bit is clear, and a 0
|
||||
if the bit is set. */
|
||||
|
||||
#define bit_is_clear(sfr, bit) (!(_SFR_BYTE(sfr) & _BV(bit)))
|
||||
|
||||
/** \def loop_until_bit_is_set
|
||||
\ingroup avr_sfr
|
||||
|
||||
\code #include <arch/io.h>\endcode
|
||||
|
||||
Wait until bit \c bit in IO register \c sfr is set. */
|
||||
|
||||
#define loop_until_bit_is_set(sfr, bit) do { } while (bit_is_clear(sfr, bit))
|
||||
|
||||
/** \def loop_until_bit_is_clear
|
||||
\ingroup avr_sfr
|
||||
|
||||
\code #include <arch/io.h>\endcode
|
||||
|
||||
Wait until bit \c bit in IO register \c sfr is clear. */
|
||||
|
||||
#define loop_until_bit_is_clear(sfr, bit) do { } while (bit_is_set(sfr, bit))
|
||||
|
||||
/*@}*/
|
||||
|
||||
#endif /* !__ASSEMBLER__ */
|
||||
|
||||
#endif /* _SFR_DEFS_H_ */
|
||||
@ -0,0 +1,103 @@
|
||||
/* Copyright (c) 2002, Marek Michalkiewicz
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
|
||||
Neither the name of the copyright holders nor the names of
|
||||
contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE. */
|
||||
|
||||
/* $Id: signal.h,v 1.1 2011-01-25 09:40:04 plazar Exp $ */
|
||||
|
||||
#ifndef _AVR_SIGNAL_H_
|
||||
#define _AVR_SIGNAL_H_
|
||||
|
||||
/** \name Macros for writing interrupt handler functions */
|
||||
|
||||
/*@{*/
|
||||
|
||||
/** \def SIGNAL(signame)
|
||||
\ingroup avr_interrupts
|
||||
|
||||
\code#include <arch/signal.h>\endcode
|
||||
|
||||
Introduces an interrupt handler function that runs with global interrupts
|
||||
initially disabled. */
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define SIGNAL(signame) \
|
||||
extern "C" void signame(void); \
|
||||
void signame (void) __attribute__ ((signal)); \
|
||||
void signame (void)
|
||||
#else
|
||||
#define SIGNAL(signame) \
|
||||
void signame (void)
|
||||
#endif
|
||||
|
||||
/** \def INTERRUPT(signame)
|
||||
\ingroup avr_interrupts
|
||||
|
||||
\code#include <arch/signal.h>\endcode
|
||||
|
||||
Introduces an interrupt handler function that runs with global interrupts
|
||||
initially enabled. This allows interrupt handlers to be interrupted. */
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define INTERRUPT(signame) \
|
||||
extern "C" void signame(void); \
|
||||
void signame (void) __attribute__ ((interrupt)); \
|
||||
void signame (void)
|
||||
#else
|
||||
#define INTERRUPT(signame) \
|
||||
void signame (void) __attribute__ ((interrupt)); \
|
||||
void signame (void)
|
||||
#endif
|
||||
|
||||
/** \def EMPTY_INTERRUPT(signame)
|
||||
\ingroup avr_interrupts
|
||||
|
||||
\code#include <arch/signal.h>\endcode
|
||||
|
||||
Defines an empty interrupt handler function. This will not generate
|
||||
any prolog or epilog code and will only return from the ISR. Do not
|
||||
define a function body as this will define it for you.
|
||||
Example:
|
||||
\code EMPTY_INTERRUPT(SIG_ADC);\endcode */
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define EMPTY_INTERRUPT(signame) \
|
||||
extern "C" void signame(void); \
|
||||
void signame (void) __attribute__ ((naked)); \
|
||||
void signame (void) { __asm__ __volatile__ ("reti" ::); }
|
||||
#else
|
||||
#define EMPTY_INTERRUPT(signame) \
|
||||
void signame (void) __attribute__ ((naked)); \
|
||||
void signame (void) { __asm__ __volatile__ ("reti" ::); }
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/*@}*/
|
||||
|
||||
#endif /* _AVR_SIGNAL_H_ */
|
||||
24
targets/wasm-tacle/parallel/PapaBench/conf/Makefile.local.in
Normal file
24
targets/wasm-tacle/parallel/PapaBench/conf/Makefile.local.in
Normal file
@ -0,0 +1,24 @@
|
||||
|
||||
# Architecture selection
|
||||
# Uncomment the line matching your architecture and your machine
|
||||
# Targets marked -alone- does not embed the CRT0 and minimal libc
|
||||
# and the matching .conf file may be tuned.
|
||||
|
||||
#CONF=powerpc-elf-alone
|
||||
#CONF=powerpc-linux-gnu
|
||||
#CONF=arm-elf
|
||||
#CONF=arm-elf-alone
|
||||
CONF=tricore-alone
|
||||
|
||||
# Tools
|
||||
CC = @WCCBIN@
|
||||
LD = @WCCBIN@
|
||||
SIZE = $(ARCH)size
|
||||
OBJCOPY = $(ARCH)objcopy
|
||||
OBJDUMP = $(ARCH)objdump -d
|
||||
|
||||
|
||||
# Other configuration
|
||||
USER_CFLAGS = -O0 -I @abs_srcdir@/../../PapaBench/avr/include/avr
|
||||
USER_LDFLAGS =
|
||||
|
||||
185
targets/wasm-tacle/parallel/PapaBench/conf/Makefile.std
Normal file
185
targets/wasm-tacle/parallel/PapaBench/conf/Makefile.std
Normal file
@ -0,0 +1,185 @@
|
||||
#
|
||||
# $Id: Makefile.std,v 1.3 2011-01-21 11:52:33 moellmer Exp $
|
||||
# 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
|
||||
#
|
||||
|
||||
# Configuration
|
||||
PAPARAZZI_DIR = \"$(BASE)/\"
|
||||
ATMELBIN = /usr/bin
|
||||
ATMEL_INCLUDES = -I $(BASE)/avr/include/
|
||||
UISPBIN=/usr/bin
|
||||
PROG_PORT = /dev/parport0
|
||||
USB_PROG_PORT = /dev/ttyUSB1
|
||||
|
||||
|
||||
# Architecture dependent
|
||||
include $(BASE)/conf/Makefile.local
|
||||
-include $(BASE)/conf/$(CONF).conf
|
||||
|
||||
SERIAL_FLAGS = \
|
||||
-dprog=avr910 \
|
||||
-dpart=auto \
|
||||
-dserial=/dev/ttyS0 \
|
||||
-dspeed=38400
|
||||
|
||||
ISP_FLAGS = \
|
||||
-dlpt=$(PROG_PORT) -dprog=stk200 -v=3
|
||||
|
||||
USB_FLAGS =
|
||||
-dserial=$(USB_PROG_PORT) \
|
||||
-dprog=avr910 \
|
||||
-dpart=auto \
|
||||
-v=3
|
||||
|
||||
|
||||
UISP = uisp
|
||||
UISP_FLAGS = $(ISP_FLAGS)
|
||||
#UISP_FLAGS = $(SERIAL_FLAGS)
|
||||
|
||||
|
||||
#
|
||||
# End of configuration part.
|
||||
#
|
||||
|
||||
|
||||
CFLAGS = \
|
||||
-Wall \
|
||||
$(ATMEL_INCLUDES) \
|
||||
$(INCLUDES) \
|
||||
$(LOCAL_CFLAGS) \
|
||||
-O2 \
|
||||
$(ARCH_CFLAGS) \
|
||||
$(USER_CFLAGS)
|
||||
ifdef PAPABENCH_SINGLE
|
||||
CFLAGS += -DPAPABENCH_SINGLE
|
||||
endif
|
||||
|
||||
|
||||
LDFLAGS = \
|
||||
$(ARCH_STDFLAGS) \
|
||||
$(ARCH_LDFLAGS) \
|
||||
$(USER_LDLFLAGS)
|
||||
|
||||
#
|
||||
# General rules
|
||||
#
|
||||
|
||||
all: \
|
||||
$(TARGET).elf $(TARGET).dis
|
||||
|
||||
compile: $(TARGET).elf \
|
||||
|
||||
load upload: check_fuses \
|
||||
$(TARGET).install
|
||||
|
||||
$(TARGET).objs = \
|
||||
$($(TARGET).srcs:.c=.o)
|
||||
|
||||
#$(TARGET).elf: $($(TARGET).objs)
|
||||
$(TARGET).elf:
|
||||
$(CC) $(CFLAGS) $($(TARGET).srcs) -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)
|
||||
|
||||
TMPFILE = 'check_fuses.tmp'
|
||||
|
||||
check_fuses: check_arch
|
||||
@echo "##### Check of fuses #####"
|
||||
@$(UISP) $(ISP_FLAGS) --rd_fuses >$(TMPFILE)
|
||||
@if (grep -ri 'Fuse Low Byte' $(TMPFILE) | cut -c24- | grep -iq $(LOW_FUSE)) && (grep -ri 'Fuse High Byte' $(TMPFILE) |cut -c24- | grep -iq $(HIGH_FUSE)) && (grep -ri 'Fuse Extended Byte' $(TMPFILE) |cut -c24- | grep -iq $(EXT_FUSE)) && (grep -ri 'Lock Bits' $(TMPFILE) |cut -c24- | grep -iq $(LOCK_FUSE)); then echo "-> Fuses are Ok"; rm $(TMPFILE); else echo "-> Wrong fuses. Type 'make wr_fuses'"; rm $(TMPFILE); exit 1; fi
|
||||
|
||||
ifdef PAPABENCH_NOLINK
|
||||
%.elf:
|
||||
else
|
||||
%.elf:
|
||||
$(LD) \
|
||||
$(LOCAL_LDFLAGS) \
|
||||
$($(@:.elf=).objs) \
|
||||
-o $@ \
|
||||
$(LDFLAGS) \
|
||||
-static
|
||||
$(SIZE) $@
|
||||
endif
|
||||
|
||||
%.s: %.c
|
||||
$(CC) $(CFLAGS) -S $<
|
||||
|
||||
%.o: %.s
|
||||
$(CC) $(CFLAGS) -c $<
|
||||
|
||||
%.hex: %.elf
|
||||
$(OBJCOPY) -O ihex -R .eeprom $< $@
|
||||
|
||||
%.dis: %.elf
|
||||
$(OBJDUMP) $< > $@
|
||||
|
||||
|
||||
%.install: %.hex check_arch
|
||||
# stk200 needs to be erased first
|
||||
$(UISP) $(UISP_FLAGS) --erase
|
||||
$(UISP) $(UISP_FLAGS) --upload if=$<
|
||||
|
||||
verify: $(TARGET).hex
|
||||
$(UISP) $(UISP_FLAGS) --verify if=$<
|
||||
|
||||
erase: check_arch
|
||||
$(UISP) $(ISP_FLAGS) --erase
|
||||
|
||||
check_arch :
|
||||
@echo "##### Check architecture #####"
|
||||
@if ($(UISP) $(UISP_FLAGS) 2>&1 | tr '[:upper:]' '[:lower:]' | grep $(ARCH)); then : ; else echo "-> Wrong architecture (mcu0 vs mcu1 ?)"; exit 1; fi
|
||||
|
||||
avr_clean:
|
||||
rm -f .depend $($(TARGET).srcs:.c=.s)
|
||||
rm -f $(TARGET).elf $(TARGET).elf.s $($(TARGET).srcs:.c=.o)
|
||||
|
||||
|
||||
#
|
||||
# Dependencies
|
||||
#
|
||||
|
||||
.depend:
|
||||
$(CC) $(CFLAGS) $($(TARGET).srcs) > .depend
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
ifneq ($(MAKECMDGOALS),erase)
|
||||
-include .depend
|
||||
endif
|
||||
endif
|
||||
|
||||
|
||||
@ -0,0 +1,14 @@
|
||||
ARCH = arm-elf-
|
||||
MACH =
|
||||
STACK = 0x80000000
|
||||
|
||||
ARCH_CRT0=\
|
||||
$(BASE)/sw/lib/crt0/arm-elf-crt0.c
|
||||
|
||||
ARCH_SOURCES=
|
||||
|
||||
ARCH_CFLAGS=\
|
||||
-DSTACK=\"$(STACK)\"
|
||||
|
||||
ARCH_LDFLAGS=\
|
||||
-nostartfiles
|
||||
2
targets/wasm-tacle/parallel/PapaBench/conf/arm-elf.conf
Normal file
2
targets/wasm-tacle/parallel/PapaBench/conf/arm-elf.conf
Normal file
@ -0,0 +1,2 @@
|
||||
ARCH = arm-elf-
|
||||
MACH =
|
||||
@ -0,0 +1,15 @@
|
||||
ARCH = powerpc-elf-
|
||||
MACH =
|
||||
STACK = 0x80000000
|
||||
|
||||
ARCH_CRT0=\
|
||||
$(BASE)/sw/lib/crt0/powerpc-elf-crt0.c
|
||||
|
||||
ARCH_SOURCES=
|
||||
|
||||
ARCH_CFLAGS=\
|
||||
-DSTACK=\"$(STACK)\"
|
||||
|
||||
ARCH_LDFLAGS=\
|
||||
-nostdlib \
|
||||
-nostartfiles
|
||||
@ -0,0 +1,2 @@
|
||||
ARCH = powerpc-linux-gnu-
|
||||
MACH =
|
||||
@ -0,0 +1,14 @@
|
||||
ARCH = tricore-
|
||||
MACH =
|
||||
STACK = 0xd0003ffc
|
||||
|
||||
#ARCH_CRT0=\
|
||||
#$(BASE)/sw/lib/crt0/arm-elf-crt0.c
|
||||
|
||||
ARCH_SOURCES=
|
||||
|
||||
ARCH_CFLAGS=\
|
||||
-DSTACK=\"$(STACK)\"
|
||||
|
||||
ARCH_LDFLAGS=\
|
||||
#-nostartfiles
|
||||
@ -0,0 +1,120 @@
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "ad7714.h"
|
||||
#include "spi.h"
|
||||
|
||||
/* unipolar, 16 bits, current boost, filter 4000 */
|
||||
//#define AD_HIGH_FILTER_CFG 0xAF
|
||||
//#define AD_LOW_FILTER_CFG 0xA0
|
||||
|
||||
/* filter 2000 */
|
||||
#define AD_HIGH_FILTER_CFG 0xA7
|
||||
#define AD_LOW_FILTER_CFG 0xD0
|
||||
|
||||
/* filter 19 */
|
||||
//const uint8_t AD_HIGH_FILTER_CFG = 0xA0;
|
||||
//const uint8_t AD_LOW_FILTER_CFG = 0x13;
|
||||
|
||||
struct AdMsg {
|
||||
uint8_t len;
|
||||
const uint8_t *data;
|
||||
};
|
||||
|
||||
#define AD7714_SETUP_LEN 6
|
||||
const uint8_t ad7714_setup_data[ AD7714_SETUP_LEN ] = {
|
||||
AD_F_HIGH_REG + AD_FD0, /* select high filter register */
|
||||
AD_HIGH_FILTER_CFG,
|
||||
AD_F_LOW_REG + AD_FD0, /* select low filter register */
|
||||
AD_LOW_FILTER_CFG,
|
||||
AD_MODE_REG + AD_FD0, /* select com register */
|
||||
AD_SELFCAL_MOD + AD_GAIN_128,
|
||||
};
|
||||
const struct AdMsg ad7714_setup = {AD7714_SETUP_LEN, ad7714_setup_data};
|
||||
|
||||
#define AD7714_READ_LEN 3
|
||||
const uint8_t ad7714_read_data[ ] = {
|
||||
AD_DATA_REG + AD_WR + AD_FD0, /* transmit read request */
|
||||
0x00, /* transmit a dumb value just to get the result */
|
||||
0xAA /* transmit a dumb value just to get the result */
|
||||
};
|
||||
const struct AdMsg ad7714_read = {AD7714_READ_LEN, ad7714_read_data};
|
||||
|
||||
#define AD7714_SELECT_CHANNEL_LEN 2
|
||||
const uint8_t ad7714_select_channel_data[ AD7714_SELECT_CHANNEL_LEN ] = {
|
||||
AD_MODE_REG + AD_FD0,
|
||||
// AD_BG_CAL_MOD + AD_GAIN_128
|
||||
AD_NOR_MOD + AD_GAIN_128
|
||||
};
|
||||
const struct AdMsg ad7714_select_channel = {AD7714_SELECT_CHANNEL_LEN, ad7714_select_channel_data};
|
||||
|
||||
|
||||
static struct AdMsg *msg;
|
||||
static uint8_t idx;
|
||||
uint16_t ad7714_sample;
|
||||
uint8_t ad7714_sample_read;
|
||||
|
||||
|
||||
void ad7714_start_transmitting ( const struct AdMsg *amsg )
|
||||
{
|
||||
/* Enable SPI, Master, MSB first, clock idle high, sample on trailing edge, clock rate fck/128 */
|
||||
SPI_START( _BV( SPE ) | _BV( MSTR ) | _BV( SPR1 ) | _BV( CPOL ) | _BV(
|
||||
CPHA ) | _BV( SPR0 ) ); //| _BV(SPR0)
|
||||
SPI_SELECT_SLAVE1();
|
||||
msg = ( struct AdMsg * )amsg;
|
||||
SPDR = msg->data[ 0 ];
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
void ad7714_on_spi_it( void )
|
||||
{
|
||||
uint8_t spi_read = SPDR;
|
||||
if ( msg == &ad7714_read ) {
|
||||
if ( idx == 1 )
|
||||
ad7714_sample = spi_read << 8;
|
||||
else
|
||||
if ( idx == 2 ) {
|
||||
ad7714_sample += spi_read;
|
||||
ad7714_sample_read = TRUE;
|
||||
}
|
||||
}
|
||||
idx++;
|
||||
if ( idx < msg->len )
|
||||
SPI_SEND( msg->data[ idx ] );
|
||||
else {
|
||||
SPI_UNSELECT_SLAVE1();
|
||||
SPI_STOP();
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t ad7714_status = 0;
|
||||
void ad7714_on_it( void )
|
||||
{
|
||||
if ( ad7714_status == 0 )
|
||||
ad7714_start_transmitting( &ad7714_setup );
|
||||
else
|
||||
if ( ad7714_status == 1 )
|
||||
ad7714_start_transmitting( &ad7714_select_channel );
|
||||
else
|
||||
ad7714_start_transmitting( &ad7714_read );
|
||||
ad7714_status++;
|
||||
}
|
||||
|
||||
void ad7714_init( void )
|
||||
{
|
||||
/* setupt interrupt on falling edge */
|
||||
cbi( EICRB, ISC60 );
|
||||
sbi( EICRB, ISC61 );
|
||||
/* clear interrupt flag */
|
||||
// if (bit_is_set(EIFR, INTF6))
|
||||
// EIFR != _BV(INTF6);
|
||||
Ad7714_Enable_It();
|
||||
}
|
||||
|
||||
|
||||
SIGNAL( SIG_INTERRUPT6 )
|
||||
{
|
||||
ad7714_on_it();
|
||||
}
|
||||
@ -0,0 +1,56 @@
|
||||
#ifndef AD_7714_H
|
||||
#define AD_7714_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
/* AD7714 COM register */
|
||||
#define AD_COM_REG (0<<4)
|
||||
#define AD_MODE_REG (1<<4)
|
||||
#define AD_F_HIGH_REG (2<<4)
|
||||
#define AD_F_LOW_REG (3<<4)
|
||||
#define AD_TEST_REG (4<<4)
|
||||
#define AD_DATA_REG (5<<4)
|
||||
#define AD_ZS_CAL_REG (6<<4)
|
||||
#define AD_FS_CAL_REG (7<<4)
|
||||
|
||||
#define AD_WR (1<<3)
|
||||
|
||||
#define AD_FD0 (4<<0)
|
||||
#define AD_FD1 (5<<0)
|
||||
#define AD_FD2 (6<<0)
|
||||
|
||||
/* AD7714 MODE register */
|
||||
#define AD_NOR_MOD (0<<5)
|
||||
#define AD_SELFCAL_MOD (1<<5)
|
||||
#define AD_ZS_SYSCAL_MOD (2<<5)
|
||||
#define AD_FS_SYSCAL_MOD (3<<5)
|
||||
#define AD_SYSOFFCAL_MOD (4<<5)
|
||||
#define AD_BG_CAL_MOD (5<<5)
|
||||
#define AD_ZS_SELFCAL_MOD (6<<5)
|
||||
#define AD_FS_SELFCAL_MOD (7<<5)
|
||||
|
||||
#define AD_GAIN_1 (0<<2)
|
||||
#define AD_GAIN_2 (1<<2)
|
||||
#define AD_GAIN_4 (2<<2)
|
||||
#define AD_GAIN_8 (3<<2)
|
||||
#define AD_GAIN_16 (4<<2)
|
||||
#define AD_GAIN_32 (5<<2)
|
||||
#define AD_GAIN_64 (6<<2)
|
||||
#define AD_GAIN_128 (7<<2)
|
||||
|
||||
#define AD_BO (1<<1)
|
||||
#define AD_FSYNC (1<<0)
|
||||
|
||||
#define Ad7714_Disable_It() { cbi(EIMSK, INT6); }
|
||||
|
||||
#define Ad7714_Enable_It() { sbi(EIMSK, INT6); }
|
||||
|
||||
#define Ad7714_Ready() (bit_is_clear(EIFR, INTF6))
|
||||
|
||||
extern uint16_t ad7714_sample;
|
||||
extern uint8_t ad7714_sample_read;
|
||||
void ad7714_init( void );
|
||||
void ad7714_on_spi_it( void );
|
||||
void ad7714_on_it( void );
|
||||
|
||||
#endif /* AD_7714_H */
|
||||
@ -0,0 +1,130 @@
|
||||
/*
|
||||
Paparazzi mcu0 adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
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 <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
#include "airframe.h"
|
||||
#include "std.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 ?) */
|
||||
_Pragma( "loopbound min 8 max 8" )
|
||||
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 );
|
||||
|
||||
}
|
||||
@ -0,0 +1,53 @@
|
||||
/*
|
||||
Paparazzi mcu0 adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
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 <inttypes.h>
|
||||
|
||||
|
||||
#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
|
||||
@ -0,0 +1,123 @@
|
||||
/*
|
||||
$Id: autopilot.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
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 LATERAL_MODE_MANUAL 0
|
||||
#define LATERAL_MODE_ROLL_RATE 1
|
||||
#define LATERAL_MODE_ROLL 2
|
||||
#define LATERAL_MODE_COURSE 3
|
||||
#define LATERAL_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 bool_t auto_pitch;
|
||||
extern uint8_t lateral_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 );
|
||||
/*receive_gps_data_task */
|
||||
void send_gps_pos( void );
|
||||
void send_radIR( void );
|
||||
void send_takeOff( void );
|
||||
/*end receive_gps_data_task*/
|
||||
void stabilisation_task( void );
|
||||
#endif /* AUTOPILOT_H */
|
||||
@ -0,0 +1,35 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: downlink.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
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 */
|
||||
@ -0,0 +1,202 @@
|
||||
/*
|
||||
Paparazzi autopilot $Id: estimator.c,v 1.2 2011-01-21 11:52:44 moellmer Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <math.h>
|
||||
|
||||
#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 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;
|
||||
float absphi;
|
||||
float init_ir2;
|
||||
|
||||
if ( initialized ) {
|
||||
float dt = gps_ftow - last_t;
|
||||
if ( dt > 0.1 ) { // Against division by zero
|
||||
float phi = ( estimator_hspeed_dir - last_hspeed_dir );
|
||||
//NORM_RAD_ANGLE(phi);
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi > M_PI ) phi -= 2 * M_PI;
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi < -M_PI ) phi += 2 * M_PI;
|
||||
phi = phi / dt * NOMINAL_AIRSPEED / g; /* tan linearized */
|
||||
//NORM_RAD_ANGLE(phi);
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi > M_PI ) phi -= 2 * M_PI;
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi < -M_PI ) phi += 2 * M_PI;
|
||||
estimator_ir = ( float )ir_roll;
|
||||
estimator_rad = phi;
|
||||
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;
|
||||
#if defined IR_RAD_OF_IR_MIN_VALUE & defined IR_RAD_OF_IR_MAX_VALUE
|
||||
float result = sum_xy / sum_xx;
|
||||
if ( result < IR_RAD_OF_IR_MIN_VALUE )
|
||||
estimator_rad_of_ir = IR_RAD_OF_IR_MIN_VALUE;
|
||||
else
|
||||
if ( result > IR_RAD_OF_IR_MAX_VALUE )
|
||||
estimator_rad_of_ir = IR_RAD_OF_IR_MAX_VALUE;
|
||||
else
|
||||
estimator_rad_of_ir = result;
|
||||
#else
|
||||
estimator_rad_of_ir = sum_xy / sum_xx;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
} else {
|
||||
initialized = TRUE;
|
||||
init_ir2 = ir_contrast;
|
||||
init_ir2 = init_ir2 * init_ir2;
|
||||
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 )
|
||||
{
|
||||
|
||||
}
|
||||
@ -0,0 +1,67 @@
|
||||
/*
|
||||
$Id: estimator.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
|
||||
/* 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 */
|
||||
@ -0,0 +1,58 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: gps.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
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 */
|
||||
@ -0,0 +1,318 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: gps_sirf.c,v 1.2 2011-01-25 09:40:36 plazar Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <string.h>
|
||||
#include "math_papabench.h"
|
||||
|
||||
#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 )
|
||||
{
|
||||
/*#ifdef WITH_SWITCH
|
||||
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
|
||||
}
|
||||
#else*/
|
||||
if ( sirf_status == UNINIT ) {
|
||||
if ( c == SIRF_START1 )
|
||||
sirf_status++;
|
||||
#ifdef SIMUL
|
||||
if ( c == IR_START )
|
||||
sirf_status = GOT_IR_START;
|
||||
#endif
|
||||
} else
|
||||
if ( sirf_status == GOT_START1 ) {
|
||||
if ( c != SIRF_START2 )
|
||||
goto error;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_START2 ) {
|
||||
sirf_len = ( c << 8 ) & 0xFF00;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_LEN1 ) {
|
||||
sirf_len += ( c & 0x00FF );
|
||||
if ( sirf_len > SIRF_MAX_PAYLOAD )
|
||||
goto error;
|
||||
sirf_msg_idx = 0;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == 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++;
|
||||
} else
|
||||
if ( sirf_status == GOT_PAYLOAD ) {
|
||||
sirf_checksum = ( c << 8 ) & 0xFF00;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_CHECKSUM1 ) {
|
||||
sirf_checksum += ( c & 0x00FF );
|
||||
/* fixme: check correct */
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_CHECKSUM2 ) {
|
||||
if ( c != SIRF_END1 )
|
||||
goto error;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_END1 ) {
|
||||
if ( c != SIRF_END2 )
|
||||
goto error;
|
||||
|
||||
if ( sirf_type == SIRF_TYP_EXT_NAV )
|
||||
gps_msg_received = TRUE;
|
||||
goto restart;
|
||||
}
|
||||
#ifdef SIMUL
|
||||
else
|
||||
if ( sirf_status == GOT_IR_START ) {
|
||||
simul_ir_roll = c << 8;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_IR1 ) {
|
||||
simul_ir_roll |= c;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_IR2 ) {
|
||||
simul_ir_pitch = c << 8;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_IR3 ) {
|
||||
simul_ir_pitch |= c;
|
||||
goto restart;
|
||||
}
|
||||
#endif
|
||||
else {}
|
||||
//#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
|
||||
@ -0,0 +1,265 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: gps_ubx.c,v 1.4 2011-01-25 09:40:36 plazar Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
//#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#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;
|
||||
// was 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 void parse_ubx( uint8_t c )
|
||||
{
|
||||
if ( ubx_status < GOT_PAYLOAD ) {
|
||||
ck_a += c;
|
||||
ck_b += ck_a;
|
||||
}
|
||||
/*#ifdef WITH_SWITCH
|
||||
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;
|
||||
}
|
||||
#else */
|
||||
if ( ubx_status == UNINIT ) {
|
||||
if ( c == UBX_SYNC1 )
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_SYNC1 ) {
|
||||
if ( c != UBX_SYNC2 )
|
||||
goto error;
|
||||
ck_a = 0;
|
||||
ck_b = 0;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == 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++;
|
||||
} else
|
||||
if ( ubx_status == GOT_CLASS ) {
|
||||
ubx_id = c;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_ID ) {
|
||||
ubx_len = c;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_LEN1 ) {
|
||||
ubx_len |= ( c << 8 );
|
||||
if ( ubx_len > UBX_MAX_PAYLOAD )
|
||||
goto error;
|
||||
ubx_msg_idx = 0;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_LEN2 ) {
|
||||
ubx_msg_buf[ ubx_msg_idx ] = c;
|
||||
ubx_msg_idx++;
|
||||
if ( ubx_msg_idx >= ubx_len )
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_PAYLOAD ) {
|
||||
if ( c != ck_a )
|
||||
goto error;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_CHECKSUM1 ) {
|
||||
if ( c != ck_b )
|
||||
goto error;
|
||||
gps_msg_received = TRUE;
|
||||
goto restart;
|
||||
} else {}
|
||||
//#endif
|
||||
return;
|
||||
error:
|
||||
restart:
|
||||
ubx_status = UNINIT;
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef SIMUL
|
||||
ReceiveUart0( parse_ubx )
|
||||
#else
|
||||
ReceiveUart1( parse_ubx )
|
||||
#endif
|
||||
@ -0,0 +1,94 @@
|
||||
/*
|
||||
$Id: if_calib.c,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
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 <inttypes.h>
|
||||
#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);
|
||||
}
|
||||
***/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,22 @@
|
||||
#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
|
||||
|
||||
#ifdef ANTON_T7
|
||||
#define IF_CALIB_MODE_OF_PULSE(pprz) (pprz < TRESHOLD1 ? IF_CALIB_MODE_UP : \
|
||||
IF_CALIB_MODE_NONE)
|
||||
#else
|
||||
#define IF_CALIB_MODE_OF_PULSE(pprz) (pprz < TRESHOLD1 ? IF_CALIB_MODE_UP : \
|
||||
(pprz < TRESHOLD2 ? IF_CALIB_MODE_NONE : \
|
||||
IF_CALIB_MODE_DOWN))
|
||||
#endif /* ANTON_T7 */
|
||||
|
||||
#endif // IF_CALIB_H
|
||||
@ -0,0 +1,76 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: infrared.c,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
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;
|
||||
float ir_rad_of_ir = 0.00375;
|
||||
|
||||
|
||||
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 );
|
||||
}
|
||||
@ -0,0 +1,42 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: infrared.h,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
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
|
||||
|
||||
|
||||
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 */
|
||||
@ -0,0 +1,125 @@
|
||||
/*
|
||||
$Id: link_fbw.c,v 1.3 2011-01-25 09:40:37 plazar Exp $
|
||||
|
||||
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 <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
#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 _Pragma( "entrypoint" ) 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;
|
||||
}
|
||||
@ -0,0 +1,44 @@
|
||||
/*
|
||||
$Id: link_fbw.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
|
||||
#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 */
|
||||
@ -0,0 +1,677 @@
|
||||
/*
|
||||
$Id: main.c,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
This file is part of paparazzi.
|
||||
|
||||
paparazzi is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation; either version 2, or (at your option)
|
||||
any later version.
|
||||
|
||||
paparazzi is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with paparazzi; see the file COPYING. If not, write to
|
||||
the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
Boston, MA 02111-1307, USA.
|
||||
|
||||
*/
|
||||
/** \file main.c
|
||||
\brief Regroup main functions
|
||||
|
||||
*/
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
|
||||
#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 minimal speed for takeoff in m/s */
|
||||
#define MIN_SPEED_FOR_TAKEOFF 5.
|
||||
|
||||
|
||||
uint8_t fatal_error_nb = 0;
|
||||
static const uint16_t version = 1;
|
||||
|
||||
/** in seconds */
|
||||
static uint16_t cputime = 0;
|
||||
|
||||
uint8_t pprz_mode = PPRZ_MODE_MANUAL;
|
||||
uint8_t vertical_mode = VERTICAL_MODE_MANUAL;
|
||||
uint8_t lateral_mode = LATERAL_MODE_MANUAL;
|
||||
uint8_t ir_estim_mode = IR_ESTIM_MODE_ON;
|
||||
bool_t auto_pitch = FALSE;
|
||||
|
||||
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;
|
||||
|
||||
float slider_1_val, slider_2_val;
|
||||
|
||||
bool_t launch = FALSE;
|
||||
static uint8_t boot = TRUE;
|
||||
static uint8_t count;
|
||||
//#define Min(x, y) (x < y ? x : y)
|
||||
//#define Max(x, y) (x > y ? x : y)
|
||||
|
||||
|
||||
#define NO_CALIB 0 /**< \enum No calibration state */
|
||||
#define WAITING_CALIB_CONTRAST 1 /**< \enum Waiting calibration contrast state */
|
||||
#define CALIB_DONE 2 /**< \enum Calibration done state */
|
||||
|
||||
/** Maximal delay for calibration */
|
||||
#define MAX_DELAY_FOR_CALIBRATION 10
|
||||
|
||||
/** \fn inline void ground_calibrate( void )
|
||||
\brief Calibrate contrast if paparazzi mode is
|
||||
set to auto1 before MAX_DELAY_FOR_CALIBRATION secondes */
|
||||
/**User must put verticaly the uav (nose bottom) and push
|
||||
radio roll stick to get new calibration
|
||||
If not, the default calibration is used.
|
||||
*/
|
||||
inline void ground_calibrate( void )
|
||||
{
|
||||
static uint8_t calib_status = NO_CALIB;
|
||||
/*#ifdef WITH_SWITCH
|
||||
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;
|
||||
}
|
||||
#else*/
|
||||
if ( calib_status == NO_CALIB ) {
|
||||
if ( cputime < MAX_DELAY_FOR_CALIBRATION && pprz_mode == PPRZ_MODE_AUTO1 ) {
|
||||
calib_status = WAITING_CALIB_CONTRAST;
|
||||
DOWNLINK_SEND_CALIB_START();
|
||||
}
|
||||
} else
|
||||
if ( calib_status == 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 );
|
||||
}
|
||||
} else {}
|
||||
|
||||
//#endif
|
||||
}
|
||||
|
||||
|
||||
/** \fn inline uint8_t pprz_mode_update( void )
|
||||
\brief Update paparazzi mode
|
||||
*/
|
||||
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 ) );
|
||||
|
||||
else
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
#ifdef RADIO_LLS
|
||||
/** \fn inline uint8_t ir_estim_mode_update( void )
|
||||
\brief update ir estimation if RADIO_LLS is true \n
|
||||
*/
|
||||
inline uint8_t ir_estim_mode_update( void )
|
||||
{
|
||||
ModeUpdate( ir_estim_mode,
|
||||
IR_ESTIM_MODE_OF_PULSE( from_fbw.channels[ RADIO_LLS ] ) );
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
/** \fn inline uint8_t mcu1_status_update( void )
|
||||
\brief @@@@@ A FIXER @@@@@
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
||||
/** Delay between @@@@@ A FIXER @@@@@ */
|
||||
#define EVENT_DELAY 20
|
||||
|
||||
/** \def EventUpdate(_cpt, _cond, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define EventUpdate(_cpt, _cond, _event) \
|
||||
if (_cond) { \
|
||||
if (_cpt < EVENT_DELAY) { \
|
||||
_cpt++; \
|
||||
if (_cpt == EVENT_DELAY) \
|
||||
_event = TRUE; \
|
||||
} \
|
||||
} else { \
|
||||
_cpt = 0; \
|
||||
_event = FALSE; \
|
||||
}
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define EventPos(_cpt, _channel, _event) \
|
||||
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[ _channel ]>(int)(0.75*MAX_PPRZ)), _event)
|
||||
|
||||
/** \def EventNeg(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define EventNeg(_cpt, _channel, _event) \
|
||||
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[ _channel ]<(int)(-0.75*MAX_PPRZ)), _event)
|
||||
|
||||
/** \fn static inline void events_update( void )
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
static inline void events_update( void )
|
||||
{
|
||||
static uint16_t event1_cpt = 0;
|
||||
static uint16_t event2_cpt = 0;
|
||||
EventPos( event1_cpt, RADIO_GAIN1, rc_event_1 );
|
||||
EventNeg( event2_cpt, RADIO_GAIN1, rc_event_2 );
|
||||
}
|
||||
|
||||
|
||||
/** \fn inline void copy_from_to_fbw ( void )
|
||||
\brief Send back uncontrolled channels (only rudder)
|
||||
*/
|
||||
inline void copy_from_to_fbw ( void )
|
||||
{
|
||||
to_fbw.channels[ RADIO_YAW ] = from_fbw.channels[ RADIO_YAW ];
|
||||
#ifdef ANTON_T7
|
||||
to_fbw.channels[ RADIO_PITCH ] = from_fbw.channels[ RADIO_PITCH ];
|
||||
#endif
|
||||
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 number of message at initialisation */
|
||||
#define INIT_MSG_NB 2
|
||||
/** @@@@@ A FIXER @@@@ */
|
||||
#define HI_FREQ_PHASE_NB 5
|
||||
|
||||
//static char signature[ 16 ] = MESSAGES_MD5SUM;
|
||||
|
||||
/** \def PERIODIC_SEND_BAT()
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_BAT() DOWNLINK_SEND_BAT(&vsupply, &estimator_flight_time, &low_battery, &block_time, &stage_time)
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#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);
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_ATTITUDE() DOWNLINK_SEND_ATTITUDE(&estimator_phi, &estimator_psi, &estimator_theta);
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_ADC() DOWNLINK_SEND_ADC(&ir_roll, &ir_pitch);
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#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);
|
||||
|
||||
#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
|
||||
|
||||
|
||||
/** \fn inline void reporting_task( void )
|
||||
\brief Send a serie of initialisation messages followed by a stream of periodic ones\n
|
||||
Called at 20Hz.
|
||||
*/
|
||||
|
||||
void send_boot( void )
|
||||
{
|
||||
/** initialisation phase during boot */
|
||||
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;
|
||||
}
|
||||
}
|
||||
void send_attitude( void ) //500ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
count++;
|
||||
if ( count == 250 ) count = 0;
|
||||
if ( count % 5 == 0 )
|
||||
PERIODIC_SEND_ATTITUDE();
|
||||
}
|
||||
}
|
||||
|
||||
void send_adc( void ) //500ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 5 == 1 ) PERIODIC_SEND_ADC();
|
||||
}
|
||||
}
|
||||
void send_settings( void ) //500ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 5 == 2 ) PERIODIC_SEND_SETTINGS();
|
||||
}
|
||||
}
|
||||
void send_desired( void ) //1000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 10 == 3 ) PERIODIC_SEND_DESIRED();
|
||||
}
|
||||
}
|
||||
void send_bat( void ) //2000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 20 == 8 ) PERIODIC_SEND_BAT();
|
||||
}
|
||||
}
|
||||
void send_climb( void ) //2000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 20 == 18 ) PERIODIC_SEND_CLIMB_PID();
|
||||
}
|
||||
}
|
||||
void send_mode( void ) //5000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 50 == 9 ) PERIODIC_SEND_PPRZ_MODE();
|
||||
}
|
||||
}
|
||||
void send_debug( void ) //5000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 50 == 29 ) PERIODIC_SEND_DEBUG();
|
||||
}
|
||||
}
|
||||
void send_nav_ref( void ) //10000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 100 == 49 ) PERIODIC_SEND_NAVIGATION_REF();
|
||||
}
|
||||
}
|
||||
|
||||
/** \fn inline uint8_t inflight_calib_mode_update ( void )
|
||||
\brief @@@@@ A FIXER @@@@@
|
||||
*/
|
||||
inline uint8_t inflight_calib_mode_update ( void )
|
||||
{
|
||||
ModeUpdate( inflight_calib_mode,
|
||||
IF_CALIB_MODE_OF_PULSE( from_fbw.channels[ RADIO_CALIB ] ) );
|
||||
}
|
||||
|
||||
|
||||
/** \fn inline void radio_control_task( void )
|
||||
\brief @@@@@ A FIXER @@@@@
|
||||
*/
|
||||
void _Pragma( "entrypoint" ) radio_control_task( void )
|
||||
{
|
||||
bool_t calib_mode_changed;
|
||||
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
|
||||
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 ];
|
||||
#ifdef ANTON_MAGICAL_MISTERY_GAINS
|
||||
roll_pgain = ROLL_PGAIN * ( 1 - 5. / 7. * from_fbw.channels[ RADIO_THROTTLE ] /
|
||||
MAX_PPRZ );
|
||||
pitch_pgain = PITCH_PGAIN * ( 1 - 1. / 3. * from_fbw.channels[ RADIO_THROTTLE ] /
|
||||
MAX_PPRZ );
|
||||
#endif /* ANTON_MAGICAL_MISTERY_GAINS */
|
||||
}
|
||||
// else asynchronously set by climb_pid_run();
|
||||
|
||||
mcu1_ppm_cpt = from_fbw.ppm_cpt;
|
||||
vsupply = from_fbw.vsupply;
|
||||
|
||||
events_update();
|
||||
|
||||
if ( !estimator_flight_time ) {
|
||||
ground_calibrate();
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 &&
|
||||
from_fbw.channels[ RADIO_THROTTLE ] > GAZ_THRESHOLD_TAKEOFF )
|
||||
launch = TRUE;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/** \fn void navigation_task( void )
|
||||
\brief Compute desired_course
|
||||
*/
|
||||
void navigation_update( void )
|
||||
{
|
||||
|
||||
/* Default to keep compatibility with previous behaviour */
|
||||
lateral_mode = LATERAL_MODE_COURSE;
|
||||
if ( pprz_mode == PPRZ_MODE_HOME )
|
||||
nav_home();
|
||||
else
|
||||
nav_update();
|
||||
}
|
||||
|
||||
void send_nav_values( void )
|
||||
{
|
||||
|
||||
DOWNLINK_SEND_NAVIGATION( &nav_block, &nav_stage, &estimator_x, &estimator_y,
|
||||
&desired_course, &dist2_to_wp, &course_pgain, &dist2_to_home );
|
||||
}
|
||||
|
||||
void course_run( void )
|
||||
{
|
||||
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
|
||||
if ( lateral_mode >= LATERAL_MODE_COURSE )
|
||||
course_pid_run(); /* aka compute nav_desired_roll */
|
||||
desired_roll = nav_desired_roll;
|
||||
}
|
||||
}
|
||||
|
||||
void _Pragma( "entrypoint" ) altitude_control_task( void )
|
||||
{
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
|
||||
if ( vertical_mode == VERTICAL_MODE_AUTO_ALT )
|
||||
altitude_pid_run();
|
||||
}
|
||||
}
|
||||
void _Pragma( "entrypoint" ) climb_control_task( void )
|
||||
{
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
|
||||
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.)
|
||||
|
||||
/** Maximum time allowed for low battery level */
|
||||
#define LOW_BATTERY_DELAY 5
|
||||
|
||||
/** \fn inline void periodic_task( void )
|
||||
\brief Do periodic tasks at 61 Hz
|
||||
*/
|
||||
/**There are four @@@@@ boucles @@@@@:
|
||||
- 20 Hz:
|
||||
- lets use \a reporting_task at 10 Hz
|
||||
- updates ir with \a ir_update
|
||||
- updates estimator of ir with \a estimator_update_state_infrared
|
||||
- set \a desired_aileron and \a desired_elevator with \a roll_pitch_pid_run
|
||||
- sends to \a fbw \a desired_gaz, \a desired_aileron and
|
||||
\a desired_elevator \note \a desired_gaz is set upon GPS
|
||||
message reception
|
||||
- 10 Hz: to get a \a stage_time_ds
|
||||
- 4 Hz:
|
||||
- calls \a estimator_propagate_state
|
||||
- do navigation with \a navigation_task
|
||||
|
||||
*/
|
||||
|
||||
#ifdef PAPABENCH_SINGLE
|
||||
uint8_t _20Hz = 0;
|
||||
uint8_t _1Hz = 0;
|
||||
#else
|
||||
static uint8_t _20Hz = 0;
|
||||
static uint8_t _1Hz = 0;
|
||||
#endif
|
||||
void periodic_task( void )
|
||||
{
|
||||
static uint8_t _10Hz = 0;
|
||||
static uint8_t _4Hz = 0;
|
||||
static uint8_t t = 0;
|
||||
|
||||
estimator_t += PERIOD;
|
||||
|
||||
_20Hz++;
|
||||
if ( _20Hz >= 3 ) _20Hz = 0;
|
||||
_10Hz++;
|
||||
if ( _10Hz >= 6 ) _10Hz = 0;
|
||||
_4Hz++;
|
||||
if ( _4Hz >= 15 ) _4Hz = 0;
|
||||
_1Hz++;
|
||||
if ( _1Hz >= 61 ) _1Hz = 0;
|
||||
|
||||
if ( !_10Hz )
|
||||
stage_time_ds = stage_time_ds + .1;
|
||||
if ( !_1Hz ) {
|
||||
if ( estimator_flight_time ) estimator_flight_time++;
|
||||
cputime++;
|
||||
stage_time_ds = ( int16_t )( stage_time_ds + .5 );
|
||||
stage_time++;
|
||||
block_time++;
|
||||
|
||||
if ( vsupply < LOW_BATTERY ) t++;
|
||||
else t = 0;
|
||||
low_battery |= ( t >= LOW_BATTERY_DELAY );
|
||||
}
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch(_4Hz) {
|
||||
case 0:
|
||||
estimator_propagate_state();
|
||||
navigation_task();
|
||||
altitude_control_task();//added 4-05-06
|
||||
climb_control_task();//added 04-05-06
|
||||
break;
|
||||
// default:
|
||||
}
|
||||
switch (_20Hz) {
|
||||
case 0:
|
||||
break;
|
||||
case 1: {
|
||||
static uint8_t odd;
|
||||
odd++;
|
||||
if (odd & 0x01)
|
||||
reporting_task();
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
stabilisation_task();
|
||||
link_fbw_send();
|
||||
break;
|
||||
default:
|
||||
fatal_error_nb++;
|
||||
}
|
||||
#else */
|
||||
if ( _4Hz == 0 ) {
|
||||
estimator_propagate_state();
|
||||
/*navigation_task */
|
||||
navigation_update();
|
||||
send_nav_values();
|
||||
course_run();
|
||||
/*end navigation*/
|
||||
altitude_control_task();
|
||||
climb_control_task();
|
||||
}
|
||||
|
||||
if ( _20Hz == 0 )
|
||||
{}
|
||||
else
|
||||
if ( _20Hz == 1 ) {
|
||||
static uint8_t odd;
|
||||
odd++;
|
||||
if ( odd & 0x01 ) {
|
||||
/*reporting_task()*/
|
||||
send_boot();
|
||||
send_attitude();
|
||||
send_adc();
|
||||
send_settings();
|
||||
send_desired();
|
||||
send_bat();
|
||||
send_climb();
|
||||
send_mode();
|
||||
send_debug();
|
||||
send_nav_ref();
|
||||
}
|
||||
} else
|
||||
if ( _20Hz == 2 ) {
|
||||
stabilisation_task();
|
||||
link_fbw_send();
|
||||
} else
|
||||
fatal_error_nb++;
|
||||
//#endif
|
||||
}
|
||||
|
||||
void _Pragma( "entrypoint" ) stabilisation_task( void )
|
||||
{
|
||||
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;
|
||||
#ifndef ANTON_T7
|
||||
to_fbw.channels[ RADIO_PITCH ] = desired_elevator;
|
||||
#endif
|
||||
|
||||
// Code for camera stabilization, FIXME put that elsewhere
|
||||
to_fbw.channels[ RADIO_GAIN1 ] = TRIM_PPRZ( MAX_PPRZ / 0.75 *
|
||||
( -estimator_phi ) );
|
||||
}
|
||||
|
||||
/*void receive_gps_data_task(void)
|
||||
{
|
||||
parse_gps_msg();
|
||||
gps_msg_received = FALSE;
|
||||
if (gps_pos_available)
|
||||
{
|
||||
use_gps_pos();
|
||||
gps_pos_available = FALSE;
|
||||
}
|
||||
}*/
|
||||
/** \fn void use_gps_pos( void )
|
||||
\brief use GPS
|
||||
*/
|
||||
/**Send by downlink the GPS and rad_of_ir messages with \a DOWNLINK_SEND_GPS
|
||||
and \a DOWNLINK_SEND_RAD_OF_IR \n
|
||||
If \a estimator_flight_time is null and \a estimator_hspeed_mod is greater
|
||||
than \a MIN_SPEED_FOR_TAKEOFF, set the \a estimator_flight_time to 1 and \a
|
||||
launch to true (which is not set in non auto launch. Then call
|
||||
\a DOWNLINK_SEND_TAKEOFF
|
||||
*/
|
||||
void send_gps_pos( void )
|
||||
{
|
||||
gps_msg_received = FALSE;
|
||||
if ( gps_pos_available ) {
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
void send_radIR( void )
|
||||
{
|
||||
if ( gps_pos_available )
|
||||
DOWNLINK_SEND_RAD_OF_IR( &estimator_ir, &estimator_rad, &estimator_rad_of_ir,
|
||||
&ir_roll_neutral, &ir_pitch_neutral );
|
||||
}
|
||||
|
||||
void send_takeOff( void )
|
||||
{
|
||||
if ( gps_pos_available ) {
|
||||
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 );
|
||||
}
|
||||
gps_pos_available = FALSE;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,101 @@
|
||||
/*
|
||||
$Id: mainloop.c,v 1.3 2011-01-25 09:40:37 plazar Exp $
|
||||
|
||||
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 <arch/interrupt.h>
|
||||
#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"
|
||||
|
||||
#ifndef PAPABENCH_SINGLE
|
||||
void fbw_init( void );
|
||||
void fbw_schedule( void );
|
||||
#endif
|
||||
|
||||
int main( void )
|
||||
{
|
||||
uint8_t init_cpt;
|
||||
/* 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();
|
||||
# ifdef PAPABENCH_SINGLE
|
||||
fbw_init();
|
||||
# endif
|
||||
|
||||
/* start interrupt task */
|
||||
//sei(); /*Fadia*/
|
||||
|
||||
/* Wait 0.5s (for modem init ?) */
|
||||
init_cpt = 30;
|
||||
_Pragma( "loopbound min 31 max 31" )
|
||||
while ( init_cpt ) {
|
||||
if ( timer_periodic() )
|
||||
init_cpt--;
|
||||
}
|
||||
|
||||
/* enter mainloop */
|
||||
#ifndef NO_MAINLOOP
|
||||
while ( 1 ) {
|
||||
#endif
|
||||
if ( timer_periodic() ) {
|
||||
periodic_task();
|
||||
# if PAPABENCH_SINGLE
|
||||
fbw_schedule();
|
||||
# endif
|
||||
}
|
||||
if ( gps_msg_received ) {
|
||||
/*receive_gps_data_task()*/
|
||||
parse_gps_msg();
|
||||
send_gps_pos();
|
||||
send_radIR();
|
||||
send_takeOff();
|
||||
}
|
||||
if ( link_fbw_receive_complete ) {
|
||||
link_fbw_receive_complete = FALSE;
|
||||
radio_control_task();
|
||||
}
|
||||
#ifndef NO_MAINLOOP
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,95 @@
|
||||
/*
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
//#include <stdlib.h>
|
||||
#include "modem.h"
|
||||
#include "std.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 )
|
||||
{
|
||||
/*
|
||||
on >= V1.2 boards, 4MHz modem clock is generated
|
||||
by one PWM module.
|
||||
*/
|
||||
#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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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 */
|
||||
@ -0,0 +1,216 @@
|
||||
/*
|
||||
$Id: nav.c,v 1.3 2011-01-21 11:52:44 moellmer Exp $
|
||||
|
||||
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 "math.h"
|
||||
|
||||
#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;
|
||||
float stage_time_ds;
|
||||
|
||||
#define RcRoll(travel) (from_fbw.channels[ RADIO_ROLL ]* (float)travel /(float)MAX_PPRZ)
|
||||
|
||||
#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; stage_time_ds = 0.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) { _Pragma("loopbound min 0 max 0") while (x < 0) x += 360; _Pragma("loopbound min 0 max 0") 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);}
|
||||
|
||||
#define MAX_DIST_CARROT 250.
|
||||
#define MIN_HEIGHT_CARROT 50.
|
||||
#define MAX_HEIGHT_CARROT 150.
|
||||
|
||||
#define Goto3D(radius) { static float carrot_x, carrot_y; int16_t pitch; int16_t roll; if (pprz_mode == PPRZ_MODE_AUTO2) { int16_t yaw = from_fbw.channels[ RADIO_YAW ]; if (yaw > MIN_DX || yaw < -MIN_DX) { carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);carrot_x = Min(carrot_x, MAX_DIST_CARROT); carrot_x = Max(carrot_x, -MAX_DIST_CARROT); } pitch = from_fbw.channels[ RADIO_PITCH ]; if (pitch > MIN_DX || pitch < -MIN_DX) { carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); carrot_y = Min(carrot_y, MAX_DIST_CARROT); carrot_y = Max(carrot_y, -MAX_DIST_CARROT);} vertical_mode = VERTICAL_MODE_AUTO_ALT; roll = from_fbw.channels[ RADIO_ROLL ]; if (roll > MIN_DX || roll < -MIN_DX) { desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); desired_altitude = Max(desired_altitude, MIN_HEIGHT_CARROT+GROUND_ALT); desired_altitude = Min(desired_altitude, MAX_HEIGHT_CARROT+GROUND_ALT); } } 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 = GROUND_ALT + MIN_HEIGHT_CARROT;
|
||||
float desired_x, desired_y;
|
||||
uint16_t nav_desired_gaz;
|
||||
float nav_pitch = NAV_PITCH;
|
||||
float nav_desired_roll;
|
||||
|
||||
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;
|
||||
float scal_prod;//changes for PowerPc Fev-06
|
||||
|
||||
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;
|
||||
|
||||
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 */
|
||||
{
|
||||
float alpha = atan2( estimator_y - waypoints[ WP_HOME ].y,
|
||||
estimator_x - waypoints[ WP_HOME ].x );
|
||||
float alpha_carrot = alpha + CARROT / ( -50 * estimator_hspeed_mod );
|
||||
fly_to_xy( waypoints[ WP_HOME ].x + cos( alpha_carrot )*fabs( 50 ),
|
||||
waypoints[ WP_HOME ].y + sin( alpha_carrot )*fabs( 50 ) );
|
||||
qdr = DegOfRad( M_PI / 2 - alpha_carrot );
|
||||
{
|
||||
_Pragma( "loopbounds min 0 max 2" )
|
||||
while ( qdr < 0 ) qdr += 360;
|
||||
_Pragma( "loopbounds min 0 max 2" )
|
||||
while ( qdr >= 360 ) qdr -= 360;
|
||||
}
|
||||
}
|
||||
|
||||
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( void )
|
||||
{
|
||||
compute_dist2_to_home();
|
||||
|
||||
auto_nav();
|
||||
}
|
||||
|
||||
|
||||
void nav_init( void )
|
||||
{
|
||||
nav_block = 0;
|
||||
nav_stage = 0;
|
||||
}
|
||||
@ -0,0 +1,58 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: nav.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
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 <std.h>
|
||||
|
||||
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 */
|
||||
extern float stage_time_ds; /* s */
|
||||
extern float nav_desired_roll;
|
||||
|
||||
void nav_update( void );
|
||||
void nav_home( void );
|
||||
void nav_init( void );
|
||||
|
||||
#endif /* NAV_H */
|
||||
@ -0,0 +1,139 @@
|
||||
/*
|
||||
$Id: pid.c,v 1.2 2011-01-21 11:52:44 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
This file is part of paparazzi.
|
||||
|
||||
paparazzi is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation; either version 2, or (at your option)
|
||||
any later version.
|
||||
|
||||
paparazzi is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with paparazzi; see the file COPYING. If not, write to
|
||||
the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
Boston, MA 02111-1307, USA.
|
||||
|
||||
*/
|
||||
|
||||
/** \file pid.c
|
||||
\brief PID controllers (roll, pitch, climb, altitude, course).
|
||||
|
||||
*/
|
||||
|
||||
//#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "pid.h"
|
||||
|
||||
#include "autopilot.h"
|
||||
#include "infrared.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;
|
||||
|
||||
float pitch_of_vz_pgain = CLIMB_PITCH_OF_VZ_PGAIN;
|
||||
float pitch_of_vz = 0.;
|
||||
|
||||
|
||||
/** \brief Computes ::desired_aileron and ::desired_elevator from attitude estimation and expected attitude. */
|
||||
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);
|
||||
_Pragma( "loopbound min 0 max 1" )
|
||||
while ( err > M_PI ) err -= 2 * M_PI;
|
||||
_Pragma( "loopbound min 0 max 1" )
|
||||
while ( err < -M_PI ) err += 2 * M_PI;
|
||||
nav_desired_roll = course_pgain * err; // * fspeed / AIR_SPEED;
|
||||
if ( nav_desired_roll > max_roll )
|
||||
nav_desired_roll = max_roll;
|
||||
else
|
||||
if ( nav_desired_roll < -max_roll )
|
||||
nav_desired_roll = -max_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;
|
||||
|
||||
float climb_pitch_pgain = CLIMB_PITCH_PGAIN;
|
||||
float climb_pitch_igain = CLIMB_PITCH_IGAIN;
|
||||
float climb_pitch_sum_err = 0.;
|
||||
float max_pitch = MAX_PITCH;
|
||||
float min_pitch = MIN_PITCH;
|
||||
|
||||
|
||||
#define MAX_CLIMB_SUM_ERR 100
|
||||
#define MAX_PITCH_CLIMB_SUM_ERR 100
|
||||
|
||||
/** \brief Computes desired_gaz and desired_pitch from desired_climb */
|
||||
void climb_pid_run ( void )
|
||||
{
|
||||
float err = estimator_z_dot - desired_climb;
|
||||
float fgaz;
|
||||
if ( auto_pitch ) { /* gaz constant */
|
||||
desired_gaz = nav_desired_gaz;
|
||||
desired_pitch = climb_pitch_pgain * ( err + climb_pitch_igain *
|
||||
climb_pitch_sum_err );
|
||||
if ( desired_pitch > max_pitch )
|
||||
desired_pitch = max_pitch;
|
||||
if ( desired_pitch < min_pitch )
|
||||
desired_pitch = min_pitch;
|
||||
climb_pitch_sum_err += err;
|
||||
if ( climb_pitch_sum_err > MAX_PITCH_CLIMB_SUM_ERR )
|
||||
climb_pitch_sum_err = MAX_PITCH_CLIMB_SUM_ERR;
|
||||
if ( climb_pitch_sum_err < - MAX_PITCH_CLIMB_SUM_ERR )
|
||||
climb_pitch_sum_err = - MAX_PITCH_CLIMB_SUM_ERR;
|
||||
} else { /* pitch almost constant */
|
||||
/* pitch offset for climb */
|
||||
pitch_of_vz = ( desired_climb > 0 ) ? desired_climb * pitch_of_vz_pgain : 0.;
|
||||
fgaz = climb_pgain * ( err + climb_igain * climb_sum_err ) + CLIMB_LEVEL_GAZ +
|
||||
CLIMB_GAZ_OF_CLIMB * desired_climb;
|
||||
climb_sum_err += err;
|
||||
if ( climb_sum_err > MAX_CLIMB_SUM_ERR ) climb_sum_err = MAX_CLIMB_SUM_ERR;
|
||||
if ( climb_sum_err < - MAX_CLIMB_SUM_ERR ) climb_sum_err = - MAX_CLIMB_SUM_ERR;
|
||||
desired_gaz = TRIM_UPPRZ( fgaz * MAX_PPRZ );
|
||||
desired_pitch = nav_pitch + pitch_of_vz;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@ -0,0 +1,61 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: pid.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
|
||||
#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;
|
||||
|
||||
extern float pitch_of_vz_pgain;
|
||||
extern float pitch_of_vz;
|
||||
|
||||
void climb_pid_run( void );
|
||||
void altitude_pid_run( void );
|
||||
|
||||
#endif /* PID_H */
|
||||
@ -0,0 +1,36 @@
|
||||
/*
|
||||
Paparazzi autopilot $Id: sirf.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
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 */
|
||||
@ -0,0 +1,43 @@
|
||||
#include <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
|
||||
#include "spi.h"
|
||||
#include "autopilot.h"
|
||||
#include "link_fbw.h"
|
||||
#include "ad7714.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++;
|
||||
}
|
||||
@ -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 */
|
||||
@ -0,0 +1,92 @@
|
||||
/*
|
||||
Paparazzi mcu0 timer functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
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 <arch/signal.h>
|
||||
#include <arch/io.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 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
|
||||
@ -0,0 +1,149 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.c,v 1.3 2011-01-25 09:40:37 plazar Exp $
|
||||
|
||||
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 <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
#include "uart.h"
|
||||
#include "std.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( const 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( const 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;
|
||||
_Pragma( "loopbound min 0 max 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 );
|
||||
}
|
||||
|
||||
@ -0,0 +1,48 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.h,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
|
||||
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 );
|
||||
void uart0_transmit( const uint8_t );
|
||||
void uart1_transmit( const uint8_t );
|
||||
|
||||
#define ReceiveUart0(cb) \
|
||||
SIGNAL( SIG_UART0_RECV ) { \
|
||||
uint8_t c = UDR0; \
|
||||
cb(c); \
|
||||
}
|
||||
#define ReceiveUart1(cb) \
|
||||
SIGNAL( SIG_UART1_RECV ) { \
|
||||
uint8_t c = UDR1; \
|
||||
cb(c); \
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,38 @@
|
||||
/*
|
||||
Paparazzi autopilot $Id: ubx.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
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 */
|
||||
@ -0,0 +1,121 @@
|
||||
/*
|
||||
Paparazzi fly by wire adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
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 <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
#include "adc_fbw.h"
|
||||
#include "std.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 fbw_adc_buf_channel( uint8_t adc_channel, struct adc_buf *s )
|
||||
{
|
||||
buffers[ adc_channel ] = s;
|
||||
}
|
||||
|
||||
void
|
||||
fbw_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 ?) */
|
||||
_Pragma( "loopbound min 8 max 8" )
|
||||
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 */
|
||||
@ -0,0 +1,62 @@
|
||||
/*
|
||||
Paparazzi fly by wire adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
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 <inttypes.h>
|
||||
|
||||
|
||||
#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 );
|
||||
void fbw_adc_init( void );
|
||||
void fbw_adc_buf_channel( uint8_t adc_channel, struct adc_buf *s );
|
||||
|
||||
#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2 */
|
||||
|
||||
#endif /* _ADC_H_ */
|
||||
@ -0,0 +1,69 @@
|
||||
/* $Id: link_autopilot.h,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
|
||||
(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 <inttypes.h>
|
||||
|
||||
#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)
|
||||
|
||||
void test_ppm_task( void );
|
||||
void check_mega128_values_task( void );
|
||||
void send_data_to_autopilot_task( void );
|
||||
void check_failsafe_task( void );
|
||||
#endif // LINK_AUTOPILOT_H
|
||||
@ -0,0 +1,219 @@
|
||||
/*
|
||||
Paparazzi $Id: main.c,v 1.4 2011-01-25 10:42:14 plazar Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <io.h>
|
||||
#include <signal.h>
|
||||
#include <interrupt.h>
|
||||
|
||||
#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<sizeof(struct inter_mcu_msg); i++) */
|
||||
/* uart_transmit(((uint8_t*)&to_mega128)[ i ]); */
|
||||
/* uart_transmit('\n'); */
|
||||
|
||||
/* uart_transmit(7); */
|
||||
/* uart_transmit(7); */
|
||||
/* uint8_t i; */
|
||||
/* for(i = 0; i < RADIO_CTL_NB; i++) { */
|
||||
/* extern uint16_t ppm_pulses[ ]; */
|
||||
/* uart_transmit(ppm_pulses[ i ]>>8); */
|
||||
/* uart_transmit(ppm_pulses[ i ] & 0xff); */
|
||||
/* } */
|
||||
/* uart_transmit('\n'); */
|
||||
/* } */
|
||||
|
||||
|
||||
/* Prepare data to be sent to mcu0 */
|
||||
static void to_autopilot_from_last_radio ( void )
|
||||
{
|
||||
uint8_t i;
|
||||
_Pragma( "loopbound min 9 max 9" )
|
||||
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
|
||||
}
|
||||
|
||||
void _Pragma( "entrypoint" ) send_data_to_autopilot_task( void )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
if ( !SpiIsSelected() && spi_was_interrupted ) {
|
||||
spi_was_interrupted = FALSE;
|
||||
to_autopilot_from_last_radio();
|
||||
spi_reset();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef PAPABENCH_SINGLE
|
||||
extern uint8_t _1Hz;
|
||||
extern uint8_t _20Hz;
|
||||
#else
|
||||
static uint8_t _1Hz;
|
||||
static uint8_t _20Hz;
|
||||
#endif
|
||||
|
||||
void fbw_init( void )
|
||||
{
|
||||
uart_init_tx();
|
||||
uart_print_string( "FBW Booting $Id: main.c,v 1.4 2011-01-25 10:42:14 plazar Exp $\n" );
|
||||
|
||||
#ifndef CTL_BRD_V1_1
|
||||
fbw_adc_init();
|
||||
fbw_adc_buf_channel( 3, &vsupply_adc_buf );
|
||||
fbw_adc_buf_channel( 6, &vservos_adc_buf );
|
||||
#endif
|
||||
timer_init();
|
||||
servo_init();
|
||||
ppm_init();
|
||||
fbw_spi_init();
|
||||
//sei(); //FN
|
||||
}
|
||||
|
||||
void fbw_schedule( void )
|
||||
{
|
||||
if ( time_since_last_mega128 < STALLED_TIME )
|
||||
time_since_last_mega128++;
|
||||
if ( time_since_last_ppm < REALLY_STALLED_TIME )
|
||||
time_since_last_ppm++;
|
||||
if ( _1Hz == 0 ) {
|
||||
last_ppm_cpt = ppm_cpt;
|
||||
ppm_cpt = 0;
|
||||
}
|
||||
test_ppm_task();
|
||||
check_mega128_values_task();
|
||||
send_data_to_autopilot_task();
|
||||
check_failsafe_task();
|
||||
if ( _20Hz >= 3 )
|
||||
servo_transmit();
|
||||
}
|
||||
|
||||
#ifndef PAPABENCH_SINGLE
|
||||
int main( void )
|
||||
{
|
||||
fbw_init();
|
||||
#ifndef NO_MAINLOOP
|
||||
while ( 1 ) {
|
||||
#endif
|
||||
fbw_schedule();
|
||||
if ( timer_periodic() ) {
|
||||
_1Hz++;
|
||||
_20Hz++;
|
||||
if ( _1Hz >= 60 )
|
||||
_1Hz = 0;
|
||||
if ( _20Hz >= 3 )
|
||||
_20Hz = 0;
|
||||
}
|
||||
#ifndef NO_MAINLOOP
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
void _Pragma( "entrypoint" ) test_ppm_task( void )
|
||||
{
|
||||
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 ( time_since_last_ppm >= STALLED_TIME )
|
||||
radio_ok = FALSE;
|
||||
if ( time_since_last_ppm >= REALLY_STALLED_TIME )
|
||||
radio_really_lost = TRUE;
|
||||
}
|
||||
void _Pragma( "entrypoint" ) check_failsafe_task( void )
|
||||
{
|
||||
if ( ( mode == MODE_MANUAL && !radio_ok ) ||
|
||||
( mode == MODE_AUTO && !mega128_ok ) )
|
||||
servo_set( failsafe );
|
||||
}
|
||||
void _Pragma( "entrypoint" ) check_mega128_values_task( void )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
if ( !SpiIsSelected() && spi_was_interrupted ) {
|
||||
if ( mega128_receive_valid ) {
|
||||
time_since_last_mega128 = 0;
|
||||
mega128_ok = TRUE;
|
||||
if ( mode == MODE_AUTO )
|
||||
servo_set( from_mega128.channels );
|
||||
}
|
||||
}
|
||||
if ( time_since_last_mega128 == STALLED_TIME )
|
||||
mega128_ok = FALSE;
|
||||
#endif
|
||||
}
|
||||
@ -0,0 +1,107 @@
|
||||
/* $Id: ppm.c,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
(c) 2003 Trammell Hudson <hudson@rotomotion.com>
|
||||
(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; }
|
||||
|
||||
#ifdef PAPABENCH_SINGLE
|
||||
SIGNAL( SIG_SPM_READY )
|
||||
#else
|
||||
SIGNAL( SIG_INPUT_CAPTURE1 )
|
||||
#endif
|
||||
{
|
||||
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 = 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;
|
||||
}
|
||||
|
||||
/* Copy from the ppm receiving buffer to the buffer sent to mcu0 */
|
||||
void last_radio_from_ppm()
|
||||
{
|
||||
LastRadioFromPpm()
|
||||
}
|
||||
@ -0,0 +1,102 @@
|
||||
/* $Id: ppm.h,v 1.2 2011-01-18 14:55:53 moellmer Exp $
|
||||
|
||||
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 <hudson@rotomotion.com>
|
||||
(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 <inttypes.h>
|
||||
#include <signal.h>
|
||||
|
||||
#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
|
||||
@ -0,0 +1,200 @@
|
||||
/* $Id: servo.c,v 1.4 2011-01-25 10:05:41 plazar Exp $
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
(c) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
(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 <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#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 */
|
||||
_Pragma( "loopbound min 10 max 10" )
|
||||
for ( i = 0 ; i < _4017_NB_CHANNELS ; i++ )
|
||||
// servo_widths[ i ] = SERVO_MIN;
|
||||
servo_widths[ i ] = ( SERVO_MIN + SERVO_MAX ) / 2;
|
||||
|
||||
/* 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
|
||||
_Pragma( "entrypoint" ) servo_transmit( void )
|
||||
{
|
||||
uint8_t servo;
|
||||
uart_transmit( ( uint8_t )0 );
|
||||
uart_transmit( ( uint8_t )0 );
|
||||
|
||||
_Pragma( "loopbound min 10 max 10" )
|
||||
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 */
|
||||
}
|
||||
@ -0,0 +1,61 @@
|
||||
/* $Id: servo.h,v 1.1 2011-01-18 12:48:40 moellmer Exp $
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
(c) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
(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 <inttypes.h>
|
||||
#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 */
|
||||
@ -0,0 +1,115 @@
|
||||
/*
|
||||
$Id: spi.c,v 1.2 2011-01-25 10:05:41 plazar Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
#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 fbw_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;
|
||||
}
|
||||
@ -0,0 +1,48 @@
|
||||
/* $Id: spi.h,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
|
||||
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 */
|
||||
@ -0,0 +1,92 @@
|
||||
/* $Id: timer.h,v 1.2 2011-01-18 14:55:53 moellmer Exp $
|
||||
|
||||
Paparazzi fbw timer functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
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 <inttypes.h>
|
||||
#include <signal.h>
|
||||
#include <io.h>
|
||||
#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
|
||||
@ -0,0 +1,133 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.c,v 1.4 2011-01-25 10:17:46 plazar Exp $
|
||||
|
||||
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 <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
|
||||
|
||||
#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 )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
/* 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 );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_init_rx()
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
/* Enable receiver */
|
||||
UCSRB |= _BV( RXEN );
|
||||
/* Enable uart receive interrupt */
|
||||
sbi( UCSRB, RXCIE );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_transmit( unsigned char data )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
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 );
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_print_hex ( uint8_t c )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
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 ] );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_print_hex16 ( uint16_t c )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
uint8_t high = ( uint8_t )( c >> 8 );
|
||||
uint8_t low = ( uint8_t )( c );
|
||||
uart_print_hex( high );
|
||||
uart_print_hex( low );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_print_string( const uint8_t *s )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
uint8_t i = 0;
|
||||
_Pragma( "loopbound min 100 max 100" )
|
||||
while ( s[ i ] ) {
|
||||
uart_transmit( s[ i ] );
|
||||
i++;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
SIGNAL( SIG_UART_TRANS )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
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 */
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@ -0,0 +1,39 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.h,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
|
||||
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 <inttypes.h>
|
||||
|
||||
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
|
||||
@ -0,0 +1,49 @@
|
||||
/*
|
||||
downlink.h
|
||||
Copyright (C) 2003 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
decodes downlink protocol
|
||||
*/
|
||||
|
||||
#ifndef DOWNLINK_H
|
||||
#define DOWNLINK_H
|
||||
|
||||
#include <glib.h>
|
||||
#include "protocol.h"
|
||||
#include "transport.h"
|
||||
#include "geometry.h"
|
||||
|
||||
#define DL_STX 0x05
|
||||
#define DL_ETX 0x06
|
||||
#define DL_HEAD_LEN 1
|
||||
#define DL_PAYLOAD_OFFSET 1
|
||||
#define DL_TAIL_LEN 2
|
||||
|
||||
|
||||
struct Transport *downlink_new( struct PprzProtocol *protocol,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data );
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
185
targets/wasm-tacle/parallel/PapaBench/sw/include/c/geometry.h
Normal file
185
targets/wasm-tacle/parallel/PapaBench/sw/include/c/geometry.h
Normal file
@ -0,0 +1,185 @@
|
||||
/*
|
||||
geometry.h
|
||||
Copyright (C) 2003 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
geometry :) originated from Pascal Brisset pascal(dot)brisset(at)free(dot)fr
|
||||
ocaml code.
|
||||
*/
|
||||
|
||||
#ifndef GEOMETRY_H
|
||||
#define GEOMETRY_H
|
||||
|
||||
#include <math.h>
|
||||
#include "angles.h"
|
||||
/*
|
||||
please use
|
||||
x, y, z, h in meters
|
||||
lat, lon in radians
|
||||
*/
|
||||
struct double_c2d {
|
||||
double x, y;
|
||||
};
|
||||
|
||||
struct double_p2d {
|
||||
double r, teta;
|
||||
};
|
||||
|
||||
struct double_e2d {
|
||||
double lat, lon;
|
||||
};
|
||||
|
||||
struct double_c3d {
|
||||
double x, y, z;
|
||||
};
|
||||
|
||||
struct double_e3d {
|
||||
double lat, lon, h;
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct double_c2d vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b );
|
||||
struct double_c2d unit_vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b );
|
||||
struct double_c2d normal_vector_c2d( const struct double_c2d u );
|
||||
|
||||
double vect_prod_c2d( const struct double_c2d a, const struct double_c2d b );
|
||||
double scal_prod_c2d( const struct double_c2d a, const struct double_c2d b );
|
||||
double module_c2d( const struct double_c2d a );
|
||||
double norme2_c2d( const struct double_c2d a );
|
||||
double distance_c2d( const struct double_c2d a, const struct double_c2d b );
|
||||
void rotate_c2d( struct double_c2d *a, double alpha );
|
||||
void translate_c2d( struct double_c2d *a, const struct double_c2d v );
|
||||
void add_c2d_s( struct double_c2d *a, const struct double_c2d b );
|
||||
struct double_c2d add_c2d( struct double_c2d a, struct double_c2d b );
|
||||
void minus_c2d_s( struct double_c2d *a, const struct double_c2d v );
|
||||
struct double_c2d minus_c2d( struct double_c2d a, const struct double_c2d v );
|
||||
void normalise_c2d_s( struct double_c2d *a );
|
||||
struct double_c2d normalise_c2d( struct double_c2d a );
|
||||
void scale_c2d_s( struct double_c2d *v, double factor );
|
||||
struct double_c2d scale_c2d( struct double_c2d v, double factor );
|
||||
double angle_c2d( struct double_c2d u, struct double_c2d v );
|
||||
|
||||
struct double_p2d p2d_of_c2d( const struct double_c2d c );
|
||||
struct double_c2d c2d_of_p2d( const struct double_p2d p );
|
||||
struct double_c2d c2d_of_polar( double r, double teta );
|
||||
struct double_p2d add_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 );
|
||||
struct double_p2d minus_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 );
|
||||
|
||||
void translate_c3d( struct double_c3d *a, const struct double_c3d v );
|
||||
void scale_c3d( struct double_c3d *v, double factor );
|
||||
|
||||
struct double_c2d lin_interp_c2d( struct double_c2d a, double ta,
|
||||
struct double_c2d b, double tb, double t );
|
||||
|
||||
|
||||
/* rad is an angle in EAST NORTH frame,
|
||||
returns the wind direction in degrees
|
||||
*/
|
||||
double wind_dir_from_angle_rad( double rad );
|
||||
|
||||
/* converts between angle in EAST NORTH frame
|
||||
and headings (0->N 90->E) */
|
||||
double heading_of_to_angle_deg( double angle );
|
||||
double heading_of_to_angle_rad( double angle );
|
||||
|
||||
/*
|
||||
compute bank angle
|
||||
*/
|
||||
double compute_phi( struct double_c2d curspeed, struct double_c2d last_speed,
|
||||
double delta_t );
|
||||
|
||||
#define SEC_OF_TIMEVAL(tv) (tv.tv_sec + 1e-6 * tv.tv_usec)
|
||||
#define DELAY_SEC_OF_TIMEVAL(tv1, tv2) (SEC_OF_TIMEVAL(tv1) - SEC_OF_TIMEVAL(tv2))
|
||||
|
||||
double mpi_pi( double val );
|
||||
|
||||
struct ellipsoid {
|
||||
double dx, dy, dz;
|
||||
double a;
|
||||
double df;
|
||||
double e;
|
||||
};
|
||||
|
||||
enum type_ellipsoid {
|
||||
NTF,
|
||||
WGS84,
|
||||
ED50,
|
||||
NAD27,
|
||||
ELLIPSOID_NB
|
||||
};
|
||||
|
||||
extern const struct ellipsoid ellipsoids[ ELLIPSOID_NB ];
|
||||
|
||||
struct lambert {
|
||||
const struct ellipsoid *ellipsoid;
|
||||
double phi0; /* radians */
|
||||
double lphi0;
|
||||
double r0;
|
||||
double lambda0;
|
||||
long x0;
|
||||
long y0;
|
||||
long ys;
|
||||
long k0; /* scale factor */
|
||||
};
|
||||
|
||||
|
||||
enum type_lambert {
|
||||
LAMBERTI,
|
||||
LAMBERTII,
|
||||
LAMBERTIIE,
|
||||
LAMBERTIII,
|
||||
LAMBERTIV,
|
||||
LAMBERT_NB
|
||||
};
|
||||
|
||||
extern const struct lambert lamberts[ LAMBERT_NB ];
|
||||
|
||||
|
||||
#define DECIMAL(D,M,S) (D + M / 60. + S / 3600.)
|
||||
#define RAD_OF_DEG(D) (D*M_PI/180.)
|
||||
#define DEG_OF_RAD(R) (R/M_PI*180.)
|
||||
#define LATITUDE_ISOMETRIQUE(PHI,E) (log(tan(M_PI_4 + PHI/2.0)) - E/2.0 * log((1.0+E*sin(PHI)) / (1.0-E*sin(PHI))))
|
||||
#define E_SQUARE(D) (2.0*D - D * D)
|
||||
#define E_PRIME_SQUARE(D) (1.0 / (1.0 - D) / (1.0 - D) - 1.0)
|
||||
|
||||
|
||||
|
||||
|
||||
/* projections */
|
||||
void lambert_of_e2d ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_e2d world_pos, struct double_c2d *lmb_pos );
|
||||
void e2d_of_lambert ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_c2d c_pos, struct double_e2d *e_pos );
|
||||
/* coordinates */
|
||||
void e3d_of_c3d( const enum type_ellipsoid elps, struct double_c3d c_pos,
|
||||
struct double_e3d *e_pos );
|
||||
void e2d_of_e2d( const enum type_ellipsoid dest, const enum type_ellipsoid src,
|
||||
struct double_e2d *e2d_pos );
|
||||
|
||||
|
||||
#endif /* GEOMETRY_H */
|
||||
@ -0,0 +1,66 @@
|
||||
/*
|
||||
glade_support.h
|
||||
Copyright (C) 2003 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
|
||||
glade hack.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef GLADE_SUPPORT_H
|
||||
#define GLADE_SUPPORT_H
|
||||
|
||||
#define DEFAULT_LEN 256
|
||||
|
||||
#define UPDATE_LABEL(object, label_name, fmt, args...) \
|
||||
{ \
|
||||
GtkLabel* label = GTK_LABEL(gtk_object_get_data(GTK_OBJECT(object), label_name)); \
|
||||
GString* str = g_string_sized_new(DEFAULT_LEN); \
|
||||
if (label) { \
|
||||
g_string_printf(str, fmt, ##args); \
|
||||
gtk_label_set_text(label, str->str); \
|
||||
} \
|
||||
else \
|
||||
g_message("##### unknown label [ %s ]", label_name); \
|
||||
g_string_free(str, TRUE); \
|
||||
} \
|
||||
|
||||
#define UPDATE_LABEL_COLOR(object, label_name, test, fmt, args...) \
|
||||
{ \
|
||||
GtkStateType state; \
|
||||
GtkLabel* label = GTK_LABEL(gtk_object_get_data(GTK_OBJECT(object), label_name)); \
|
||||
GString* str = g_string_sized_new(DEFAULT_LEN); \
|
||||
if (label) { \
|
||||
g_string_printf(str, fmt, ##args); \
|
||||
gtk_label_set_text(label, str->str); \
|
||||
} \
|
||||
else \
|
||||
g_message("##### unknown label [ %s ]", label_name); \
|
||||
g_string_free(str, TRUE); \
|
||||
for (state = GTK_STATE_NORMAL; state <= GTK_STATE_INSENSITIVE; state++) \
|
||||
gtk_widget_modify_fg(GTK_WIDGET(label), state, (test)?&my_red_color:>k_widget_get_default_style()->fg[ state ]); \
|
||||
} \
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* GLADE_SUPPORT_H */
|
||||
51
targets/wasm-tacle/parallel/PapaBench/sw/include/c/logger.h
Normal file
51
targets/wasm-tacle/parallel/PapaBench/sw/include/c/logger.h
Normal file
@ -0,0 +1,51 @@
|
||||
/*
|
||||
logger.h
|
||||
Copyright (C) 2003 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
write and read logs for the paparazzi
|
||||
*/
|
||||
|
||||
|
||||
#ifndef LOGGER_H
|
||||
#define LOGGER_H
|
||||
|
||||
#include <glib.h>
|
||||
#include <libxml/parser.h>
|
||||
|
||||
#include "downlink.h"
|
||||
#include "protocol.h"
|
||||
|
||||
struct Logger {
|
||||
GIOChannel *channel;
|
||||
};
|
||||
|
||||
|
||||
struct Logger *logger_new( const gchar *out_file, xmlDocPtr doc );
|
||||
void logger_log( struct Logger *this, struct PprzMsg *msg );
|
||||
void logger_free( struct Logger *this );
|
||||
|
||||
GList *logger_parse_log( const gchar *filename,
|
||||
struct PprzProtocol **protocol );
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
70
targets/wasm-tacle/parallel/PapaBench/sw/include/c/network.h
Normal file
70
targets/wasm-tacle/parallel/PapaBench/sw/include/c/network.h
Normal file
@ -0,0 +1,70 @@
|
||||
/*
|
||||
$id:$
|
||||
Copyright (C) 2003 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
network functions
|
||||
*/
|
||||
|
||||
#ifndef NETWORK_H
|
||||
#define NETWORK_H
|
||||
|
||||
#include <glib.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
|
||||
#include "protocol.h"
|
||||
|
||||
struct NetworkClient {
|
||||
int socket;
|
||||
struct sockaddr_in bcast_addr;
|
||||
struct sockaddr_in server_addr;
|
||||
GFunc rcv_callback;
|
||||
GFunc err_callback;
|
||||
gpointer user_data;
|
||||
struct PprzProtocol *protocol;
|
||||
};
|
||||
|
||||
struct NetworkServer {
|
||||
int bcast_socket;
|
||||
struct sockaddr_in bcast_addr;
|
||||
int listening_socket;
|
||||
GFunc rcv_callback;
|
||||
GFunc err_callback;
|
||||
gpointer user_data;
|
||||
struct PprzProtocol *protocol;
|
||||
};
|
||||
|
||||
|
||||
struct NetworkServer *network_server_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data );
|
||||
void network_server_dispatch( struct NetworkServer *this, struct PprzMsg *msg );
|
||||
|
||||
|
||||
struct NetworkClient *network_client_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data );
|
||||
void network_client_send_to_server( struct NetworkClient *this,
|
||||
struct PprzMsg *msg );
|
||||
|
||||
|
||||
#endif /* NETWORK_H */
|
||||
120
targets/wasm-tacle/parallel/PapaBench/sw/include/c/protocol.h
Normal file
120
targets/wasm-tacle/parallel/PapaBench/sw/include/c/protocol.h
Normal file
@ -0,0 +1,120 @@
|
||||
/*
|
||||
$Id: protocol.h,v 1.1 2011-01-18 12:48:44 moellmer Exp $
|
||||
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 PROTOCOL_H
|
||||
#define PROTOCOL_H
|
||||
|
||||
#include <glib.h>
|
||||
#include <libxml/xmlmemory.h>
|
||||
#include <libxml/parser.h>
|
||||
|
||||
enum PprzFieldType {
|
||||
TYP_UINT_8,
|
||||
TYP_UINT_16,
|
||||
TYP_UINT_32,
|
||||
TYP_INT_8,
|
||||
TYP_INT_16,
|
||||
TYP_INT_32,
|
||||
TYP_FLOAT,
|
||||
TYP_ARRAY_UINT_8,
|
||||
TYP_NB
|
||||
};
|
||||
|
||||
struct PprzFieldClass {
|
||||
gchar *name;
|
||||
enum PprzFieldType type;
|
||||
guint offset;
|
||||
gchar *format;
|
||||
gchar *unit;
|
||||
gchar *description;
|
||||
};
|
||||
|
||||
struct PprzMsgClass {
|
||||
guint id;
|
||||
gchar *name;
|
||||
GList *fields_classes;
|
||||
GHashTable *fields_classes_by_name;
|
||||
struct PprzFieldClass **fields_classes_by_id;
|
||||
guint nb_fields_classes;
|
||||
guint size; /* size in byte off all fields + 1 for id */
|
||||
};
|
||||
|
||||
struct PprzMsg {
|
||||
GTimeVal date;
|
||||
struct PprzMsgClass *class;
|
||||
guchar *bytes;
|
||||
};
|
||||
|
||||
struct PprzProtocol {
|
||||
GList *msgs_classes;
|
||||
GHashTable *msgs_classes_by_name;
|
||||
struct PprzMsgClass **msgs_classes_by_id;
|
||||
guint nb_msgs_classes;
|
||||
};
|
||||
|
||||
struct PprzProtocol *pprz_protocol_new_from_xml( xmlDocPtr doc,
|
||||
xmlNodePtr cur );
|
||||
void pprz_protocol_free( struct PprzProtocol *this );
|
||||
|
||||
struct PprzMsg *pprz_msg_new( struct PprzMsgClass *msg_class, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_id( struct PprzProtocol *this,
|
||||
guchar id, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_name( struct PprzProtocol *this,
|
||||
const gchar *name, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_bin( struct PprzProtocol *this,
|
||||
const guchar *buf, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_ascii( struct PprzProtocol *this,
|
||||
const gchar *line );
|
||||
void pprz_msg_free( struct PprzMsg *msg );
|
||||
|
||||
void pprz_protocol_ascii_of_msg( struct PprzMsg *msg, GString **buf );
|
||||
|
||||
const gchar *pprz_protocol_str_of_field_type( enum PprzFieldType type );
|
||||
const guint pprz_protocol_size_of_field_type( enum PprzFieldType type );
|
||||
|
||||
const struct PprzMsgClass *protocol_msg_class_of_id( struct PprzProtocol *this,
|
||||
guint id );
|
||||
|
||||
void pprz_msg_ascii_of_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, GString *buf );
|
||||
|
||||
gpointer pprz_msg_get_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class );
|
||||
gpointer pprz_msg_get_field_by_name( struct PprzMsg *msg, const gchar *name );
|
||||
gpointer pprz_msg_get_field_by_place( struct PprzMsg *msg, guint place );
|
||||
|
||||
gboolean pprz_msg_set_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, gconstpointer value );
|
||||
gboolean pprz_msg_set_field_by_name( struct PprzMsg *msg, const gchar *name,
|
||||
gconstpointer value );
|
||||
gboolean pprz_msg_set_field_by_id( struct PprzMsg *msg, guint id,
|
||||
gconstpointer value );
|
||||
|
||||
gboolean pprz_msg_set_field_ascii( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, const gchar *str_val );
|
||||
gboolean pprz_msg_set_field_ascii_by_name( struct PprzMsg *msg,
|
||||
const gchar *name, const gchar *str_val );
|
||||
gboolean pprz_msg_set_field_ascii_by_id( struct PprzMsg *msg, guint id,
|
||||
const gchar *str_val );
|
||||
|
||||
|
||||
#endif /* PROTOCOL_H */
|
||||
60
targets/wasm-tacle/parallel/PapaBench/sw/include/c/traces.h
Normal file
60
targets/wasm-tacle/parallel/PapaBench/sw/include/c/traces.h
Normal file
@ -0,0 +1,60 @@
|
||||
/*
|
||||
traces.h
|
||||
Copyright (C) 2003 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 TRACES_H
|
||||
#define TRACES_H
|
||||
|
||||
#define TRACE_ON
|
||||
#define TIME_STAMP_TRACE
|
||||
|
||||
#ifdef TRACE_ON
|
||||
#include <stdio.h>
|
||||
|
||||
#define TRACE_ERROR 0x00000001L
|
||||
#define TRACE_DATA_READ 0x00000002L
|
||||
#define TRACE_MSG_RAW 0x00000004L
|
||||
#define TRACE_MSG 0x00000008L
|
||||
#define TRACE_CHECKSUM 0x00000010L
|
||||
#define TRACE_DOWNLINK 0x00000020L
|
||||
#define TRACE_MODEM 0x00000040L
|
||||
#define TRACE_MODEM_VERB 0x00000080L
|
||||
#define TRACE_MODEM_READ 0x00000100L
|
||||
#define TRACE_PARSER 0x00000200L
|
||||
#define TRACE_TRANSPORT 0x00000400L
|
||||
#define TRACE_TRANSPORT_VERB 0x00000800L
|
||||
|
||||
extern unsigned long TraceLevel;
|
||||
|
||||
#define TRACE(type,fmt,args...) \
|
||||
{ \
|
||||
if (TraceLevel&type) { \
|
||||
fprintf(stderr, "[ "#type" ] " fmt, ##args); \
|
||||
} \
|
||||
}
|
||||
const char *print_hex( const unsigned char *buf, unsigned int len );
|
||||
|
||||
#else
|
||||
#define TRACE(type,fmt,args...)
|
||||
#endif /* TRACE_ON */
|
||||
|
||||
#endif /* TRACES_H */
|
||||
|
||||
@ -0,0 +1,75 @@
|
||||
#ifndef TRANSPORT_H
|
||||
#define TRANSPORT_H
|
||||
|
||||
#include <glib.h>
|
||||
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#define TRANSPORT_BUF_LEN 1024
|
||||
#define TRANSPORT_HEAD_LEN 1
|
||||
#define TRANSPORT_PAYLOAD_OFFSET TRANSPORT_HEAD_LEN
|
||||
#define TRANSPORT_TAIL_LEN 2
|
||||
|
||||
enum TransportError {
|
||||
TRANSPORT_NO_ERROR,
|
||||
TRANSPORT_CHECKSUM_ERROR,
|
||||
TRANSPORT_NO_ETX,
|
||||
TRANSPORT_INVALID_MSG_ID,
|
||||
TRANSPORT_INVALID_MSG_LEN,
|
||||
TRANSPORT_BUF_OVFW,
|
||||
TRANSPORT_ERROR_NB
|
||||
};
|
||||
|
||||
struct TransportMsg {
|
||||
GTimeVal date;
|
||||
guint len;
|
||||
guchar *data;
|
||||
};
|
||||
|
||||
struct TransportStatus {
|
||||
guint32 run_time;
|
||||
guint32 nb_byte;
|
||||
guint32 nb_msg;
|
||||
guint32 nb_err;
|
||||
gfloat byte_rate;
|
||||
gfloat msg_rate;
|
||||
};
|
||||
|
||||
struct Transport {
|
||||
void( *err_callback )( gpointer callback_data, GError *error );
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg );
|
||||
gpointer callback_data;
|
||||
GQuark quark;
|
||||
gchar stx;
|
||||
gchar etx;
|
||||
gboolean fixed_size;
|
||||
gboolean two_bytes_checksum;
|
||||
guint nb_msg_type;
|
||||
guint *size_msg;
|
||||
guint max_msg_size;
|
||||
struct TransportStatus status;
|
||||
gulong nb_msg_last_status;
|
||||
gulong nb_byte_last_status;
|
||||
guchar buf[ TRANSPORT_BUF_LEN ];
|
||||
guint buf_len;
|
||||
GTimeVal start_date, last_status_date;
|
||||
|
||||
};
|
||||
|
||||
struct Transport *transport_new( gboolean fixed_size,
|
||||
gboolean two_bytes_checksum, guint nb_msg, guint *size_msg, guint max_msg_size,
|
||||
guchar stx, guchar etx,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data );
|
||||
|
||||
struct TransportStatus *transport_get_status( struct Transport *this );
|
||||
|
||||
void transport_feed_data( struct Transport *this, const guchar *buf,
|
||||
guint len );
|
||||
|
||||
void transport_free( struct Transport *this );
|
||||
|
||||
|
||||
#endif /* TRANSPORT_H */
|
||||
12
targets/wasm-tacle/parallel/PapaBench/sw/include/inttypes.h
Normal file
12
targets/wasm-tacle/parallel/PapaBench/sw/include/inttypes.h
Normal file
@ -0,0 +1,12 @@
|
||||
#ifndef INTTYPES_H_
|
||||
#define INTTYPES_H_
|
||||
|
||||
typedef signed char int8_t;
|
||||
typedef unsigned char uint8_t;
|
||||
typedef signed short int16_t;
|
||||
typedef unsigned short uint16_t;
|
||||
typedef signed long int32_t;
|
||||
typedef unsigned long uint32_t;
|
||||
|
||||
#endif // INTTYPES_H_
|
||||
|
||||
21
targets/wasm-tacle/parallel/PapaBench/sw/include/math.h
Normal file
21
targets/wasm-tacle/parallel/PapaBench/sw/include/math.h
Normal file
@ -0,0 +1,21 @@
|
||||
#ifndef MATH_H_
|
||||
#define MATH_H_
|
||||
|
||||
#define M_PI 3.141592653589793238462643
|
||||
#define HALFPI 1.57079632679489661923
|
||||
#define sin(x) (pp_sin(x))
|
||||
#define atan2(x,y) (pp_atan2(x,y))
|
||||
#define sqrt(x) (pp_sqrt(x))
|
||||
#define cos(x) (pp_sin(x+HALFPI)) /*cosinus [ radians ]*/
|
||||
#define fabs(x) (pp_fabs(x))
|
||||
#define pp_fabs(x) ((x)< 0. ? -(x) : (x)) /*floating absolute value*/
|
||||
#define Max(x,y) (pp_Max(x,y))
|
||||
#define pp_Max(x,y) ((x) >= (y) ? (x) : (y))
|
||||
#define Min(x,y) (pp_Min(x,y))
|
||||
#define pp_Min(x,y) ((x) <= (y) ? (x) : (y))
|
||||
|
||||
extern double pp_sin( double x );
|
||||
extern double pp_sqrt( double x );
|
||||
extern double pp_atan2( double x, double y );
|
||||
|
||||
#endif /*MATH_H_*/
|
||||
22
targets/wasm-tacle/parallel/PapaBench/sw/include/std.h
Normal file
22
targets/wasm-tacle/parallel/PapaBench/sw/include/std.h
Normal file
@ -0,0 +1,22 @@
|
||||
#ifndef STD_H
|
||||
#define STD_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE 0
|
||||
#endif
|
||||
#ifndef TRUE
|
||||
#define TRUE (!FALSE)
|
||||
#endif
|
||||
|
||||
typedef uint8_t bool_t; // Boolean values
|
||||
|
||||
#ifndef cbi
|
||||
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
|
||||
#endif
|
||||
#ifndef sbi
|
||||
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
|
||||
#endif
|
||||
|
||||
#endif /* STD_H */
|
||||
65
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/downlink.c
Normal file
65
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/downlink.c
Normal file
@ -0,0 +1,65 @@
|
||||
/*
|
||||
downlink.c
|
||||
Copyright (C) 2003 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 <string.h>
|
||||
|
||||
#include "downlink.h"
|
||||
|
||||
#define GDOUBLE_FROM_DIF_TV(a,b) ((gdouble)(a.tv_sec-b.tv_sec) + 1e-6 * (gdouble)(a.tv_usec-b.tv_usec))
|
||||
|
||||
|
||||
#define READ_GFLOAT_FROM_BUF(dest, buf) dest = *((gfloat*)(buf)); buf += 4;
|
||||
#define READ_GUINT32_FROM_BUF(dest, buf) dest = GUINT32_FROM_LE(*((guint32*)(buf))); buf += 4;
|
||||
#define READ_GUINT16_FROM_BUF(dest, buf) dest = GUINT16_FROM_LE(*((guint16*)(buf))); buf += 2;
|
||||
#define READ_GINT32_FROM_BUF(dest, buf) dest = GINT32_FROM_LE(*((gint32*)(buf))); buf += 4;
|
||||
#define READ_GINT16_FROM_BUF(dest, buf) dest = GINT16_FROM_LE(*((gint16*)(buf))); buf += 2;
|
||||
#define READ_GUINT8_FROM_BUF(dest, buf) dest = (guint8)(*(buf++));
|
||||
|
||||
struct Transport *downlink_new( struct PprzProtocol *protocol,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data )
|
||||
{
|
||||
struct Transport *transport;
|
||||
guint *fixed_payload_len = g_new( guint,
|
||||
g_list_length( protocol->msgs_classes ) );
|
||||
guint i = 0;
|
||||
GList *msgs_classes = protocol->msgs_classes;
|
||||
while ( msgs_classes ) {
|
||||
fixed_payload_len[ i ] = ( ( struct PprzMsgClass * )msgs_classes->data )->size;
|
||||
// printf("fixed_payload_len : %d %d\n", i, fixed_payload_len[ i ]);
|
||||
i++;
|
||||
msgs_classes = msgs_classes->next;
|
||||
}
|
||||
|
||||
transport = transport_new( TRUE, /* fixed_size */
|
||||
TRUE, /* two bytes checksum */
|
||||
i, /* nb_msg */
|
||||
fixed_payload_len, /* size_msg */
|
||||
100, /* max_msg_size */
|
||||
DL_STX, DL_ETX, /* STX, ETX */
|
||||
err_callback, /* err callback */
|
||||
msg_callback, /* msg callback */
|
||||
callback_data ); /* callback data */
|
||||
return transport;
|
||||
}
|
||||
451
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/geometry.c
Normal file
451
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/geometry.c
Normal file
@ -0,0 +1,451 @@
|
||||
/*
|
||||
geometry.c
|
||||
Copyright (C) 2003 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 "geometry.h"
|
||||
|
||||
//#include <math.h>
|
||||
#include "math_papabench.h"
|
||||
|
||||
const struct ellipsoid ellipsoids[ ELLIPSOID_NB ] = {
|
||||
{-168., -60., 320., 6378249.2, 0.0034075495234250643, 0.08248325676}, /* NTF */
|
||||
{ 0., 0., 0., 6378137.0, 0.0033528106647474805, 0.08181919106}, /* WGS84 */
|
||||
{-87.0, -98., -121.0, 6378388.0, 0.003367003367003367, 0.08199188998}, /* ED50 */
|
||||
{ 0.0, 125.0, 194.0, 6378067.6, 0.0033155460257, 0.08181919106} /* NAD27 */
|
||||
};
|
||||
/* From http://www.tandt.be/wis/WiS/eqntf.html and http://www.ign.fr/MP/GEOD/geodesie/coordonnees.html */
|
||||
const struct lambert lamberts[ LAMBERT_NB ] = {
|
||||
{ /* LAMBERTI */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 49., 30., 0. ) ), /* phi0 */
|
||||
0.991996665, /* lphi0 */
|
||||
5457616.674, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
200000, /* y0 */
|
||||
5657617, /* ys */
|
||||
0.99987734 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTII */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
5999695.77, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
2200000, /* y0 */
|
||||
6199696, /* ys */
|
||||
0.99987742 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTIIE */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
5999695.77, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
2200000, /* y0 */
|
||||
8199696, /* ys */
|
||||
0.99987742 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTIII */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 44., 6., 0. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
6591905.08, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
3200000, /* y0 */
|
||||
6791905, /* ys */
|
||||
0.99987750 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTIV */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 42., 9., 54. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
7053300.18, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
234, /* x0 */ /* FIXME mmmmmmm suspicious */
|
||||
4185861, /* y0 */
|
||||
7239162, /* ys */
|
||||
0.99994471 /* k0 */
|
||||
},
|
||||
};
|
||||
|
||||
double distance_c2d( const struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
return sqrt( ( a.x - b.x ) * ( a.x - b.x ) + ( a.y - b.y ) * ( a.y - b.y ) );
|
||||
}
|
||||
|
||||
struct double_c2d vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b )
|
||||
{
|
||||
struct double_c2d v;
|
||||
v.x = b.x - a.x;
|
||||
v.y = b.y - a.y;
|
||||
return v;
|
||||
}
|
||||
|
||||
struct double_c2d unit_vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b )
|
||||
{
|
||||
struct double_c2d v = vector_c2d( a, b );
|
||||
scale_c2d_s( &v, 1 / module_c2d( v ) );
|
||||
return v;
|
||||
}
|
||||
|
||||
struct double_c2d normal_vector_c2d( const struct double_c2d u )
|
||||
{
|
||||
struct double_c2d v;
|
||||
v.x = -u.y;
|
||||
v.y = u.x;
|
||||
return v;
|
||||
}
|
||||
|
||||
double vect_prod_c2d( const struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
return a.x * b.y - a.y * b.x;
|
||||
}
|
||||
|
||||
double scal_prod_c2d( const struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
return a.x * b.x + a.y * b.y;
|
||||
}
|
||||
|
||||
void rotate_c2d( struct double_c2d *a, double alpha )
|
||||
{
|
||||
double tmp;
|
||||
tmp = a->x * cos( alpha ) - a->y * sin( alpha );
|
||||
a->y = a->x * sin( alpha ) + a->y * cos( alpha );
|
||||
a->x = tmp;
|
||||
}
|
||||
|
||||
double norme2_c2d( const struct double_c2d a )
|
||||
{
|
||||
return a.x * a.x + a.y * a.y;
|
||||
}
|
||||
|
||||
double module_c2d( const struct double_c2d a )
|
||||
{
|
||||
return sqrt( norme2_c2d( a ) );
|
||||
}
|
||||
|
||||
void translate_c2d( struct double_c2d *a, const struct double_c2d v )
|
||||
{
|
||||
add_c2d_s( a, v );
|
||||
}
|
||||
|
||||
void add_c2d_s( struct double_c2d *a, const struct double_c2d b )
|
||||
{
|
||||
a->x = a->x + b.x;
|
||||
a->y = a->y + b.y;
|
||||
}
|
||||
|
||||
struct double_c2d add_c2d( struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
add_c2d_s( &a, b );
|
||||
return a;
|
||||
}
|
||||
|
||||
void normalise_c2d_s( struct double_c2d *a )
|
||||
{
|
||||
scale_c2d_s( a, 1 / module_c2d( *a ) );
|
||||
}
|
||||
|
||||
struct double_c2d normalise_c2d( struct double_c2d a )
|
||||
{
|
||||
normalise_c2d_s( &a );
|
||||
return a;
|
||||
}
|
||||
|
||||
void minus_c2d_s( struct double_c2d *a, const struct double_c2d b )
|
||||
{
|
||||
a->x = a->x - b.x;
|
||||
a->y = a->y - b.y;
|
||||
}
|
||||
|
||||
struct double_c2d minus_c2d( struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
minus_c2d_s( &a, b );
|
||||
return a;
|
||||
}
|
||||
|
||||
void scale_c2d_s( struct double_c2d *v, double factor )
|
||||
{
|
||||
v->x = v->x * factor;
|
||||
v->y = v->y * factor;
|
||||
}
|
||||
|
||||
struct double_c2d scale_c2d( struct double_c2d v, double factor )
|
||||
{
|
||||
scale_c2d_s( &v, factor );
|
||||
return v;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define MPI_PI(val)\
|
||||
{\
|
||||
while (val> M_PI) val -= 2. * M_PI;\
|
||||
while (val<-M_PI) val += 2. * M_PI;\
|
||||
}
|
||||
|
||||
double mpi_pi( double val )
|
||||
{
|
||||
MPI_PI( val );
|
||||
return val;
|
||||
};
|
||||
|
||||
struct double_c2d lin_interp_c2d( struct double_c2d a, double ta,
|
||||
struct double_c2d b, double tb, double t )
|
||||
{
|
||||
double delta_t = tb - ta;
|
||||
struct double_c2d res;
|
||||
res.x = a.x + ( t - ta ) / delta_t *( b.x - a.x );
|
||||
res.y = a.y + ( t - ta ) / delta_t *( b.y - a.y );
|
||||
return res;
|
||||
}
|
||||
|
||||
double wind_dir_from_angle_deg( double deg )
|
||||
{
|
||||
double w = 180. + 90. - deg;
|
||||
while ( w > 360 ) w -= 360;
|
||||
return w;
|
||||
}
|
||||
|
||||
double wind_dir_from_angle_rad( double rad )
|
||||
{
|
||||
double w = M_PI + M_PI_2 - rad;
|
||||
while ( w > 2 * M_PI ) w -= ( 2 * M_PI );
|
||||
return w;
|
||||
}
|
||||
|
||||
double heading_of_to_angle_deg( double angle )
|
||||
{
|
||||
double a = 90. - angle + 360.;
|
||||
while ( a >= 180. ) a -= 360.;
|
||||
return a;
|
||||
}
|
||||
|
||||
double heading_of_to_angle_rad( double angle )
|
||||
{
|
||||
double two_pi = 2 * M_PI;
|
||||
double a = M_PI_2 - angle + two_pi;
|
||||
while ( a >= two_pi ) a -= two_pi;
|
||||
return a;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define G 9.81
|
||||
#define ACCEL_DOWN 0
|
||||
double compute_phi( struct double_c2d curspeed, struct double_c2d last_speed,
|
||||
double delta_t )
|
||||
{
|
||||
double accel_norm, accel_tgt, phi;
|
||||
struct double_c2d accel = minus_c2d( curspeed, last_speed );
|
||||
// printf("%lf %lf %lf %lf %lf\n", curspeed.x, curspeed.y, last_speed.x, last_speed.y, delta_t);
|
||||
scale_c2d_s( &accel, 1 / delta_t );
|
||||
accel_norm = vect_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
|
||||
accel_tgt = scal_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
|
||||
phi = -atan2( accel_norm, G - ACCEL_DOWN );
|
||||
return phi;
|
||||
}
|
||||
|
||||
double angle_c2d( struct double_c2d u, struct double_c2d v )
|
||||
{
|
||||
double alpha1 = atan2( u.y, u.x );
|
||||
double alpha2 = atan2( v.y, v.x );
|
||||
double alpha = alpha2 - alpha1;
|
||||
MPI_PI( alpha );
|
||||
return alpha;
|
||||
}
|
||||
|
||||
struct double_p2d p2d_of_c2d( const struct double_c2d c )
|
||||
{
|
||||
struct double_p2d p;
|
||||
p.r = module_c2d( c );
|
||||
p.teta = atan2( c.y, c.x );
|
||||
return p;
|
||||
}
|
||||
|
||||
struct double_c2d c2d_of_p2d( const struct double_p2d p )
|
||||
{
|
||||
struct double_c2d c;
|
||||
c.x = p.r * cos( p.teta );
|
||||
c.y = p.r * sin( p.teta );
|
||||
return c;
|
||||
}
|
||||
|
||||
struct double_p2d add_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 )
|
||||
{
|
||||
/* p.r = sqrt(p1.r*p1.r + p2.r*p2.r + 2 * p1.r * p2.r * cos ( p1.teta - p2.teta )); */
|
||||
/* p.teta = 0; */
|
||||
struct double_c2d c1 = c2d_of_p2d( p1 );
|
||||
struct double_c2d c2 = c2d_of_p2d( p2 );
|
||||
struct double_c2d c3 = add_c2d( c1, c2 );
|
||||
struct double_p2d p = p2d_of_c2d( c3 );
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
struct double_p2d minus_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 )
|
||||
{
|
||||
struct double_c2d c1 = c2d_of_p2d( p1 );
|
||||
struct double_c2d c2 = c2d_of_p2d( p2 );
|
||||
struct double_c2d c3 = minus_c2d( c1, c2 );
|
||||
struct double_p2d p = p2d_of_c2d( c3 );
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
struct double_c2d c2d_of_polar( double r, double teta )
|
||||
{
|
||||
struct double_c2d res = {r * cos( teta ), r * sin( teta )};
|
||||
return res;
|
||||
}
|
||||
|
||||
void translate_c3d( struct double_c3d *a, const struct double_c3d v )
|
||||
{
|
||||
a->x = a->x + v.x;
|
||||
a->y = a->y + v.y;
|
||||
a->z = a->z + v.z;
|
||||
}
|
||||
|
||||
void scale_c3d( struct double_c3d *v, double factor )
|
||||
{
|
||||
v->x = v->x * factor;
|
||||
v->y = v->y * factor;
|
||||
v->z = v->z * factor;
|
||||
}
|
||||
|
||||
static double inverse_latitude_isometrique( double lat, double e,
|
||||
double epsilon )
|
||||
{
|
||||
double exp_l = exp( lat );
|
||||
double phi0 = 2. * atan( exp_l - M_PI_2 );
|
||||
double loop( double phi ) {
|
||||
double sin_phi = e * sin( phi );
|
||||
double new_phi = 2. * atan ( pow( ( 1. + sin_phi ) / ( 1. - sin_phi ),
|
||||
e / 2 ) * exp_l ) - M_PI_2;
|
||||
if ( fabs( phi - new_phi ) < epsilon ) return new_phi;
|
||||
return loop( new_phi );
|
||||
}
|
||||
return loop( phi0 );
|
||||
}
|
||||
|
||||
static double lambert_n ( const enum type_lambert lmb )
|
||||
{
|
||||
return sin( lamberts[ lmb ].phi0 );
|
||||
}
|
||||
static double lambert_c ( const enum type_lambert lmb )
|
||||
{
|
||||
return lamberts[ lmb ].r0 * exp( lamberts[ lmb ].phi0 * lambert_n( lmb ) );
|
||||
}
|
||||
|
||||
void lambert_of_e2d ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_e2d e2d_pos, struct double_c2d *lmb_pos )
|
||||
{
|
||||
double n, ll, r, gamma;
|
||||
e2d_of_e2d( NTF, elps, &e2d_pos );
|
||||
n = lambert_n( lmb );
|
||||
ll = LATITUDE_ISOMETRIQUE( e2d_pos.lat, lamberts[ lmb ].ellipsoid->e );
|
||||
r = lamberts[ lmb ].r0 * exp( lamberts[ lmb ].lphi0 * sin( lamberts[ lmb ].phi0 ) ) *
|
||||
exp( -ll * n );
|
||||
gamma = ( e2d_pos.lon - lamberts[ lmb ].lambda0 ) * n;
|
||||
lmb_pos->x = ( double )lamberts[ lmb ].x0 + r * sin( gamma );
|
||||
lmb_pos->y = ( double )lamberts[ lmb ].ys - r * cos( gamma );
|
||||
}
|
||||
|
||||
void e2d_of_lambert ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_c2d c_pos, struct double_e2d *e_pos )
|
||||
{
|
||||
double dx = c_pos.x - lamberts[ lmb ].x0;
|
||||
double dy = c_pos.y - lamberts[ lmb ].ys;
|
||||
double r = sqrt( dx * dx + dy * dy );
|
||||
double gamma = atan2( dx, -dy );
|
||||
double lambda = lamberts[ lmb ].lambda0 + gamma / lambert_n( lmb );
|
||||
double ll = -1. / lambert_n( lmb ) * log( fabs( r / lambert_c( lmb ) ) );
|
||||
double phi = inverse_latitude_isometrique( ll, lamberts[ lmb ].ellipsoid->e,
|
||||
1e-11 );
|
||||
e_pos->lon = lambda;
|
||||
e_pos->lat = phi;
|
||||
e2d_of_e2d( elps, NTF, e_pos );
|
||||
}
|
||||
|
||||
void e2d_of_e2d( const enum type_ellipsoid dest, const enum type_ellipsoid src,
|
||||
struct double_e2d *e2d_pos )
|
||||
{
|
||||
if ( dest != src ) {
|
||||
double sin_lat = sin( e2d_pos->lat );
|
||||
double cos_lat = cos( e2d_pos->lat );
|
||||
double sin_lon = sin( e2d_pos->lon );
|
||||
double cos_lon = cos( e2d_pos->lon );
|
||||
double d16 = E_SQUARE( ellipsoids[ src ].df );
|
||||
double d17 = E_SQUARE( ellipsoids[ dest ].df );
|
||||
double d18 = ellipsoids[ src ].a / sqrt ( 1.0 - d16 * sin_lat * sin_lat );
|
||||
double d20 = d18 * cos_lat * cos_lon;
|
||||
double d21 = d18 * cos_lat * sin_lon;
|
||||
double d22 = d18 * ( 1.0 - d16 ) * sin_lat;
|
||||
double d23 = d20 - ellipsoids[ dest ].dx + ellipsoids[ src ].dx;
|
||||
double d24 = d21 - ellipsoids[ dest ].dy + ellipsoids[ src ].dy;
|
||||
double d25 = d22 - ellipsoids[ dest ].dz + ellipsoids[ src ].dz;
|
||||
double d26 = sqrt ( d23 * d23 + d24 * d24 );
|
||||
double d27 = atan2( d25, d26 * ( 1.0 - ellipsoids[ dest ].df ) );
|
||||
double d28 = ellipsoids[ dest ].a * ( 1.0 - ellipsoids[ dest ].df );
|
||||
double d29 = E_PRIME_SQUARE( ellipsoids[ dest ].df );
|
||||
double d3 = atan2( ( d25 + d29 * d28 * sin( d27 ) * sin( d27 ) * sin( d27 ) ),
|
||||
( d26 - d17 * ellipsoids[ dest ].a * cos( d27 ) * cos( d27 ) * cos( d27 ) ) );
|
||||
double d4 = atan2( d24, d23 );
|
||||
e2d_pos->lon = d4;
|
||||
e2d_pos->lat = d3;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
converts 3d cartesian coordinate to 3d ellipsoidal
|
||||
*/
|
||||
void e3d_of_c3d( const enum type_ellipsoid elps, struct double_c3d c_pos,
|
||||
struct double_e3d *e_pos )
|
||||
{
|
||||
#define WGS_84_SMA 6378137.00
|
||||
#define WGS_84_SMB 6356752.31
|
||||
#define E2 ((WGS_84_SMA*WGS_84_SMA-WGS_84_SMB*WGS_84_SMB)/(WGS_84_SMA*WGS_84_SMA))
|
||||
#define PREC 0.000000000001
|
||||
double dist = sqrt( c_pos.x * c_pos.x + c_pos.y * c_pos.y );
|
||||
double v, old_phi, new_phi = atan2( ( double )c_pos.z, dist * ( 1 - E2 ) );
|
||||
do {
|
||||
old_phi = new_phi;
|
||||
v = WGS_84_SMA / sqrt( 1 - ( E2 * sin( old_phi ) * sin( old_phi ) ) );
|
||||
new_phi = atan2( ( ( double )c_pos.z + E2 * v * sin ( old_phi ) ), dist );
|
||||
} while ( fabs( new_phi - old_phi ) > PREC );
|
||||
e_pos->lat = new_phi;
|
||||
e_pos->lon = atan2( c_pos.y, c_pos.x );
|
||||
e_pos->h = ( dist / cos ( new_phi ) ) - v;
|
||||
}
|
||||
|
||||
|
||||
|
||||
193
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/logger.c
Normal file
193
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/logger.c
Normal file
@ -0,0 +1,193 @@
|
||||
/*
|
||||
logger.c
|
||||
Copyright (C) 2003 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 <sys/types.h>
|
||||
#include <dirent.h>
|
||||
#include <libgen.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
#include <sys/stat.h>
|
||||
#include <glib.h>
|
||||
|
||||
#include "logger.h"
|
||||
|
||||
//static GList* logger_parse_messages(struct PprzProtocol* protocol, xmlDocPtr doc, xmlNodePtr cur);
|
||||
static GList *logger_parse_message( struct PprzProtocol *protocol, GList *msgs,
|
||||
const guchar *buf );
|
||||
|
||||
struct Logger *logger_new( const gchar *out_file, xmlDocPtr doc )
|
||||
{
|
||||
const gchar *opening = "<data>\n";
|
||||
struct Logger *this = g_new( struct Logger, 1 );
|
||||
if ( this ) {
|
||||
GError *err = NULL;
|
||||
|
||||
gchar *copy = strdup( out_file );
|
||||
gchar *out_dir = dirname( copy );
|
||||
DIR *d;
|
||||
if ( !( d = opendir( out_dir ) ) ) {
|
||||
if ( errno == ENOENT ) {
|
||||
if ( mkdir( out_dir, S_IRWXU | S_IRWXG ) ) {
|
||||
g_message( "log directory %s doesn't exist and could not create it", out_dir );
|
||||
/* FIXME : FREE */
|
||||
return NULL;
|
||||
}
|
||||
} else {
|
||||
g_message( "could not acces log directory %s %s", out_dir,
|
||||
g_strerror( errno ) );
|
||||
/* FIXME : FREE */
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
closedir( d );
|
||||
g_free( copy );
|
||||
|
||||
xmlSaveFormatFile ( out_file, doc, 1 );
|
||||
/* FIXME : check result */
|
||||
|
||||
this->channel = g_io_channel_new_file( out_file, "a", &err );
|
||||
if ( err ) {
|
||||
g_message( "could not open log file %s : %s", out_file, err->message );
|
||||
g_error_free( err );
|
||||
g_free( this );
|
||||
return NULL;
|
||||
}
|
||||
g_io_channel_write_chars( this->channel, opening, strlen( opening ), NULL,
|
||||
NULL );
|
||||
}
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void logger_free( struct Logger *this )
|
||||
{
|
||||
const gchar *closing = "</data>\n</log>\n";
|
||||
g_io_channel_write_chars( this->channel, closing, strlen( closing ), NULL,
|
||||
NULL );
|
||||
g_io_channel_flush( this->channel, NULL );
|
||||
// g_io_channel_free(this->channel);
|
||||
g_free( this );
|
||||
}
|
||||
|
||||
void logger_log( struct Logger *this, struct PprzMsg *msg )
|
||||
{
|
||||
GString *str;
|
||||
pprz_protocol_ascii_of_msg( msg, &str );
|
||||
if ( str ) {
|
||||
GError *err = NULL;
|
||||
guint len;
|
||||
g_io_channel_write_chars ( this->channel, str->str, str->len, &len, &err );
|
||||
if ( err ) {
|
||||
g_warning( "log write failed %s", err->message );
|
||||
g_error_free( err );
|
||||
g_string_free( str, TRUE );
|
||||
return;
|
||||
}
|
||||
g_string_free( str, TRUE );
|
||||
g_io_channel_flush( this->channel, &err );
|
||||
if ( err ) {
|
||||
g_warning( "log flush failed %s", err->message );
|
||||
g_error_free( err );
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
GList *logger_parse_log( const gchar *filename,
|
||||
struct PprzProtocol **protocol )
|
||||
{
|
||||
xmlDocPtr doc;
|
||||
xmlNodePtr cur;
|
||||
|
||||
GList *msgs = NULL;
|
||||
const gchar *protocol_closure = "</protocol>";
|
||||
gchar *contents, *data_start;
|
||||
gsize len;
|
||||
GError *err;
|
||||
gboolean res = g_file_get_contents( filename, &contents, &len, &err );
|
||||
|
||||
if ( !res ) {
|
||||
g_message( "could not read file %s : %s", filename, err->message );
|
||||
g_error_free( err );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( !( data_start = g_strstr_len( contents, len, protocol_closure ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "protocol closing not found" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
data_start += strlen( protocol_closure );
|
||||
|
||||
if ( !( doc = xmlParseMemory( contents, data_start - contents ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "error parsing protocol xml" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( !( cur = xmlDocGetRootElement( doc ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "empty protocol" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( !( *protocol = pprz_protocol_new_from_xml( doc, cur ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "error interpreting protocol" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
msgs = logger_parse_message( *protocol, msgs, data_start );
|
||||
g_free( contents );
|
||||
|
||||
return msgs;
|
||||
}
|
||||
|
||||
/* static GList* logger_parse_messages(struct PprzProtocol* protocol, xmlDocPtr doc, xmlNodePtr cur) { */
|
||||
|
||||
/* if (!cur) return NULL; */
|
||||
/* if (xmlStrcmp(cur->name, (const xmlChar *) "data")) */
|
||||
/* return cur->next?logger_parse_messages(protocol, doc, cur->next):NULL; */
|
||||
/* cur = cur->xmlChildrenNode; */
|
||||
|
||||
/* if (xmlStrcmp(cur->name, (const xmlChar *) "text")) */
|
||||
/* return NULL; */
|
||||
|
||||
/* return logger_parse_message(protocol, NULL, cur->content); */
|
||||
/* } */
|
||||
|
||||
static GList *logger_parse_message( struct PprzProtocol *protocol, GList *msgs,
|
||||
const guchar *buf )
|
||||
{
|
||||
guchar *next;
|
||||
struct PprzMsg *msg = pprz_protocol_msg_new_of_ascii( protocol, buf );
|
||||
if ( msg )
|
||||
msgs = g_list_append( msgs, msg );
|
||||
if ( ( next = index( buf, '\n' ) ) )
|
||||
return logger_parse_message( protocol, msgs, next + 1 );
|
||||
return msgs;
|
||||
}
|
||||
93
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/math.c
Normal file
93
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/math.c
Normal file
@ -0,0 +1,93 @@
|
||||
/*
|
||||
Math library emulation functions
|
||||
|
||||
Copyright (C) 2007 IRIT - UPS <casse@irit.fr>
|
||||
|
||||
This file is part of papabench.
|
||||
|
||||
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 papabench; see the file COPYING. If not, write to
|
||||
the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
Boston, MA 02111-1307, USA.
|
||||
|
||||
*/
|
||||
#include <math.h>
|
||||
|
||||
double pp_atan2( double x, double y )
|
||||
{
|
||||
double coeff_1 = M_PI / 4;
|
||||
double coeff_2 = 3 * coeff_1;
|
||||
double abs_y = fabs( y ) + 1e-10;
|
||||
double angle, r;
|
||||
if ( x > 0 ) {
|
||||
r = ( x - abs_y ) / ( x + abs_y );
|
||||
angle = coeff_1 - coeff_1 * r;
|
||||
} else {
|
||||
r = ( x + abs_y ) / ( abs_y - x );
|
||||
angle = coeff_2 - coeff_1 * r;
|
||||
}
|
||||
if ( y < 0 )
|
||||
return ( -angle );
|
||||
else
|
||||
return angle;
|
||||
}
|
||||
|
||||
|
||||
/* Calculates sin(x), angle x must be in rad.
|
||||
Range: -pi/2 <= x <= pi/2
|
||||
Precision: +/- .000,000,005
|
||||
*/
|
||||
|
||||
double pp_sin( double x )
|
||||
{
|
||||
double xi, y, q, q2;
|
||||
int sign;
|
||||
|
||||
xi = x;
|
||||
sign = 1;
|
||||
_Pragma( "loopbound min 0 max 0" )
|
||||
while ( xi < -1.57079632679489661923 ) xi += 6.28318530717958647692;
|
||||
_Pragma( "loopbound min 0 max 0" )
|
||||
while ( xi > 4.71238898038468985769 ) xi -= 6.28318530717958647692;
|
||||
if ( xi > 1.57079632679489661923 ) {
|
||||
xi -= 3.141592653589793238462643;
|
||||
sign = -1;
|
||||
}
|
||||
q = xi / 1.57079632679;
|
||||
q2 = q * q;
|
||||
y = ( ( ( ( .00015148419 * q2
|
||||
- .00467376557 ) * q2
|
||||
+ .07968967928 ) * q2
|
||||
- .64596371106 ) * q2
|
||||
+ 1.57079631847 ) * q;
|
||||
return ( sign < 0 ? -y : y );
|
||||
}
|
||||
|
||||
#define SQRT_PRECISION 5
|
||||
double pp_sqrt( double n )
|
||||
{
|
||||
#if 0
|
||||
float x, m;
|
||||
int i, e;
|
||||
|
||||
/* compute the approximation */
|
||||
m = frexpf( n, &e );
|
||||
x = ldexp( m, e / 2 );
|
||||
|
||||
/* perform the computation */
|
||||
_Pragma( "loopbound min 5 max 5 )
|
||||
for ( i = 0; i < SQRT_PRECISION; i++ )
|
||||
x = ( x + n / x ) / 2;
|
||||
return x;
|
||||
#endif
|
||||
}
|
||||
192
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/network.c
Normal file
192
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/network.c
Normal file
@ -0,0 +1,192 @@
|
||||
/*
|
||||
$id:$
|
||||
Copyright (C) 2003 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
network functions
|
||||
*/
|
||||
|
||||
#include "network.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <netdb.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <sys/socket.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
static gboolean server_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data );
|
||||
static gboolean client_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data );
|
||||
|
||||
struct NetworkServer *network_server_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data )
|
||||
{
|
||||
struct NetworkServer *this = g_new( struct NetworkServer, 1 );
|
||||
if ( this ) {
|
||||
GIOChannel *channel;
|
||||
struct protoent *pte = getprotobyname( "UDP" );
|
||||
static int so_broadcast = TRUE;
|
||||
|
||||
this->bcast_socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto );
|
||||
setsockopt( this->bcast_socket, SOL_SOCKET, SO_BROADCAST, &so_broadcast,
|
||||
sizeof( so_broadcast ) );
|
||||
|
||||
this->bcast_addr.sin_family = PF_INET;
|
||||
this->bcast_addr.sin_port = htons( bcast_port );
|
||||
this->bcast_addr.sin_addr.s_addr = inet_addr( bcast_addr );
|
||||
|
||||
channel = g_io_channel_unix_new( this->bcast_socket );
|
||||
g_io_add_watch ( channel, G_IO_IN | G_IO_PRI, server_on_data_received, this );
|
||||
|
||||
this->protocol = protocol;
|
||||
|
||||
this->rcv_callback = rcv_callback;
|
||||
this->err_callback = err_callback;
|
||||
this->user_data = user_data;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
#define FIXME_LEN 2048
|
||||
|
||||
void network_server_dispatch( struct NetworkServer *this, struct PprzMsg *msg )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *type = buf + sizeof( GTimeVal );
|
||||
guchar *data = type + 1;
|
||||
guint len = sizeof( GTimeVal ) + msg->class->size;
|
||||
*date = msg->date;
|
||||
*type = msg->class->id;
|
||||
memcpy( data, msg->bytes, msg->class->size - 1 );
|
||||
if ( sendto( this->bcast_socket, buf, len, 0,
|
||||
( struct sockaddr * )&this->bcast_addr, sizeof( this->bcast_addr ) ) == -1 )
|
||||
g_message( "error broadcasting" );
|
||||
}
|
||||
|
||||
struct NetworkClient *network_client_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data )
|
||||
{
|
||||
struct NetworkClient *this = g_new( struct NetworkClient, 1 );
|
||||
if ( this ) {
|
||||
int so_reuseaddr = TRUE;
|
||||
GIOChannel *channel;
|
||||
struct protoent *pte = getprotobyname( "UDP" );
|
||||
|
||||
this->socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto );
|
||||
setsockopt( this->socket, SOL_SOCKET, SO_REUSEADDR, &so_reuseaddr,
|
||||
sizeof( so_reuseaddr ) );
|
||||
this->bcast_addr.sin_family = PF_INET;
|
||||
this->bcast_addr.sin_port = htons( bcast_port );
|
||||
this->bcast_addr.sin_addr.s_addr = inet_addr( bcast_addr );
|
||||
bind( this->socket, ( struct sockaddr * )&this->bcast_addr,
|
||||
sizeof( this->bcast_addr ) );
|
||||
|
||||
channel = g_io_channel_unix_new( this->socket );
|
||||
g_io_add_watch ( channel, G_IO_IN | G_IO_PRI, client_on_data_received, this );
|
||||
|
||||
this->protocol = protocol;
|
||||
|
||||
this->rcv_callback = rcv_callback;
|
||||
this->err_callback = err_callback;
|
||||
this->user_data = user_data;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
void network_client_send_to_server( struct NetworkClient *this,
|
||||
struct PprzMsg *msg )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *type = buf + sizeof( GTimeVal );
|
||||
guchar *data = type + 1;
|
||||
guint len = sizeof( GTimeVal ) + msg->class->size;
|
||||
*date = msg->date;
|
||||
*type = msg->class->id;
|
||||
memcpy( data, msg->bytes, msg->class->size - 1 );
|
||||
if ( sendto( this->socket, buf, len, 0,
|
||||
( struct sockaddr * )&this->bcast_addr, sizeof( this->bcast_addr ) ) == -1 )
|
||||
g_message( "error sending to server" );
|
||||
}
|
||||
|
||||
static gboolean server_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
struct sockaddr_in from;
|
||||
int from_len;
|
||||
gint len;
|
||||
struct NetworkServer *this = ( struct NetworkServer * )data;
|
||||
if ( ( len = recvfrom( this->bcast_socket, buf, FIXME_LEN, 0,
|
||||
( struct sockaddr * )&from, &from_len ) ) == -1 ) {
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return FALSE;
|
||||
} else {
|
||||
struct PprzMsg *msg;
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *ptr_type = buf + sizeof( GTimeVal );
|
||||
if ( !( msg = pprz_protocol_msg_new_of_bin( this->protocol, ptr_type,
|
||||
*date ) ) ) {
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return TRUE;
|
||||
}
|
||||
this->rcv_callback( msg, this->user_data );
|
||||
// g_message("network data received %s %d", inet_ntoa(from.sin_addr), ntohs(from.sin_port));
|
||||
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static gboolean client_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
struct sockaddr_in from;
|
||||
int from_len;
|
||||
gint len;
|
||||
struct NetworkClient *this = ( struct NetworkClient * )data;
|
||||
if ( ( len = recvfrom( this->socket, buf, FIXME_LEN, 0,
|
||||
( struct sockaddr * )&from, &from_len ) ) == -1 ) {
|
||||
g_message( "client_on_data_received [ %s ]", strerror( errno ) );
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return TRUE;
|
||||
} else {
|
||||
struct PprzMsg *msg;
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *ptr_type = buf + sizeof( GTimeVal );
|
||||
if ( !( msg = pprz_protocol_msg_new_of_bin( this->protocol, ptr_type,
|
||||
*date ) ) ) {
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return TRUE;
|
||||
}
|
||||
this->rcv_callback( msg, this->user_data );
|
||||
// g_message("network data received %s %d", inet_ntoa(from.sin_addr), ntohs(from.sin_port));
|
||||
}
|
||||
this->server_addr = from;
|
||||
return TRUE;
|
||||
}
|
||||
551
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/protocol.c
Normal file
551
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/protocol.c
Normal file
@ -0,0 +1,551 @@
|
||||
/*
|
||||
$Id: protocol.c,v 1.1 2011-01-18 12:48:45 moellmer Exp $
|
||||
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 "protocol.h"
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
struct PprzTypInfo {
|
||||
gchar *xml_name;
|
||||
guint size;
|
||||
gchar *default_format;
|
||||
};
|
||||
|
||||
const struct PprzTypInfo type_info[ TYP_NB ] = {
|
||||
{"uint8", 1, "%hhu"},
|
||||
{"uint16", 2, "%hu" },
|
||||
{"uint32", 4, "%u" },
|
||||
{"int8", 1, "%hhd"},
|
||||
{"int16", 2, "%hd" },
|
||||
{"int32", 4, "%d" },
|
||||
{"float", 4, "%f" },
|
||||
{"uint8_array", 1, "%0x"}
|
||||
};
|
||||
|
||||
static gboolean parse_xml_msg( struct PprzProtocol *this, xmlNodePtr ptr );
|
||||
static gboolean parse_xml_field( struct PprzMsgClass *msg_class,
|
||||
xmlNodePtr ptr );
|
||||
static void ascii_of_fields( struct PprzMsg *msg, GList *fields_class,
|
||||
GString *buf );
|
||||
static gboolean field_of_ascii( GList *fields_class, gchar *line,
|
||||
struct PprzMsg *msg );
|
||||
|
||||
static enum PprzFieldType type_of_string( const gchar *type_str );
|
||||
|
||||
static struct PprzMsgClass *msg_class_of_name( struct PprzProtocol *this,
|
||||
const gchar *name );
|
||||
static struct PprzMsgClass *msg_class_of_id( struct PprzProtocol *this,
|
||||
guchar id );
|
||||
|
||||
static struct PprzFieldClass *field_class_of_name( struct PprzMsg *msg,
|
||||
const gchar *name );
|
||||
/* static struct PprzFieldClass* field_class_of_id(struct PprzMsg* msg, guchar id); */
|
||||
|
||||
struct PprzProtocol *pprz_protocol_new_from_xml( xmlDocPtr doc,
|
||||
xmlNodePtr cur )
|
||||
{
|
||||
struct PprzProtocol *this;
|
||||
if ( !cur ) return NULL; /* empty document */
|
||||
if ( xmlStrcmp( cur->name,
|
||||
( const xmlChar * ) "protocol" ) ) /* not protocol */
|
||||
return cur->next ? pprz_protocol_new_from_xml( doc, cur->next ) : NULL;
|
||||
this = g_new( struct PprzProtocol, 1 );
|
||||
this->msgs_classes = NULL;
|
||||
this->msgs_classes_by_name = g_hash_table_new( g_str_hash, g_str_equal );
|
||||
if ( !parse_xml_msg( this, cur->xmlChildrenNode ) ) {
|
||||
g_free( this );
|
||||
return NULL;
|
||||
}
|
||||
this->nb_msgs_classes = g_list_length( this->msgs_classes );
|
||||
this->msgs_classes_by_id = g_new( struct PprzMsgClass *,
|
||||
this->nb_msgs_classes );
|
||||
{
|
||||
GList *cur = this->msgs_classes;
|
||||
guint i = 0;
|
||||
while ( cur ) {
|
||||
this->msgs_classes_by_id[ i ] = cur->data;
|
||||
i++;
|
||||
cur = cur->next;
|
||||
}
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
const gchar *pprz_protocol_str_of_field_type( enum PprzFieldType type )
|
||||
{
|
||||
return type_info[ type ].xml_name;
|
||||
}
|
||||
|
||||
const guint pprz_protocol_size_of_field_type( enum PprzFieldType type )
|
||||
{
|
||||
return type_info[ type ].size;
|
||||
}
|
||||
|
||||
static enum PprzFieldType type_of_string( const gchar *type_str )
|
||||
{
|
||||
guint ret = 0;
|
||||
while ( ret < TYP_NB && strcmp( type_info[ ret ].xml_name, type_str ) )
|
||||
ret++;
|
||||
return ret;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_message_new( struct PprzMsgClass *msg_class,
|
||||
GTimeVal date )
|
||||
{
|
||||
struct PprzMsg *this = g_new( struct PprzMsg, 1 );
|
||||
this->date = date;
|
||||
this->class = msg_class;
|
||||
this->bytes = g_new( guchar, this->class->size - 1 );
|
||||
return this;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_id( struct PprzProtocol *this,
|
||||
guchar id, GTimeVal date )
|
||||
{
|
||||
struct PprzMsgClass *msg_class = msg_class_of_id( this, id );
|
||||
if ( msg_class ) return pprz_message_new( msg_class, date );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_name( struct PprzProtocol *this,
|
||||
const gchar *name, GTimeVal date )
|
||||
{
|
||||
struct PprzMsgClass *msg_class = msg_class_of_name( this, name );
|
||||
if ( msg_class ) return pprz_message_new( msg_class, date );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_bin( struct PprzProtocol *this,
|
||||
const guchar *buf, GTimeVal date )
|
||||
{
|
||||
struct PprzMsg *msg = pprz_protocol_msg_new_by_id( this, buf[ 0 ], date );
|
||||
if ( msg )
|
||||
memcpy( msg->bytes, buf + 1, msg->class->size - 1 );
|
||||
return msg;
|
||||
}
|
||||
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_ascii( struct PprzProtocol *this,
|
||||
const gchar *line )
|
||||
{
|
||||
gchar *kw;
|
||||
GTimeVal date;
|
||||
struct PprzMsg *msg;
|
||||
if ( sscanf( line, "%ld.%06ld %as", &date.tv_sec, &date.tv_usec, &kw ) != 3 )
|
||||
return NULL;
|
||||
if ( !( msg = pprz_protocol_msg_new_by_name( this, kw, date ) ) )
|
||||
return NULL;
|
||||
if ( msg->class->fields_classes ) { // message has arguments
|
||||
gchar *space = index( line, ' ' );
|
||||
g_assert( space );
|
||||
g_assert( space + 1 ); // checked before by sscanf
|
||||
space = index( space + 1, ' ' );
|
||||
g_assert( space ); // checked before by sscanf
|
||||
if ( !field_of_ascii( msg->class->fields_classes, space, msg ) ) {
|
||||
pprz_msg_free( msg );
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
void pprz_msg_free( struct PprzMsg *this )
|
||||
{
|
||||
g_free( this->bytes );
|
||||
g_free( this );
|
||||
}
|
||||
|
||||
gboolean pprz_msg_set_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, gconstpointer value )
|
||||
{
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (field_class->type) {
|
||||
case TYP_UINT_8:
|
||||
(guint8*)(msg->bytes+field_class->offset) = *(guint8*)value;
|
||||
break;
|
||||
case TYP_UINT_16:
|
||||
(guint16*)(msg->bytes+field_class->offset) = *(guint16*)value;
|
||||
break;
|
||||
case TYP_UINT_32:
|
||||
(guint32*)(msg->bytes+field_class->offset) = *(guint32*)value;
|
||||
break;
|
||||
case TYP_INT_8:
|
||||
(gint8*)(msg->bytes+field_class->offset) = *(gint8*)value;
|
||||
break;
|
||||
case TYP_INT_16:
|
||||
(gint16*)(msg->bytes+field_class->offset) = *(gint16*)value;
|
||||
break;
|
||||
case TYP_INT_32:
|
||||
(gint32*)(msg->bytes+field_class->offset) = *(gint32*)value;
|
||||
break;
|
||||
case TYP_FLOAT:
|
||||
(gfloat*)(msg->bytes+field_class->offset) = *(gfloat*)value;
|
||||
break;
|
||||
case TYP_ARRAY_UINT_8:
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
default:
|
||||
g_warning("in ascii of field : unknown type");
|
||||
}
|
||||
#else*/
|
||||
if ( field_class->type == TYP_UINT_8 ) {
|
||||
*( guint8 * )( msg->bytes + field_class->offset ) = *( guint8 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_16 ) {
|
||||
*( guint16 * )( msg->bytes + field_class->offset ) = *( guint16 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_32 ) {
|
||||
*( guint32 * )( msg->bytes + field_class->offset ) = *( guint32 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_8 ) {
|
||||
*( gint8 * )( msg->bytes + field_class->offset ) = *( gint8 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_16 ) {
|
||||
*( gint16 * )( msg->bytes + field_class->offset ) = *( gint16 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_32 ) {
|
||||
*( gint32 * )( msg->bytes + field_class->offset ) = *( gint32 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_FLOAT ) {
|
||||
*( gfloat * )( msg->bytes + field_class->offset ) = *( gfloat * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_ARRAY_UINT_8 ) {
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
} else
|
||||
g_warning( "in ascii of field : unknown type" );
|
||||
//#endif
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
gboolean pprz_msg_set_field_by_name( struct PprzMsg *msg, const gchar *name,
|
||||
gconstpointer value )
|
||||
{
|
||||
struct PprzFieldClass *field_class = field_class_of_name( msg, name );
|
||||
if ( field_class )
|
||||
return pprz_msg_set_field( msg, field_class, value );
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
gboolean pprz_msg_set_field_by_id( struct PprzMsg *msg, guint id,
|
||||
gconstpointer value )
|
||||
{
|
||||
GList *cell = g_list_nth( msg->class->fields_classes, id );
|
||||
if ( cell )
|
||||
return pprz_msg_set_field( msg, cell->data, value );
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
gpointer pprz_msg_get_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class )
|
||||
{
|
||||
return msg->bytes + field_class->offset;
|
||||
}
|
||||
|
||||
gpointer pprz_msg_get_field_by_name( struct PprzMsg *msg, const gchar *name )
|
||||
{
|
||||
struct PprzFieldClass *field_class = field_class_of_name( msg, name );
|
||||
if ( field_class )
|
||||
return pprz_msg_get_field( msg, field_class );
|
||||
g_message( "pprz_msg_get_field_by_name : unknown field [ %s ] in msg [ %s ]", name,
|
||||
msg->class->name );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define DEFAULT_LINE_LEN 256
|
||||
void pprz_protocol_ascii_of_msg( struct PprzMsg *msg, GString **buf )
|
||||
{
|
||||
*buf = g_string_sized_new( DEFAULT_LINE_LEN );
|
||||
g_string_printf( *buf, "%ld.%06ld %s ", msg->date.tv_sec, msg->date.tv_usec,
|
||||
msg->class->name );
|
||||
if ( msg->class->fields_classes ) ascii_of_fields( msg,
|
||||
msg->class->fields_classes, *buf );
|
||||
g_string_append_printf( *buf, "\n" );
|
||||
}
|
||||
|
||||
void pprz_msg_ascii_of_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, GString *buf )
|
||||
{
|
||||
guchar *format;
|
||||
guchar *data;
|
||||
format = field_class->format;
|
||||
if ( !format ) format = type_info[ field_class->type ].default_format;
|
||||
data = msg->bytes + field_class->offset;
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (field_class->type) {
|
||||
case TYP_UINT_8:
|
||||
g_string_append_printf(buf, format, *((guint8*)data));
|
||||
break;
|
||||
case TYP_UINT_16:
|
||||
g_string_append_printf(buf, format, *((guint16*)data));
|
||||
break;
|
||||
case TYP_UINT_32:
|
||||
g_string_append_printf(buf, format, *((guint32*)data));
|
||||
break;
|
||||
case TYP_INT_8:
|
||||
g_string_append_printf(buf, format, *((gint8*)data));
|
||||
break;
|
||||
case TYP_INT_16:
|
||||
g_string_append_printf(buf, format, *((gint16*)data));
|
||||
break;
|
||||
case TYP_INT_32:
|
||||
g_string_append_printf(buf, format, *((gint32*)data));
|
||||
break;
|
||||
case TYP_FLOAT:
|
||||
g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
case TYP_ARRAY_UINT_8:
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
default:
|
||||
g_warning("in ascii of field : unknown type");
|
||||
}
|
||||
#else*/
|
||||
if ( field_class->type == TYP_UINT_8 ) {
|
||||
g_string_append_printf( buf, format, *( ( guint8 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_16 ) {
|
||||
g_string_append_printf( buf, format, *( ( guint16 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_32 ) {
|
||||
g_string_append_printf( buf, format, *( ( guint32 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_8 ) {
|
||||
g_string_append_printf( buf, format, *( ( gint8 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_16 ) {
|
||||
g_string_append_printf( buf, format, *( ( gint16 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_32 ) {
|
||||
g_string_append_printf( buf, format, *( ( gint32 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_FLOAT ) {
|
||||
g_string_append_printf( buf, format, *( ( gfloat * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_ARRAY_UINT_8 ) {
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
} else
|
||||
g_warning( "in ascii of field : unknown type" );
|
||||
//#endif
|
||||
}
|
||||
|
||||
|
||||
static gboolean field_of_ascii( GList *fields_classes, gchar *line,
|
||||
struct PprzMsg *msg )
|
||||
{
|
||||
struct PprzFieldClass *field_class = ( struct PprzFieldClass * )
|
||||
fields_classes->data;
|
||||
gchar *end_ptr;
|
||||
gchar *ptr_field = msg->bytes + field_class->offset;
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (field_class->type) {
|
||||
case TYP_UINT_8:
|
||||
((guint8*)ptr_field) = (guint8)strtoul(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_UINT_16:
|
||||
((guint16*)ptr_field) = (guint16)strtoul(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_UINT_32:
|
||||
((guint32*)ptr_field) = (guint32)strtoul(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_INT_8:
|
||||
((gint8*)ptr_field) = (gint8)strtol(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_INT_16:
|
||||
((gint16*)ptr_field) = (gint16)strtol(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_INT_32:
|
||||
((gint32*)ptr_field) = (gint32)strtol(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_FLOAT:
|
||||
((gfloat*)ptr_field) = (gfloat)strtod(line, &end_ptr);
|
||||
break;
|
||||
case TYP_ARRAY_UINT_8:
|
||||
break;
|
||||
default:
|
||||
printf("in field of ascii: unknown type\n");
|
||||
}
|
||||
#else*/
|
||||
if ( field_class->type == TYP_UINT_8 ) {
|
||||
*( ( guint8 * )ptr_field ) = ( guint8 )strtoul( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_16 ) {
|
||||
*( ( guint16 * )ptr_field ) = ( guint16 )strtoul( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_32 ) {
|
||||
*( ( guint32 * )ptr_field ) = ( guint32 )strtoul( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_8 ) {
|
||||
*( ( gint8 * )ptr_field ) = ( gint8 )strtol( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_16 ) {
|
||||
*( ( gint16 * )ptr_field ) = ( gint16 )strtol( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_32 ) {
|
||||
*( ( gint32 * )ptr_field ) = ( gint32 )strtol( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_FLOAT ) {
|
||||
*( ( gfloat * )ptr_field ) = ( gfloat )strtod( line, &end_ptr );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_ARRAY_UINT_8 )
|
||||
break;
|
||||
else
|
||||
printf( "in field of ascii: unknown type\n" );
|
||||
//#endif
|
||||
if ( fields_classes->next )
|
||||
return field_of_ascii( fields_classes->next, end_ptr, msg );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void ascii_of_fields( struct PprzMsg *msg, GList *fields_class,
|
||||
GString *buf )
|
||||
{
|
||||
struct PprzFieldClass *field_class = ( struct PprzFieldClass * )
|
||||
fields_class->data;
|
||||
pprz_msg_ascii_of_field( msg, field_class, buf );
|
||||
if ( fields_class->next ) {
|
||||
g_string_append_printf( buf, " " );
|
||||
ascii_of_fields( msg, fields_class->next, buf );
|
||||
}
|
||||
}
|
||||
|
||||
static gboolean parse_xml_msg( struct PprzProtocol *this, xmlNodePtr ptr )
|
||||
{
|
||||
struct PprzMsgClass *new_msg_class;
|
||||
if ( !xmlNodeIsText ( ptr ) ) {
|
||||
if ( xmlStrcmp( ptr->name, ( const xmlChar * )"message" ) ) /* not a message */
|
||||
return FALSE;
|
||||
new_msg_class = g_new( struct PprzMsgClass, 1 );
|
||||
new_msg_class->id = g_list_length( this->msgs_classes );
|
||||
new_msg_class->name = xmlGetProp( ptr, "id" );
|
||||
new_msg_class->fields_classes = NULL;
|
||||
new_msg_class->fields_classes_by_name = g_hash_table_new( g_str_hash,
|
||||
g_str_equal );
|
||||
new_msg_class->size = 1;
|
||||
this->msgs_classes = g_list_append( this->msgs_classes, new_msg_class );
|
||||
g_hash_table_insert( this->msgs_classes_by_name, new_msg_class->name,
|
||||
new_msg_class );
|
||||
if ( ptr->xmlChildrenNode &&
|
||||
!parse_xml_field( new_msg_class, ptr->xmlChildrenNode ) )
|
||||
/* fixme : free space */
|
||||
return FALSE;
|
||||
new_msg_class->nb_fields_classes = g_list_length(
|
||||
new_msg_class->fields_classes );
|
||||
new_msg_class->fields_classes_by_id = g_new( struct PprzFieldClass *,
|
||||
new_msg_class->nb_fields_classes );
|
||||
{
|
||||
GList *cur = new_msg_class->fields_classes;
|
||||
guint i = 0;
|
||||
while ( cur ) {
|
||||
new_msg_class->fields_classes_by_id[ i ] = cur->data;
|
||||
i++;
|
||||
cur = cur->next;
|
||||
}
|
||||
}
|
||||
}
|
||||
if ( ptr->next )
|
||||
return parse_xml_msg( this, ptr->next );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static gboolean parse_xml_field( struct PprzMsgClass *msg_class,
|
||||
xmlNodePtr ptr )
|
||||
{
|
||||
struct PprzFieldClass *new_field_class;
|
||||
xmlChar *type_str;
|
||||
if ( !xmlNodeIsText ( ptr ) ) {
|
||||
if ( xmlStrcmp( ptr->name, ( const xmlChar * )"field" ) ) /* not a field */
|
||||
return FALSE;
|
||||
new_field_class = g_new( struct PprzFieldClass, 1 );
|
||||
new_field_class->name = xmlGetProp( ptr, "id" );
|
||||
new_field_class->format = xmlGetProp( ptr, "format" );
|
||||
new_field_class->unit = xmlGetProp( ptr, "unit" );
|
||||
new_field_class->description = xmlGetProp( ptr, "description" );
|
||||
type_str = xmlGetProp( ptr, "type" );
|
||||
if ( ( new_field_class->type = type_of_string( type_str ) ) >= TYP_NB ) {
|
||||
/* fixme : free space */
|
||||
return FALSE;
|
||||
}
|
||||
xmlFree( type_str );
|
||||
msg_class->fields_classes = g_list_append( msg_class->fields_classes,
|
||||
new_field_class );
|
||||
g_hash_table_insert( msg_class->fields_classes_by_name, new_field_class->name,
|
||||
new_field_class );
|
||||
new_field_class->offset = msg_class->size - 1;
|
||||
msg_class->size += type_info[ new_field_class->type ].size;
|
||||
}
|
||||
if ( ptr->next )
|
||||
/* check return and free space */
|
||||
return parse_xml_field( msg_class, ptr->next );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
static struct PprzMsgClass *msg_class_of_name( struct PprzProtocol *this,
|
||||
const gchar *name )
|
||||
{
|
||||
return g_hash_table_lookup( this->msgs_classes_by_name, name );
|
||||
}
|
||||
|
||||
static struct PprzMsgClass *msg_class_of_id( struct PprzProtocol *this,
|
||||
guchar id )
|
||||
{
|
||||
if ( id >= this->nb_msgs_classes )
|
||||
return NULL;
|
||||
return this->msgs_classes_by_id[ id ];
|
||||
}
|
||||
|
||||
static struct PprzFieldClass *field_class_of_name( struct PprzMsg *msg,
|
||||
const gchar *name )
|
||||
{
|
||||
return g_hash_table_lookup( msg->class->fields_classes_by_name, name );
|
||||
}
|
||||
|
||||
/* static struct PprzFieldClass* field_class_of_id(struct PprzMsg* msg, guchar id) { */
|
||||
/* GList* cell = g_list_nth(msg->class->fields_classes, id); */
|
||||
/* return cell?cell->data:NULL; */
|
||||
/* } */
|
||||
52
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/traces.c
Normal file
52
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/traces.c
Normal file
@ -0,0 +1,52 @@
|
||||
/*
|
||||
traces.c
|
||||
Copyright (C) 2003 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 "traces.h"
|
||||
|
||||
#ifdef TRACE_ON
|
||||
unsigned long TraceLevel = TRACE_ERROR | TRACE_TRANSPORT
|
||||
; // | TRACE_MODEM_READ ;// | TRACE_DATA_READ | TRACE_DOWNLINK; //| TRACE_MODEM_READ;// ;//| TRACE_MSG// | TRACE_DOWNLINK;
|
||||
|
||||
// unsigned long TraceLevel = TRACE_DOWNLINK | TRACE_MSG | TRACE_ERROR ;
|
||||
|
||||
const char *print_hex( const unsigned char *buf, unsigned int len )
|
||||
{
|
||||
const char d2h[ ] = "0123456789ABCDEF";
|
||||
int i = 0;
|
||||
#define HEX_BUF_LEN 1024
|
||||
#define FMT_LEN 3
|
||||
#define FMT "%c%c "
|
||||
static unsigned char priv_buf[ HEX_BUF_LEN ];
|
||||
/* printf("[ %d ] ", len); */
|
||||
for ( i = 0; i < len && i < HEX_BUF_LEN / FMT_LEN; i++ ) {
|
||||
// printf(FMT, d2h[ (int)buf[ i ]/16 ], d2h[ (int)buf[ i ]%16 ]);
|
||||
sprintf( priv_buf + FMT_LEN * i, FMT, d2h[ ( int )buf[ i ] / 16 ],
|
||||
d2h[ ( int )buf[ i ] % 16 ] );
|
||||
}
|
||||
if ( len == 0 )
|
||||
priv_buf[ 0 ] = '\0';
|
||||
return priv_buf;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
244
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/transport.c
Normal file
244
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/transport.c
Normal file
@ -0,0 +1,244 @@
|
||||
#include "transport.h"
|
||||
|
||||
|
||||
#include "geometry.h"
|
||||
#include "traces.h"
|
||||
|
||||
static void parse_buf( struct Transport *this );
|
||||
static gboolean check_checksum( struct Transport *this, guint msg_len );
|
||||
static void remove_n_from_buf( struct Transport *this, guint len );
|
||||
static void remove_until_stx( struct Transport *this );
|
||||
|
||||
struct Transport *transport_new( gboolean fixed_size,
|
||||
gboolean two_bytes_checksum, guint nb_msg,
|
||||
guint *size_msg, guint max_msg_size,
|
||||
guchar stx, guchar etx,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data )
|
||||
{
|
||||
struct Transport *this = g_new( struct Transport, 1 );
|
||||
if ( this ) {
|
||||
this->stx = stx;
|
||||
this->etx = etx;
|
||||
this->err_callback = err_callback;
|
||||
this->msg_callback = msg_callback;
|
||||
this->callback_data = callback_data;
|
||||
this->buf_len = 0;
|
||||
this->fixed_size = fixed_size;
|
||||
this->two_bytes_checksum = two_bytes_checksum;
|
||||
this->quark = g_quark_from_string( "Transport" );
|
||||
g_get_current_time( &this->start_date );
|
||||
if ( fixed_size ) {
|
||||
this->nb_msg_type = nb_msg;
|
||||
this->size_msg = size_msg;
|
||||
//this->max_msg_size = FIXME find max size in array;;
|
||||
} else
|
||||
this->max_msg_size = max_msg_size;
|
||||
this->nb_byte_last_status = 0;
|
||||
this->nb_msg_last_status = 0;
|
||||
this->status.nb_byte = 0;
|
||||
this->status.nb_msg = 0;
|
||||
this->status.nb_err = 0;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
#define GDOUBLE_FROM_DIF_TV(a,b) ((gdouble)(a.tv_sec-b.tv_sec) + 1e-6 * (gdouble)(a.tv_usec-b.tv_usec))
|
||||
|
||||
struct TransportStatus *transport_get_status( struct Transport *this )
|
||||
{
|
||||
GTimeVal now;
|
||||
gdouble duration;
|
||||
|
||||
g_get_current_time( &now );
|
||||
this->status.run_time = DELAY_SEC_OF_TIMEVAL( now, this->start_date );
|
||||
|
||||
duration = GDOUBLE_FROM_DIF_TV( now, this->last_status_date );
|
||||
this->status.byte_rate = this->nb_byte_last_status / duration;
|
||||
this->status.msg_rate = this->nb_msg_last_status / duration;
|
||||
|
||||
this->nb_byte_last_status = 0;
|
||||
this->nb_msg_last_status = 0;
|
||||
this->last_status_date = now;
|
||||
|
||||
return &this->status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void transport_free( struct Transport *this )
|
||||
{
|
||||
g_free( this );
|
||||
}
|
||||
|
||||
void transport_feed_data( struct Transport *this, const guchar *buf,
|
||||
guint len )
|
||||
{
|
||||
if ( len >= TRANSPORT_BUF_LEN - this->buf_len ) {
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_BUF_OVFW, /* guint code */
|
||||
"buffer overflow" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
} else {
|
||||
this->status.nb_byte += len;
|
||||
this->nb_byte_last_status += len;
|
||||
memcpy( this->buf + this->buf_len, buf, len );
|
||||
this->buf_len += len;
|
||||
parse_buf( this );
|
||||
}
|
||||
}
|
||||
|
||||
static gboolean check_checksum( struct Transport *this, guint msg_len )
|
||||
{
|
||||
guint checksum_idx = msg_len - TRANSPORT_TAIL_LEN;
|
||||
guint i;
|
||||
if ( this->two_bytes_checksum ) {
|
||||
guchar cka = 0, ckb = 0;
|
||||
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ ) {
|
||||
cka += this->buf[ i ];
|
||||
ckb += cka;
|
||||
}
|
||||
return ( cka == this->buf[ checksum_idx ] &&
|
||||
ckb == this->buf[ checksum_idx + 1 ] );
|
||||
} else {
|
||||
guchar checksum = 0;
|
||||
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ )
|
||||
checksum ^= this->buf[ i ];
|
||||
if ( checksum != this->buf[ checksum_idx ] )
|
||||
TRACE( TRACE_ERROR, "transport checksum error (found : 0x%02X read : 0x%02X)\n",
|
||||
checksum, this->buf[ checksum_idx ] );
|
||||
return checksum == this->buf[ checksum_idx ];
|
||||
}
|
||||
}
|
||||
|
||||
static void remove_n_from_buf( struct Transport *this, guint len )
|
||||
{
|
||||
memmove( this->buf, this->buf + len, this->buf_len - len );
|
||||
this->buf_len -= len;
|
||||
}
|
||||
|
||||
static void remove_until_stx( struct Transport *this )
|
||||
{
|
||||
guchar *stx = memchr( this->buf, this->stx, this->buf_len );
|
||||
if ( stx ) {
|
||||
if ( stx != this->buf ) {
|
||||
memmove( this->buf, stx, this->buf_len - ( stx - this->buf ) );
|
||||
this->buf_len -= ( stx - this->buf );
|
||||
}
|
||||
} else
|
||||
this->buf_len = 0;
|
||||
}
|
||||
|
||||
static gboolean get_msg_len( struct Transport *this, guint *len )
|
||||
{
|
||||
if ( this->fixed_size ) {
|
||||
guint type = this->buf[ TRANSPORT_PAYLOAD_OFFSET ];
|
||||
if ( type >= this->nb_msg_type ) { /* BAP ID */
|
||||
TRACE( TRACE_TRANSPORT, "%d bad type\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
|
||||
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
||||
this->buf_len ) );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_INVALID_MSG_ID, /* guint code */
|
||||
"invalid msg id" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
return FALSE;
|
||||
} else *len = this->size_msg[ type ] + TRANSPORT_HEAD_LEN + TRANSPORT_TAIL_LEN;
|
||||
} else {
|
||||
*len = this->buf[ TRANSPORT_PAYLOAD_OFFSET ] + TRANSPORT_HEAD_LEN +
|
||||
TRANSPORT_TAIL_LEN;
|
||||
if ( *len >= this->max_msg_size ) { /* BAD LEN */
|
||||
TRACE( TRACE_TRANSPORT, "%d bad len\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_INVALID_MSG_LEN, /* guint code */
|
||||
"invalid message len" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
static void parse_buf( struct Transport *this )
|
||||
{
|
||||
/* make sure first byte in buffer is STX */
|
||||
remove_until_stx( this );
|
||||
// if (this->fixed_size)
|
||||
// TRACE(TRACE_TRANSPORT, "parse_buf %s\n", print_hex(this->buf, this->buf_len));
|
||||
if ( this->buf_len < TRANSPORT_PAYLOAD_OFFSET ) /* NO PAYLOAD */
|
||||
TRACE( TRACE_TRANSPORT_VERB, "no payload\n" );
|
||||
else {
|
||||
guint msg_len;
|
||||
if ( !get_msg_len( this,
|
||||
&msg_len ) ) { /* BAD LEN ( or TYPE) */
|
||||
remove_n_from_buf( this, 1 );
|
||||
parse_buf( this );
|
||||
} else {
|
||||
if ( this->buf_len < msg_len ) /* NOT ENOUGTH DATA */
|
||||
TRACE( TRACE_TRANSPORT_VERB, "not enough data\n" );
|
||||
else {
|
||||
if ( !check_checksum( this, msg_len ) ) {
|
||||
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
||||
this->buf_len ) );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_CHECKSUM_ERROR, /* guint code */
|
||||
"checksum error" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
|
||||
remove_n_from_buf( this, 1 );
|
||||
parse_buf( this );
|
||||
} else {
|
||||
if ( !this->two_bytes_checksum && this->buf[ msg_len - 1 ] != this->etx ) {
|
||||
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
||||
this->buf_len ) );
|
||||
TRACE( TRACE_TRANSPORT_VERB, "%d is not ETX\n", this->buf[ msg_len - 1 ] );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_NO_ETX, /* guint code */
|
||||
"NO ETX" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
remove_n_from_buf( this, 1 );
|
||||
parse_buf( this );
|
||||
} else {
|
||||
struct TransportMsg msg;
|
||||
guint payload_len = msg_len - TRANSPORT_HEAD_LEN - TRANSPORT_TAIL_LEN;
|
||||
g_get_current_time( &msg.date );
|
||||
msg.len = payload_len;
|
||||
msg.data = this->buf + TRANSPORT_PAYLOAD_OFFSET;
|
||||
this->status.nb_msg++;
|
||||
this->nb_msg_last_status++;
|
||||
|
||||
TRACE( TRACE_TRANSPORT_VERB,
|
||||
"transport msg date : %ld%06ld len : %d data : %s\n", msg.date.tv_sec,
|
||||
msg.date.tv_usec, msg.len, print_hex( msg.data, msg.len ) );
|
||||
this->msg_callback( this->callback_data, &msg );
|
||||
remove_n_from_buf( this, msg_len );
|
||||
parse_buf( this );
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,12 @@
|
||||
int main( int argc, char **argv, char **envp );
|
||||
|
||||
static char *argv[ ] = { "", 0 };
|
||||
static char *envp[ ] = { 0 };
|
||||
|
||||
void _start( void )
|
||||
{
|
||||
__asm( "lis 1," STACK ">>16" );
|
||||
__asm( "addis 1,1," STACK "&0xffff" );
|
||||
main( 1, argv, envp );
|
||||
while ( 1 );
|
||||
}
|
||||
116
targets/wasm-tacle/parallel/PapaBench/sw/var/include/airframe.h
Normal file
116
targets/wasm-tacle/parallel/PapaBench/sw/var/include/airframe.h
Normal file
@ -0,0 +1,116 @@
|
||||
/* This file has been generated from conf/airframes/twinstar3.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#ifndef AIRFRAME_H
|
||||
#define AIRFRAME_H
|
||||
|
||||
#define AIRFRAME_NAME "Twinstar trois"
|
||||
|
||||
#define CTL_BRD_V1_2_1 1
|
||||
|
||||
#define ADC_CHANNEL_IR1 1
|
||||
#define ADC_CHANNEL_IR2 2
|
||||
|
||||
#define NB_SERVO 6
|
||||
|
||||
#define SERVO_MOTOR_LEFT 3
|
||||
#define SERVO_MOTOR_LEFT_TRAVEL 0.833333333333
|
||||
#define SERVOS_NEUTRALS_3 1000
|
||||
|
||||
#define SERVO_MOTOR_RIGHT 9
|
||||
#define SERVO_MOTOR_RIGHT_TRAVEL 0.833333333333
|
||||
#define SERVOS_NEUTRALS_9 1000
|
||||
|
||||
#define SERVO_AILERON_LEFT 0
|
||||
#define SERVO_AILERON_LEFT_TRAVEL -0.55
|
||||
#define SERVOS_NEUTRALS_0 1600
|
||||
|
||||
#define SERVO_AILERON_RIGHT 2
|
||||
#define SERVO_AILERON_RIGHT_TRAVEL -0.520833333333
|
||||
#define SERVOS_NEUTRALS_2 1650
|
||||
|
||||
#define SERVO_ELEVATOR 6
|
||||
#define SERVO_ELEVATOR_TRAVEL 0.496666666667
|
||||
#define SERVOS_NEUTRALS_6 1530
|
||||
|
||||
#define SERVO_RUDDER 7
|
||||
#define SERVO_RUDDER_TRAVEL -0.483333333333
|
||||
#define SERVOS_NEUTRALS_7 1450
|
||||
|
||||
|
||||
#define SERVOS_MINS {1850,1000,2000,1000,1000,1000,1220,1850,1000,1000}
|
||||
#define SERVOS_NEUTRALS {1600,1500,1650,1000,1500,1500,1530,1450,1500,1000}
|
||||
#define SERVOS_MAXS {1190,2000,1375,2000,2000,2000,1816,1270,2000,2000}
|
||||
|
||||
#define SERVO_MIN_US 1000ul
|
||||
#define SERVO_MAX_US 2000ul
|
||||
|
||||
#define ServoSet(values) { \
|
||||
uint16_t servo_value;\
|
||||
int16_t _var_roll;\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_MOTOR_LEFT) + (int16_t)((2 * values[ RADIO_GAIN1 ])*SERVO_MOTOR_LEFT_TRAVEL);\
|
||||
servo_widths[ SERVO_MOTOR_LEFT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_MOTOR_RIGHT) + (int16_t)((2 * values[ RADIO_THROTTLE ])*SERVO_MOTOR_RIGHT_TRAVEL);\
|
||||
servo_widths[ SERVO_MOTOR_RIGHT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_ELEVATOR) + (int16_t)((values[ RADIO_PITCH ])*SERVO_ELEVATOR_TRAVEL);\
|
||||
servo_widths[ SERVO_ELEVATOR ] = ChopServo(servo_value);\
|
||||
\
|
||||
_var_roll = values[ RADIO_ROLL ];\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_AILERON_LEFT) + (int16_t)(((_var_roll > 0 ? 1 : AILERON_DIFF) * _var_roll)*SERVO_AILERON_LEFT_TRAVEL);\
|
||||
servo_widths[ SERVO_AILERON_LEFT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_AILERON_RIGHT) + (int16_t)(((_var_roll > 0 ? AILERON_DIFF : 1) * _var_roll)*SERVO_AILERON_RIGHT_TRAVEL);\
|
||||
servo_widths[ SERVO_AILERON_RIGHT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_RUDDER) + (int16_t)((values[ RADIO_YAW ] + values[ RADIO_ROLL ]*COMBI_SWITCH)*SERVO_RUDDER_TRAVEL);\
|
||||
servo_widths[ SERVO_RUDDER ] = ChopServo(servo_value);\
|
||||
\
|
||||
}
|
||||
#define AILERON_DIFF 0.66
|
||||
#define COMBI_SWITCH 1.0
|
||||
|
||||
#define IR_ROLL_NEUTRAL_DEFAULT -915
|
||||
#define IR_PITCH_NEUTRAL_DEFAULT 110
|
||||
#define IR_DEFAULT_CONTRAST 200
|
||||
#define IR_RAD_OF_IR_CONTRAST 0.75
|
||||
#define IR_RollOfIrs(x1,x2) (-1*(x1)+ -1*(x2))
|
||||
#define IR_PitchOfIrs(x1,x2) (-1*(x1)+ 1*(x2))
|
||||
#define IR_RAD_OF_IR_MAX_VAL 0.0045
|
||||
#define IR_RAD_OF_IR_MIN_VAL 0.00075
|
||||
|
||||
#define ROLL_PGAIN 10000.
|
||||
#define PITCH_OF_ROLL 0.0
|
||||
#define PITCH_PGAIN 15000.
|
||||
#define MAX_ROLL 0.35
|
||||
#define MAX_PITCH 0.35
|
||||
#define MIN_PITCH -0.35
|
||||
|
||||
#define CLIMB_PITCH_PGAIN -0.1
|
||||
#define CLIMB_PITCH_IGAIN 0.025
|
||||
#define CLIMB_PGAIN -0.03
|
||||
#define CLIMB_IGAIN 0.1
|
||||
#define CLIMB_MAX 1.
|
||||
#define CLIMB_LEVEL_GAZ 0.31
|
||||
#define CLIMB_PITCH_OF_VZ_PGAIN 0.05
|
||||
#define CLIMB_GAZ_OF_CLIMB 0.2
|
||||
|
||||
#define COURSE_PGAIN -0.2
|
||||
#define ALTITUDE_PGAIN -0.025
|
||||
#define NAV_PITCH 0.
|
||||
|
||||
#define VOLTAGE_ADC_A 0.0175
|
||||
#define VOLTAGE_ADC_B 0.088
|
||||
#define VoltageOfAdc(adc) (VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)
|
||||
#define LOW_BATTERY 93
|
||||
|
||||
#define NOMINAL_AIRSPEED 10.
|
||||
#define CARROT 5.
|
||||
|
||||
#define ROLL_RESPONSE_FACTOR 4.
|
||||
#define YAW_RESPONSE_FACTOR 4.
|
||||
#define WEIGHT 1.3
|
||||
|
||||
|
||||
#endif // AIRFRAME_H
|
||||
@ -0,0 +1,546 @@
|
||||
/* This file has been generated from conf/flight_plans/braunschweig.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#ifndef FLIGHT_PLAN_H
|
||||
#define FLIGHT_PLAN_H
|
||||
|
||||
#define FLIGHT_PLAN_NAME "EMAV 2004, 8 shape"
|
||||
#define NAV_UTM_EAST0 605530
|
||||
#define NAV_UTM_NORTH0 5797350
|
||||
#define QFU 270.0
|
||||
#define WP_HOME 0
|
||||
#define WAYPOINTS { \
|
||||
{0.0, 0.0, 200},\
|
||||
{0.0, 0.0, 200},\
|
||||
{115.0, -75.0, 200},\
|
||||
{156.7, -41.7, 200},\
|
||||
{115.0, 0.0, 200},\
|
||||
{0.0, -75.0, 200},\
|
||||
{-51.7, -36.7, 200},\
|
||||
};
|
||||
#define NB_WAYPOINT 7
|
||||
#define GROUND_ALT 125.
|
||||
#define SECURITY_ALT 150.
|
||||
#define MAX_DIST_FROM_HOME 500.
|
||||
#ifdef NAV_C
|
||||
|
||||
static inline void auto_nav( void )
|
||||
{
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (nav_block) {
|
||||
Block(0) // init
|
||||
switch(nav_stage) {
|
||||
Label(while_1)
|
||||
Stage(0)
|
||||
if (! (!(estimator_flight_time))) Goto(endwhile_2) else NextStage();
|
||||
Stage(1)
|
||||
Goto(while_1)
|
||||
Label(endwhile_2)
|
||||
Stage(2)
|
||||
if ((estimator_flight_time>8)) NextStage() else {
|
||||
desired_course = RadOfDeg(QFU);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.150000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
|
||||
nav_desired_gaz = TRIM_UPPRZ(0.800000*MAX_PPRZ);
|
||||
}
|
||||
return;
|
||||
Stage(3)
|
||||
if ((estimator_z>SECURITY_ALT)) NextStage() else {
|
||||
desired_course = RadOfDeg(QFU);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_CLIMB;
|
||||
desired_climb = 8.000000;
|
||||
}
|
||||
return;
|
||||
Stage(4)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(1) // two
|
||||
if RcEvent1() { GotoBlock(2) }
|
||||
switch(nav_stage) {
|
||||
Label(while_3)
|
||||
Stage(0)
|
||||
if (! (TRUE)) Goto(endwhile_4) else NextStage();
|
||||
Stage(1)
|
||||
if (approaching(1)) NextStageFrom(1) else {
|
||||
fly_to(1);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(2)
|
||||
if (approaching(4)) NextStageFrom(4) else {
|
||||
fly_to(4);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(3)
|
||||
Goto(while_3)
|
||||
Label(endwhile_4)
|
||||
Stage(4)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(2) // height
|
||||
if RcEvent1() { GotoBlock(3) }
|
||||
switch(nav_stage) {
|
||||
Label(while_5)
|
||||
Stage(0)
|
||||
if (! (TRUE)) Goto(endwhile_6) else NextStage();
|
||||
Stage(1)
|
||||
if (approaching(6)) NextStageFrom(6) else {
|
||||
fly_to(6);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 6 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(2)
|
||||
if (approaching(1)) NextStageFrom(1) else {
|
||||
fly_to(1);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(3)
|
||||
if (approaching(2)) NextStageFrom(2) else {
|
||||
route_to(last_wp, 2);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 2 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(4)
|
||||
if (approaching(3)) NextStageFrom(3) else {
|
||||
fly_to(3);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 3 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(5)
|
||||
if (approaching(4)) NextStageFrom(4) else {
|
||||
fly_to(4);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(6)
|
||||
if (approaching(5)) NextStageFrom(5) else {
|
||||
route_to(last_wp, 5);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 5 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(7)
|
||||
Goto(while_5)
|
||||
Label(endwhile_6)
|
||||
Stage(8)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(3) // xyz
|
||||
if RcEvent1() { GotoBlock(4) }
|
||||
switch(nav_stage) {
|
||||
Stage(0)
|
||||
|
||||
|
||||
//NormCourse(x) { while (x < 0) x += 360; while (x >= 360) x -= 360;}
|
||||
|
||||
|
||||
//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);}
|
||||
|
||||
|
||||
//Goto3D(50)
|
||||
{static float carrot_x, carrot_y; int16_t pitch; int16_t roll; if (pprz_mode == PPRZ_MODE_AUTO2) { int16_t yaw = from_fbw.channels[ RADIO_YAW ]; if (yaw >
|
||||
MIN_DX || yaw < -MIN_DX) { carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);carrot_x = Min(carrot_x, MAX_DIST_CARROT); carrot_x = Max(carrot_x, -MAX_DIST_CARROT); }
|
||||
pitch = from_fbw.channels[ RADIO_PITCH ]; if (pitch > MIN_DX || pitch < -MIN_DX) { carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); carrot_y = Min(carrot_y,
|
||||
MAX_DIST_CARROT); carrot_y = Max(carrot_y, -MAX_DIST_CARROT);} vertical_mode = VERTICAL_MODE_AUTO_ALT; roll = from_fbw.channels[ RADIO_ROLL ]; if (roll >
|
||||
MIN_DX || roll < -MIN_DX) { desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); desired_altitude = Max(desired_altitude, MIN_HEIGHT_CARROT+GROUND_ALT);
|
||||
desired_altitude = Min(desired_altitude, MAX_HEIGHT_CARROT+GROUND_ALT); } } { float alpha = atan2(estimator_y - carrot_y, estimator_x - carrot_x);float alpha_carrot = alpha + CARROT / (-50 * estimator_hspeed_mod);
|
||||
fly_to_xy(carrot_x+cos(alpha_carrot)*fabs(50),carrot_y+sin(alpha_carrot)*fabs(50)); qdr = DegOfRad(M_PI/2 - alpha_carrot); { while (qdr < 0) qdr += 360;
|
||||
while (qdr >= 360) qdr -= 360;}} }
|
||||
|
||||
|
||||
return;
|
||||
Stage(1)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(4) // circle
|
||||
if RcEvent1() { GotoBlock(5) }
|
||||
switch(nav_stage) {
|
||||
Stage(0)
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 0 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle(0, 150);
|
||||
return;
|
||||
Stage(1)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(5) // hippo
|
||||
if RcEvent1() { GotoBlock(1) }
|
||||
switch(nav_stage) {
|
||||
Label(while_7)
|
||||
Stage(0)
|
||||
if (! (TRUE)) Goto(endwhile_8) else NextStage();
|
||||
Stage(1)
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle(1, 100);
|
||||
if (Qdr(0)) NextStage();
|
||||
return;
|
||||
Stage(2)
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle(4, 100);
|
||||
if (Qdr(180)) NextStage();
|
||||
return;
|
||||
Stage(3)
|
||||
Goto(while_7)
|
||||
Label(endwhile_8)
|
||||
Stage(4)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
}
|
||||
#else*/
|
||||
if ( nav_block == 0 ) {
|
||||
// init
|
||||
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_1 )
|
||||
nav_stage = 0;
|
||||
if ( ! ( !( estimator_flight_time ) ) ) Goto( endwhile_2 )
|
||||
else NextStage();
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
Goto( while_1 )
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
Label( endwhile_2 )
|
||||
nav_stage = 2;
|
||||
if ( ( estimator_flight_time > 8 ) ) NextStage() else {
|
||||
desired_course = RadOfDeg( QFU );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.150000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
|
||||
nav_desired_gaz = TRIM_UPPRZ( 0.800000 * MAX_PPRZ );
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
if ( ( estimator_z > SECURITY_ALT ) ) NextStage() else {
|
||||
desired_course = RadOfDeg( QFU );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_CLIMB;
|
||||
desired_climb = 8.000000;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 4 )
|
||||
NextBlock()
|
||||
else { }
|
||||
} else
|
||||
if ( nav_block == 1 ) { // two
|
||||
nav_block = 1;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 2 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_3 )
|
||||
if ( ! ( TRUE ) ) Goto( endwhile_4 ) else NextStage();
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
if ( approaching( 1 ) ) NextStageFrom( 1 ) else {
|
||||
fly_to( 1 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
nav_stage = 2;
|
||||
if ( approaching( 4 ) ) NextStageFrom( 4 ) else {
|
||||
fly_to( 4 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
Goto( while_3 )
|
||||
} else
|
||||
if ( nav_stage == 4 ) {
|
||||
Label( endwhile_4 )
|
||||
nav_stage = 4;
|
||||
NextBlock()
|
||||
} else { }
|
||||
} else
|
||||
if ( nav_block == 2 ) { // height
|
||||
nav_block = 2;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 3 )
|
||||
}
|
||||
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_5 )
|
||||
nav_stage = 0;
|
||||
if ( ! ( TRUE ) ) Goto( endwhile_6 ) else NextStage();
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
if ( approaching( 6 ) ) NextStageFrom( 6 ) else {
|
||||
fly_to( 6 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 6 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
nav_stage = 2;
|
||||
if ( approaching( 1 ) ) NextStageFrom( 1 ) else {
|
||||
fly_to( 1 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
if ( approaching( 2 ) ) NextStageFrom( 2 ) else {
|
||||
route_to( last_wp, 2 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 2 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 4 ) {
|
||||
nav_stage = 4;
|
||||
if ( approaching( 3 ) ) NextStageFrom( 3 ) else {
|
||||
fly_to( 3 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 3 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 5 ) {
|
||||
nav_stage = 5;
|
||||
if ( approaching( 4 ) ) NextStageFrom( 4 ) else {
|
||||
fly_to( 4 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 6 ) {
|
||||
nav_stage = 6;
|
||||
if ( approaching( 5 ) ) NextStageFrom( 5 ) else {
|
||||
route_to( last_wp, 5 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 5 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 7 ) {
|
||||
nav_stage = 7;
|
||||
Goto( while_5 )
|
||||
}
|
||||
|
||||
else
|
||||
if ( nav_stage == 8 ) {
|
||||
Label( endwhile_6 )
|
||||
nav_stage = 8;
|
||||
NextBlock();
|
||||
} else { }
|
||||
} else
|
||||
if ( nav_block == 3 ) { // xyz
|
||||
nav_block = 3;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 4 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
nav_stage = 0;
|
||||
//Goto3D(50)
|
||||
{
|
||||
static float carrot_x, carrot_y;
|
||||
int16_t pitch;
|
||||
int16_t roll;
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 ) {
|
||||
int16_t yaw = from_fbw.channels[ RADIO_YAW ];
|
||||
if ( yaw > MIN_DX || yaw < -MIN_DX ) {
|
||||
carrot_x += FLOAT_OF_PPRZ( yaw, 0, -20. );
|
||||
carrot_x = Min( carrot_x, MAX_DIST_CARROT );
|
||||
carrot_x = Max( carrot_x, -MAX_DIST_CARROT );
|
||||
}
|
||||
pitch = from_fbw.channels[ RADIO_PITCH ];
|
||||
if ( pitch > MIN_DX || pitch < -MIN_DX ) {
|
||||
carrot_y += FLOAT_OF_PPRZ( pitch, 0, -20. );
|
||||
carrot_y = Min( carrot_y,
|
||||
MAX_DIST_CARROT );
|
||||
carrot_y = Max( carrot_y, -MAX_DIST_CARROT );
|
||||
}
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
roll = from_fbw.channels[ RADIO_ROLL ];
|
||||
if ( roll > MIN_DX || roll < -MIN_DX ) {
|
||||
desired_altitude += FLOAT_OF_PPRZ( roll, 0, -1.0 );
|
||||
desired_altitude = Max( desired_altitude, MIN_HEIGHT_CARROT + GROUND_ALT );
|
||||
desired_altitude = Min( desired_altitude, MAX_HEIGHT_CARROT + GROUND_ALT );
|
||||
}
|
||||
}
|
||||
{
|
||||
float alpha = atan2( estimator_y - carrot_y, estimator_x - carrot_x );
|
||||
float alpha_carrot = alpha + CARROT / ( -50 * estimator_hspeed_mod );
|
||||
fly_to_xy( carrot_x + cos( alpha_carrot )*fabs( 50 ),
|
||||
carrot_y + sin( alpha_carrot )*fabs( 50 ) );
|
||||
qdr = DegOfRad( M_PI / 2 - alpha_carrot );
|
||||
{
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( qdr < 0 ) qdr += 360;
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( qdr >= 360 ) qdr -= 360;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
NextBlock()
|
||||
} else { }
|
||||
} else
|
||||
if ( nav_block == 4 ) {
|
||||
nav_block = 4;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 5 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
nav_stage = 0;
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 0 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle( 0, 150 );
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
NextBlock()
|
||||
} else {}
|
||||
} else
|
||||
if ( nav_block == 5 ) {
|
||||
nav_block = 5;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 1 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_7 )
|
||||
nav_stage = 0;
|
||||
if ( ! ( TRUE ) ) Goto( endwhile_8 ) else NextStage();
|
||||
}
|
||||
|
||||
else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle( 1, 100 );
|
||||
if ( Qdr( 0 ) ) NextStage();
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
nav_stage = 2;
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle( 4, 100 );
|
||||
if ( Qdr( 180 ) ) NextStage();
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
Goto( while_7 )
|
||||
} else
|
||||
if ( nav_stage == 4 ) {
|
||||
Label( endwhile_8 )
|
||||
nav_stage = 4;
|
||||
NextBlock()
|
||||
} else { }
|
||||
} else { }
|
||||
|
||||
//#endif
|
||||
}
|
||||
#endif // NAV_C
|
||||
|
||||
#endif // FLIGHT_PLAN_H
|
||||
@ -0,0 +1,55 @@
|
||||
/* This file has been generated from conf/flight_plans/braunschweig.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#ifndef INFLIGHT_CALIB_H
|
||||
#define INFLIGHT_CALIB_H
|
||||
|
||||
void inflight_calib( bool_t mode_changed )
|
||||
{
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO1 ) {
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static int16_t ir_pitch_neutral_init;
|
||||
if ( mode_changed ) {
|
||||
ir_pitch_neutral_init = ir_pitch_neutral;
|
||||
slider1_init = from_fbw.channels[ RADIO_GAIN1 ];
|
||||
}
|
||||
ir_pitch_neutral = ParamValInt16( ir_pitch_neutral_init, 60.000000,
|
||||
from_fbw.channels[ RADIO_GAIN1 ], slider1_init );
|
||||
slider_1_val = ( float )ir_pitch_neutral;
|
||||
}
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static int16_t ir_roll_neutral_init;
|
||||
if ( mode_changed ) {
|
||||
ir_roll_neutral_init = ir_roll_neutral;
|
||||
slider2_init = from_fbw.channels[ RADIO_GAIN2 ];
|
||||
}
|
||||
ir_roll_neutral = ParamValInt16( ir_roll_neutral_init, -60.000000,
|
||||
from_fbw.channels[ RADIO_GAIN2 ], slider2_init );
|
||||
slider_2_val = ( float )ir_roll_neutral;
|
||||
}
|
||||
}
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 ) {
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static float course_pgain_init;
|
||||
if ( mode_changed ) {
|
||||
course_pgain_init = course_pgain;
|
||||
slider1_init = from_fbw.channels[ RADIO_GAIN1 ];
|
||||
}
|
||||
course_pgain = ParamValFloat( course_pgain_init, 0.100000,
|
||||
from_fbw.channels[ RADIO_GAIN1 ], slider1_init );
|
||||
slider_1_val = ( float )course_pgain;
|
||||
}
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static float max_roll_init;
|
||||
if ( mode_changed ) {
|
||||
max_roll_init = max_roll;
|
||||
slider2_init = from_fbw.channels[ RADIO_GAIN2 ];
|
||||
}
|
||||
max_roll = ParamValFloat( max_roll_init, -0.200000,
|
||||
from_fbw.channels[ RADIO_GAIN2 ], slider2_init );
|
||||
slider_2_val = ( float )max_roll;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // INFLIGHT_CALIB_H
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user