mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
upgrade from paparazzi2 ;-)
This commit is contained in:
Pascal Brisset
@@ -103,7 +103,7 @@ receive: tmtc
|
||||
static_h :
|
||||
PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` make -f Makefile.gen
|
||||
|
||||
ac_h :
|
||||
ac_h : static_h
|
||||
PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` $(TOOLS)/gen_aircraft.out $(AIRCRAFT)
|
||||
|
||||
sim_ac: ac_h sim_sitl
|
||||
|
||||
@@ -1,9 +1,54 @@
|
||||
<radio name="cockpitMM">
|
||||
<channel ctl="D" function="ROLL" min="1990" neutral="1485" max="982" average="0"/>
|
||||
<channel ctl="C" function="PITCH" min="1993" neutral="1490" max="984" average="0"/>
|
||||
<channel ctl="B" function="YAW" min="1994" neutral="1490" max="986" average="0"/>
|
||||
<channel ctl="A" function="THROTTLE" min="1992" neutral="1114" max="1114" average="0"/>
|
||||
<channel ctl="G" function="GAIN1" min="1996" neutral="1485" max="988" average="10"/> <!-- center slider -->
|
||||
<channel ctl="E" function="MODE" min="1997" neutral="1492" max="988" average="10"/> <!-- Top right switch -->
|
||||
<channel ctl="F" function="CALIB" min="2000" neutral="1480" max="990" average="10"/> <!-- Top left rotary knob -->
|
||||
<?xml version="1.0"?>
|
||||
<!-- $Id$
|
||||
--
|
||||
-- (c) 2003 Pascal Brisset, Antoine Drouin
|
||||
--
|
||||
-- This file is part of paparazzi.
|
||||
--
|
||||
-- paparazzi is free software; you can redistribute it and/or modify
|
||||
-- it under the terms of the GNU General Public License as published by
|
||||
-- the Free Software Foundation; either version 2, or (at your option)
|
||||
-- any later version.
|
||||
--
|
||||
-- paparazzi is distributed in the hope that it will be useful,
|
||||
-- but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
-- GNU General Public License for more details.
|
||||
--
|
||||
-- You should have received a copy of the GNU General Public License
|
||||
-- along with paparazzi; see the file COPYING. If not, write to
|
||||
-- the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
-- Boston, MA 02111-1307, USA.
|
||||
-->
|
||||
|
||||
<!--
|
||||
-- Attributes of root (Radio) tag :
|
||||
-- name: name of RC
|
||||
-- min: min width of a pulse to be considered as a data pulse
|
||||
-- max: max width of a pulse to be considered as a data pulse
|
||||
-- sync: min width of a pulse to be considered as a synchro pulse
|
||||
-- min, max and sync are expressed in nano-seconds
|
||||
-->
|
||||
|
||||
<!--
|
||||
-- Attributes of channel tag :
|
||||
-- ctl: name of the command on the transmitter - only for displaying
|
||||
-- no: order in the PPM frame
|
||||
-- function: logical command
|
||||
-- averaged: channel filtered through several frames (for discrete commands)
|
||||
-- min: minimum pulse length (micro-seconds)
|
||||
-- max: maximum pulse length (micro-seconds)
|
||||
-- neutral: neutral pulse length (micro-seconds)
|
||||
Note: a command may be reversed by exchanging min and max values
|
||||
-->
|
||||
|
||||
<!DOCTYPE radio SYSTEM "radio.dtd">
|
||||
<radio name="cockpitMM" min="850" max="2100" sync="5000">
|
||||
<channel ctl="D" function="ROLL" min="2000" neutral="1498" max="1000" average="0"/>
|
||||
<channel ctl="C" function="PITCH" min="2000" neutral="1498" max="1000" average="0"/>
|
||||
<channel ctl="B" function="YAW" min="2000" neutral="1498" max="1000" average="0"/>
|
||||
<channel ctl="A" function="THROTTLE" min="1120" neutral="1120" max="2000" average="0"/>
|
||||
<channel ctl="G" function="GAIN1" min="2000" neutral="1498" max="1000" average="10"/> <!-- center slider -->
|
||||
<channel ctl="E" function="MODE" min="2000" neutral="1500" max="1000" average="10"/> <!-- Top right switch -->
|
||||
<channel ctl="F" function="CALIB" min="2000" neutral="1500" max="1000" average="10"/> <!-- Top left rotary knob -->
|
||||
</radio>
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
<!-- Radio configuration DTD -->
|
||||
|
||||
<!ELEMENT radio (channel+)>
|
||||
<!ELEMENT channel EMPTY>
|
||||
|
||||
<!ATTLIST radio
|
||||
name CDATA #REQUIRED
|
||||
min CDATA #REQUIRED
|
||||
max CDATA #REQUIRED
|
||||
sync CDATA #REQUIRED>
|
||||
|
||||
<!ATTLIST channel
|
||||
ctl CDATA #REQUIRED
|
||||
function CDATA #REQUIRED
|
||||
min CDATA #REQUIRED
|
||||
neutral CDATA #REQUIRED
|
||||
max CDATA #REQUIRED
|
||||
average CDATA #REQUIRED>
|
||||
@@ -65,7 +65,6 @@ $(TARGET).srcs = \
|
||||
uart.c \
|
||||
estimator.c \
|
||||
if_calib.c \
|
||||
traffic_info.c \
|
||||
mainloop.c
|
||||
|
||||
include ../../../conf/Makefile.local
|
||||
|
||||
@@ -35,4 +35,11 @@
|
||||
/* Boolean values */
|
||||
typedef uint8_t bool_t;
|
||||
|
||||
#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 */
|
||||
|
||||
+35
-1
@@ -30,7 +30,7 @@ let sep = Str.regexp "\\."
|
||||
|
||||
let child xml ?select c =
|
||||
let rec find = function
|
||||
Xml.Element (tag, _attributes, _children) as elt :: elts ->
|
||||
Xml.Element (tag, attributes, _children) as elt :: elts ->
|
||||
if tag = c then
|
||||
match select with
|
||||
None -> elt
|
||||
@@ -85,3 +85,37 @@ let to_string_fmt = fun xml ->
|
||||
List.map (fun (a,v) -> (l a, v)) ats,
|
||||
List.map lower cs) in
|
||||
Xml.to_string_fmt (lower xml)
|
||||
|
||||
|
||||
let subst_attrib = fun attrib value xml ->
|
||||
let u = String.uppercase in
|
||||
let uattrib = u attrib in
|
||||
match xml with
|
||||
Xml.Element (tag, attrs, children) ->
|
||||
let rec loop = function
|
||||
[] -> [(attrib, value)]
|
||||
| (a,v) as c::ats ->
|
||||
if u a = uattrib then loop ats else c::loop ats in
|
||||
Xml.Element (tag,
|
||||
loop attrs,
|
||||
children)
|
||||
| Xml.PCData _ -> xml
|
||||
|
||||
|
||||
let subst_child = fun t x xml ->
|
||||
let u = String.uppercase in
|
||||
match xml with
|
||||
Xml.Element (tag, attrs, children) ->
|
||||
Xml.Element (tag,
|
||||
attrs,
|
||||
List.map (fun xml -> if u (Xml.tag xml) = u t then x else xml) children)
|
||||
| Xml.PCData _ -> xml
|
||||
|
||||
|
||||
let float_attrib = fun xml a ->
|
||||
let v = attrib xml a in
|
||||
try
|
||||
float_of_string v
|
||||
with
|
||||
_ -> failwith (Printf.sprintf "Error: float expected in '%s'" v)
|
||||
|
||||
|
||||
@@ -46,3 +46,8 @@ val attrib_or_default : Xml.xml -> string -> string -> string
|
||||
val to_string_fmt : Xml.xml -> string
|
||||
(** [to_string_fmt xml] Returns a formatted string where tag and attribute
|
||||
names are lowercase *)
|
||||
|
||||
val subst_attrib : string -> string -> Xml.xml -> Xml.xml
|
||||
val subst_child : string -> Xml.xml -> Xml.xml -> Xml.xml
|
||||
val float_attrib : Xml.xml -> string -> float
|
||||
|
||||
|
||||
+10
-12
@@ -44,8 +44,6 @@ let parse_expression = fun s ->
|
||||
|
||||
open Latlong
|
||||
|
||||
let float_attrib = fun xml a -> float_of_string (ExtXml.attrib xml a)
|
||||
|
||||
(* Translation and rotation *)
|
||||
type affine = { dx : float; dy : float; angle : float (* Deg Clockwise *) }
|
||||
|
||||
@@ -103,8 +101,8 @@ let transform_values = fun attribs_not_modified affine env attribs ->
|
||||
attribs
|
||||
|
||||
let transform_waypoint = fun prefix affine xml ->
|
||||
let x = float_attrib xml "x"
|
||||
and y = float_attrib xml "y" in
|
||||
let x = ExtXml.float_attrib xml "x"
|
||||
and y = ExtXml.float_attrib xml "y" in
|
||||
let (x, y) = rotate affine.angle (x, y) in
|
||||
let (x, y) = (x+.affine.dx, y+.affine.dy) in
|
||||
let alt = try ["alt", ExtXml.attrib xml "alt"] with ExtXml.Error _ -> [] in
|
||||
@@ -214,9 +212,9 @@ let parse_include = fun dir include_xml ->
|
||||
let proc_name = ExtXml.attrib include_xml "name" in
|
||||
let prefix = fun x -> proc_name ^ "." ^ x in
|
||||
let affine = {
|
||||
dx = float_attrib include_xml "x";
|
||||
dy = float_attrib include_xml "y";
|
||||
angle = float_attrib include_xml "rotate"
|
||||
dx = ExtXml.float_attrib include_xml "x";
|
||||
dy = ExtXml.float_attrib include_xml "y";
|
||||
angle = ExtXml.float_attrib include_xml "rotate"
|
||||
} in
|
||||
let reroutes =
|
||||
List.filter
|
||||
@@ -312,7 +310,7 @@ let xml_assoc_attrib = fun a v xmls ->
|
||||
| _ -> failwith "xml_assoc_attrib"
|
||||
|
||||
let coords_of_waypoint = fun wp ->
|
||||
(float_attrib wp "x", float_attrib wp "y")
|
||||
(ExtXml.float_attrib wp "x", ExtXml.float_attrib wp "y")
|
||||
|
||||
|
||||
let new_waypoint = fun wp qdr dist waypoints ->
|
||||
@@ -329,8 +327,8 @@ let new_waypoint = fun wp qdr dist waypoints ->
|
||||
|
||||
let replace_wp = fun stage waypoints ->
|
||||
try
|
||||
let qdr = float_attrib stage "wp_qdr"
|
||||
and dist = float_attrib stage "wp_dist" in
|
||||
let qdr = ExtXml.float_attrib stage "wp_qdr"
|
||||
and dist = ExtXml.float_attrib stage "wp_dist" in
|
||||
let wp = ExtXml.attrib stage "wp" in
|
||||
|
||||
let name = new_waypoint wp qdr dist waypoints in
|
||||
@@ -343,8 +341,8 @@ let replace_wp = fun stage waypoints ->
|
||||
|
||||
let replace_from = fun stage waypoints ->
|
||||
try
|
||||
let qdr = float_attrib stage "from_qdr"
|
||||
and dist = float_attrib stage "from_dist" in
|
||||
let qdr = ExtXml.float_attrib stage "from_qdr"
|
||||
and dist = ExtXml.float_attrib stage "from_dist" in
|
||||
let wp = ExtXml.attrib stage "from" in
|
||||
|
||||
let name = new_waypoint wp qdr dist waypoints in
|
||||
|
||||
@@ -19,13 +19,6 @@ type expression =
|
||||
|
||||
let c_var_of_ident = fun x -> "_var_" ^ x
|
||||
|
||||
(* Valid unary and binary opetarors *)
|
||||
(*let binary_operators = ["+"; ">"; "-"; "*"]
|
||||
let unary_operators = ["!"; "-"]
|
||||
|
||||
let is_binary = fun op -> List.mem op binary_operators
|
||||
let is_unary = fun op -> List.mem op unary_operators*)
|
||||
|
||||
let rec sprint_expression = function
|
||||
Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1)))
|
||||
| Ident i -> sprintf "%s" i
|
||||
@@ -46,19 +39,27 @@ let functions = [
|
||||
"Qdr";
|
||||
"And";
|
||||
"Or";
|
||||
"RcRoll";
|
||||
"RcEvent1";
|
||||
"RcEvent2"] (*@ binary_operators @ unary_operators*)
|
||||
"RcEvent2";
|
||||
"RadOfDeg"] (*@ binary_operators @ unary_operators*)
|
||||
|
||||
(* Valid identifiers *)
|
||||
let variables = [
|
||||
"launch";
|
||||
"estimator_z";
|
||||
"estimator_flight_time";
|
||||
"estimator_hspeed_mod";
|
||||
"estimator_theta";
|
||||
"circle_count";
|
||||
"vsupply";
|
||||
"stage_time";
|
||||
"stage_time_ds";
|
||||
"block_time";
|
||||
"SECURITY_ALT";
|
||||
"GROUND_ALT";
|
||||
"TRUE";
|
||||
"FALSE";
|
||||
"QFU"
|
||||
]
|
||||
|
||||
|
||||
@@ -48,8 +48,12 @@ let define_macro name n x =
|
||||
|
||||
let parse_element = fun prefix s ->
|
||||
match Xml.tag s with
|
||||
"define" ->
|
||||
define (prefix^ExtXml.attrib s "name") (ExtXml.attrib s "value")
|
||||
"define" -> begin
|
||||
try
|
||||
define (prefix^ExtXml.attrib s "name") (ExtXml.attrib s "value");
|
||||
define (prefix^(ExtXml.attrib s "name")^"_NB_SAMPLE") (ExtXml.attrib s "nb_sample");
|
||||
with _ -> ();
|
||||
end
|
||||
| "linear" ->
|
||||
let name = ExtXml.attrib s "name"
|
||||
and n = int_of_string (ExtXml.attrib s "arity") in
|
||||
@@ -74,7 +78,9 @@ let parse_servo =
|
||||
servo_params.(no_servo) <- { min = min; neutral = neutral; max = max }
|
||||
|
||||
|
||||
let pprz_value = Str.regexp "@\\([A-Z]+\\)"
|
||||
(* Characters checked in Gen_radio.checl_function_name *)
|
||||
let pprz_value = Str.regexp "@\\([A-Z_0-9]+\\)"
|
||||
|
||||
let var_value = Str.regexp "\\$\\([_a-z0-9]+\\)"
|
||||
let preprocess_command = fun s ->
|
||||
let s = Str.global_replace pprz_value "values[RADIO_\\1]" s in
|
||||
|
||||
+73
-11
@@ -27,6 +27,8 @@
|
||||
open Printf
|
||||
open Latlong
|
||||
|
||||
let sof = string_of_float
|
||||
|
||||
let check_expressions = ref true
|
||||
|
||||
let parse_expression = Fp_proc.parse_expression
|
||||
@@ -124,8 +126,15 @@ let stage = ref 0
|
||||
let output_label l = lprintf "Label(%s)\n" l
|
||||
|
||||
let output_vmode x wp last_wp =
|
||||
let pitch = try parsed_attrib x "pitch" with _ -> "0.0" in
|
||||
lprintf "nav_pitch = %s;\n" pitch;
|
||||
let pitch = try Xml.attrib x "pitch" with _ -> "0.0" in
|
||||
if pitch = "auto"
|
||||
then begin
|
||||
lprintf "auto_pitch = TRUE;\n";
|
||||
lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz")
|
||||
end else begin
|
||||
lprintf "auto_pitch = FALSE;\n";
|
||||
lprintf "nav_pitch = %s;\n" (parse pitch);
|
||||
end;
|
||||
let vmode = try ExtXml.attrib x "vmode" with _ -> "alt" in
|
||||
begin
|
||||
match vmode with
|
||||
@@ -141,18 +150,22 @@ let output_vmode x wp last_wp =
|
||||
try
|
||||
check_altitude (float_of_string a) x
|
||||
with
|
||||
(* Impossible to check the altitude on an expression: *)
|
||||
Failure "float_of_string" -> ()
|
||||
end;
|
||||
a
|
||||
with _ ->
|
||||
if wp = "" then failwith "alt or waypoint required in alt vmode" else
|
||||
sprintf "waypoints[%s].a" wp in
|
||||
if wp = ""
|
||||
then failwith "alt or waypoint required in alt vmode"
|
||||
else sprintf "waypoints[%s].a" wp in
|
||||
lprintf "desired_altitude = %s;\n" alt;
|
||||
lprintf "pre_climb = 0.;\n"
|
||||
| "glide" ->
|
||||
lprintf "vertical_mode = VERTICAL_MODE_AUTO_ALT;\n";
|
||||
lprintf "glide_to(%s, %s);\n" last_wp wp
|
||||
| "gaz" ->
|
||||
if (pitch = "auto") then
|
||||
failwith "auto pich mode not compatible with vmode=gaz";
|
||||
lprintf "vertical_mode = VERTICAL_MODE_AUTO_GAZ;\n";
|
||||
lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz")
|
||||
| x -> failwith (sprintf "Unknown vmode '%s'" x)
|
||||
@@ -194,8 +207,8 @@ let rec compile_stage = fun block x ->
|
||||
| "for" ->
|
||||
List.iter (compile_stage block) (Xml.children x);
|
||||
incr stage (* To count the loop stage *)
|
||||
| "return_from_excpt" | "goto" | "deroute" | "exit_block" | "follow"
|
||||
| "heading" | "go" | "stay" | "xyz" | "circle" -> ()
|
||||
| "return_from_excpt" | "goto" | "deroute" | "exit_block"
|
||||
| "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" -> ()
|
||||
| s -> failwith (sprintf "Unknown stage: %s\n" s)
|
||||
end
|
||||
|
||||
@@ -256,6 +269,16 @@ let rec print_stage = fun index_of_waypoints x ->
|
||||
let d = ExtXml.attrib x "distance" in
|
||||
lprintf "Follow(%s, %s);\n" id d;
|
||||
lprintf "return;\n"
|
||||
| "attitude" ->
|
||||
stage ();
|
||||
let until = parsed_attrib x "until" in
|
||||
lprintf "if (%s) NextStage() else {\n" until;
|
||||
right ();
|
||||
lprintf "lateral_mode = LATERAL_MODE_ROLL;\n";
|
||||
lprintf "nav_desired_roll = RadOfDeg(%s);\n" (parsed_attrib x "roll");
|
||||
ignore (output_vmode x "" "");
|
||||
left (); lprintf "}\n";
|
||||
lprintf "return;\n"
|
||||
| "go" ->
|
||||
stage ();
|
||||
let wp =
|
||||
@@ -311,6 +334,20 @@ let rec print_stage = fun index_of_waypoints x ->
|
||||
ExtXml.Error _ -> ()
|
||||
end;
|
||||
lprintf "return;\n"
|
||||
| "set" ->
|
||||
stage ();
|
||||
let var = parsed_attrib x "var" in
|
||||
let valuee = parsed_attrib x "value" in
|
||||
lprintf "%s = %s;\n" var valuee;
|
||||
begin
|
||||
try
|
||||
let c = parsed_attrib x "until" in
|
||||
lprintf "if (%s) NextStage();\n" c
|
||||
with
|
||||
ExtXml.Error _ ->
|
||||
lprintf "NextStage();\n";
|
||||
end;
|
||||
lprintf "return;\n"
|
||||
| s -> failwith "Unreachable"
|
||||
end;
|
||||
left ()
|
||||
@@ -386,7 +423,29 @@ let check_distance = fun (hx, hy) max_d wp ->
|
||||
let d = sqrt ((x-.hx)**2. +. (y-.hy)**2.) in
|
||||
if d > max_d then
|
||||
fprintf stderr "\nWARNING: Waypoint '%s' too far from HOME (%.0f>%.0f)\n\n" (name_of wp) d max_d
|
||||
|
||||
|
||||
(* Check coherence between global ref and waypoints ref *)
|
||||
(* Returns a patched xml with utm_x0 and utm_y0 set *)
|
||||
let check_geo_ref = fun xml ->
|
||||
let get_float = fun x -> float_attrib xml x in
|
||||
let lat0_deg = get_float "lat0"
|
||||
and lon0_deg = get_float "lon0" in
|
||||
let utm0 = utm_of WGS84 { posn_lat=(Deg>>Rad)lat0_deg;
|
||||
posn_long=(Deg>>Rad)lon0_deg } in
|
||||
|
||||
let max_d = get_float "max_dist_from_home" in
|
||||
let check_zone = fun u ->
|
||||
if (utm_of WGS84 (of_utm WGS84 u)).utm_zone <> utm0.utm_zone then
|
||||
failwith "Fatal error: You are too close (less than twice the max distance) to an UTM zone border !" in
|
||||
check_zone { utm0 with utm_x = utm0.utm_x +. 2.*.max_d };
|
||||
check_zone { utm0 with utm_x = utm0.utm_x -. 2.*.max_d };
|
||||
|
||||
let wpts = ExtXml.child xml "waypoints" in
|
||||
let wpts = ExtXml.subst_attrib "utm_x0" (sof utm0.utm_x) wpts in
|
||||
let wpts = ExtXml.subst_attrib "utm_y0" (sof utm0.utm_y) wpts in
|
||||
let x = ExtXml.subst_child "waypoints" wpts xml in
|
||||
x
|
||||
|
||||
let dummy_waypoint =
|
||||
Xml.Element ("waypoint",
|
||||
@@ -397,7 +456,7 @@ let dummy_waypoint =
|
||||
|
||||
|
||||
let _ =
|
||||
let xml_file = ref ""
|
||||
let xml_file = ref "fligh_plan.xml"
|
||||
and dump = ref false in
|
||||
Arg.parse [("-dump", Arg.Set dump, "Dump compile result");
|
||||
("-nocheck", Arg.Clear check_expressions, "Disable expression checking")]
|
||||
@@ -407,9 +466,11 @@ let _ =
|
||||
failwith (sprintf "Usage: %s <xml-flight-plan-file>" Sys.argv.(0));
|
||||
try
|
||||
let xml = Xml.parse_file !xml_file in
|
||||
|
||||
let xml = check_geo_ref xml in
|
||||
|
||||
let dir = Filename.dirname !xml_file in
|
||||
let xml = Fp_proc.process_includes dir xml in
|
||||
(*** prerr_endline (Xml.to_string_fmt xml); prerr_endline "\n\n\n"; ***)
|
||||
let xml = Fp_proc.process_relative_waypoints xml in
|
||||
let waypoints = ExtXml.child xml "waypoints"
|
||||
and blocks = Xml.children (ExtXml.child xml "blocks") in
|
||||
@@ -474,9 +535,9 @@ let _ =
|
||||
lprintf "};\n";
|
||||
Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints));
|
||||
|
||||
Xml2h.define "GROUND_ALT" (string_of_float !ground_alt);
|
||||
Xml2h.define "SECURITY_ALT" (string_of_float (!security_height +. !ground_alt));
|
||||
Xml2h.define "MAX_DIST_FROM_HOME" (string_of_float mdfh);
|
||||
Xml2h.define "GROUND_ALT" (sof !ground_alt);
|
||||
Xml2h.define "SECURITY_ALT" (sof (!security_height +. !ground_alt));
|
||||
Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh);
|
||||
|
||||
let index_of_waypoints =
|
||||
let i = ref (-1) in
|
||||
@@ -489,3 +550,4 @@ let _ =
|
||||
with
|
||||
Xml.Error e -> prerr_endline (Xml.error e); exit 1
|
||||
| Dtd.Prove_error e -> prerr_endline (Dtd.prove_error e); exit 1
|
||||
|
||||
|
||||
+74
-19
@@ -30,28 +30,83 @@ open Xml2h
|
||||
let h_name = "RADIO_H"
|
||||
|
||||
let fos = float_of_string
|
||||
type us = int
|
||||
|
||||
type channel = { min : string; max : string; neutral : string; averaged : string }
|
||||
type channel = {
|
||||
name : string;
|
||||
min : us;
|
||||
max : us;
|
||||
neutral : us;
|
||||
averaged : bool }
|
||||
|
||||
let default_neutral = "1600"
|
||||
let default_min = "1000"
|
||||
let default_max = "2200"
|
||||
|
||||
(* Characters used in Gen_airframe.pprz_value *)
|
||||
let check_function_name = fun s ->
|
||||
for i = 0 to String.length s - 1 do
|
||||
match s.[i] with
|
||||
'A'..'Z' | '0'..'9' | '_' -> ()
|
||||
| _ ->
|
||||
failwith (sprintf "Character '%c' not allowed in function name '%s'" s.[i] s)
|
||||
done
|
||||
|
||||
let parse_channel =
|
||||
let no_channel = ref 0 in
|
||||
fun c ->
|
||||
let name = ExtXml.attrib c "function" in
|
||||
check_function_name name;
|
||||
let ctl = "RADIO_CTL_"^ExtXml.attrib c "ctl"
|
||||
and fct = "RADIO_" ^ ExtXml.attrib c "function" in
|
||||
and fct = "RADIO_" ^ name in
|
||||
define ctl (string_of_int !no_channel);
|
||||
define fct ctl;
|
||||
no_channel := !no_channel + 1;
|
||||
{ min = ExtXml.attrib_or_default c "min" default_min;
|
||||
neutral = ExtXml.attrib_or_default c "neutral" default_neutral;
|
||||
max = ExtXml.attrib_or_default c "max" default_max;
|
||||
averaged = ExtXml.attrib_or_default c "average" "0"
|
||||
let int_attrib = fun x -> int_of_string (ExtXml.attrib c x) in
|
||||
{ min = int_attrib "min";
|
||||
neutral = int_attrib "neutral";
|
||||
max = int_attrib "max";
|
||||
averaged = ExtXml.attrib_or_default c "average" "0" <> "0";
|
||||
name = name
|
||||
}
|
||||
|
||||
|
||||
let gen_last_radio_from_ppm = fun channels ->
|
||||
printf "#define LastRadioFromPpm() {\\\n";
|
||||
printf " static uint8_t avg_cpt = 0; /* Counter for averaging */\\\n";
|
||||
printf " int16_t tmp_radio;\\\n";
|
||||
List.iter
|
||||
(fun c ->
|
||||
printf " tmp_radio = ppm_pulses[RADIO_%s] - (CLOCK*%d);\\\n" c.name c.neutral;
|
||||
let period = if c.averaged then "AVERAGING_PERIOD" else "1" in
|
||||
let value, min_pprz =
|
||||
if c.neutral = c.min then
|
||||
sprintf "tmp_radio * (MAX_PPRZ / %s / (float)(CLOCK*(%d-%d)))" period c.max c.min, "0"
|
||||
else
|
||||
sprintf "tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/%s/(float)(CLOCK*(%d-%d))) : (MIN_PPRZ/%s/(float)(CLOCK*(%d-%d))))" period c.max c.neutral period c.min c.neutral, "MIN_PPRZ" in
|
||||
if c.averaged then begin
|
||||
printf " avg_last_radio[RADIO_%s] += %s;\\\n" c.name value
|
||||
end else begin
|
||||
printf " last_radio[RADIO_%s] = %s;\\\n" c.name value;
|
||||
printf " if (last_radio[RADIO_%s] > MAX_PPRZ) last_radio[RADIO_%s] = MAX_PPRZ;\\\n else if (last_radio[RADIO_%s] < %s) last_radio[RADIO_%s] = %s; \\\n\\\n" c.name c.name c.name min_pprz c.name min_pprz;
|
||||
end
|
||||
)
|
||||
channels;
|
||||
printf "avg_cpt++;\\\n";
|
||||
printf " if (avg_cpt == AVERAGING_PERIOD) {\\\n";
|
||||
printf " avg_cpt = 0;\\\n";
|
||||
List.iter
|
||||
(fun c ->
|
||||
if c.averaged then begin
|
||||
printf " last_radio[RADIO_%s] = avg_last_radio[RADIO_%s];\\\n" c.name c.name;
|
||||
printf " avg_last_radio[RADIO_%s] = 0;\\\n" c.name;
|
||||
printf " if (last_radio[RADIO_%s] > MAX_PPRZ) last_radio[RADIO_%s] = MAX_PPRZ;\\\n else if (last_radio[RADIO_%s] < MIN_PPRZ) last_radio[RADIO_%s] = MIN_PPRZ; \\\n\\\n" c.name c.name c.name c.name;
|
||||
end
|
||||
)
|
||||
channels;
|
||||
printf " last_radio_contains_avg_channels = TRUE;\\\n";
|
||||
printf " }\\\n";
|
||||
printf "}\n"
|
||||
|
||||
|
||||
|
||||
let _ =
|
||||
if Array.length Sys.argv < 2 then
|
||||
failwith "Usage: gen_radio xml_file";
|
||||
@@ -71,19 +126,19 @@ let _ =
|
||||
define "RADIO_CTL_NB" (string_of_int (List.length channels));
|
||||
nl ();
|
||||
|
||||
(* For compatibility *)
|
||||
define "PPM_PULSE_NEUTRAL_US" default_neutral;
|
||||
nl ();
|
||||
let channels_params = List.map parse_channel channels in
|
||||
nl ();
|
||||
define "RADIO_MINS_US" (sprint_float_array (List.map (fun x -> x.min) channels_params));
|
||||
define "RADIO_NEUTRALS_US" (sprint_float_array (List.map (fun x -> x.neutral) channels_params));
|
||||
define "RADIO_MAXS_US" (sprint_float_array (List.map (fun x -> x.max) channels_params));
|
||||
define "RADIO_AVERAGED" (sprint_float_array (List.map (fun x -> x.averaged) channels_params));
|
||||
define "RADIO_NEUTRALS_PPM" (sprint_float_array (List.map (fun x -> string_of_int ((int_of_string x.neutral)*16)) channels_params));
|
||||
define "RADIO_TRAVEL_PPM" (sprint_float_array (List.map (fun x -> string_of_float (9600. *. 2. /. (float ((int_of_string x.max) - (int_of_string x.min))) /. 16.)) channels_params));
|
||||
|
||||
let ppm_min = ExtXml.attrib xml "min" in
|
||||
let ppm_max = ExtXml.attrib xml "max" in
|
||||
let ppm_sync= ExtXml.attrib xml "sync" in
|
||||
|
||||
printf "#define PPM_MIN_PULSE_WIDTH %sul*CLOCK\n" ppm_min;
|
||||
printf "#define PPM_MAX_PULSE_WIDTH %sul*CLOCK\n" ppm_max;
|
||||
printf "#define PPM_SYNC_PULSE (uint8_t)(((uint32_t)(%sul*CLOCK))/1024ul)\n" ppm_sync;
|
||||
nl ();
|
||||
define "AveragedChannel(ch)" "(((int[])RADIO_AVERAGED)[ch])";
|
||||
|
||||
gen_last_radio_from_ppm channels_params;
|
||||
|
||||
printf "\n#endif // %s\n" h_name
|
||||
|
||||
|
||||
Reference in New Issue
Block a user