Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

ocaml sim: fix reading of parameters from airframe xml file, was missing unit conversions

Browse Source
This commit is contained in:
Felix Ruess
2012-01-30 23:21:08 +01:00
parent 21e1feb9ca
commit eadde440d0
1 changed files with 40 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/flightModel.ml
+40 -8
View File
@@ -87,18 +87,50 @@ module Make(A:Data.MISSION) = struct
Not_found ->
failwith (Printf.sprintf "Child 'section' with 'name=%s' expected in '%s'\n" name (Xml.to_string A.ac.airframe))
let tag_define = fun sect name ->
try
(ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define")
with
Not_found ->
failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect))
let defined_value = fun sect name ->
try
(Xml.attrib (ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define") "value")
(Xml.attrib (tag_define sect name) "value")
with
Not_found ->
failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect))
Not_found ->
failwith (Printf.sprintf "Child 'define' with 'name=%s' in '%s' has no value\n" name (Xml.to_string sect))
let float_value = fun section s ->
let x = (defined_value section s) in
try float_of_string x with Failure "float_of_string" ->
failwith (sprintf "float_of_string: %s" x)
(* FIXME: refactor code_unit_scale of tag to pprz.ml *)
let code_unit_scale_of_tag = function t ->
(* if unit attribute is not specified don't even attempt to convert the units *)
let u = try ExtXml.attrib t "unit" with _ -> failwith "Unit conversion error" in
let cu = try ExtXml.attrib t "code_unit" with _ -> "" in
(* default value for code_unit is rad[/s] when unit is deg[/s] *)
try match (u, cu) with
("deg", "") -> Pprz.scale_of_units u "rad" (* implicit conversion to rad *)
| ("deg/s", "") -> Pprz.scale_of_units u "rad/s" (* implicit conversion to rad/s *)
| (_, "") -> failwith "Unit conversion error" (* code unit is not defined and no implicit conversion *)
| (_,_) -> Pprz.scale_of_units u cu (* try to convert *)
with
Pprz.Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; exit 1
| Pprz.Unknown_conversion (su, scu) -> prerr_endline (sprintf "Warning: unknown unit conversion: from %s to %s" su scu); flush stderr; failwith "Unknown unit conversion"
| _ -> failwith "Unit conversion error"
let code_value = fun section s ->
let t = (tag_define section s) in
try
let coef = try (code_unit_scale_of_tag t) with _ -> 1. in
(ExtXml.float_attrib t "value") *. coef
with
_ ->
failwith (Printf.sprintf "Can't convert 'define' with 'name=%s' in '%s' to floating point value\n" s (Xml.to_string section))
let simu_section =
try section "SIMU" with _ -> Xml.Element("", [], [])
@@ -139,12 +171,12 @@ module Make(A:Data.MISSION) = struct
let infrared_section = try section "INFRARED" with _ -> Xml.Element("",[],[])
let nominal_airspeed = float_of_string (defined_value misc_section "NOMINAL_AIRSPEED")
let maximum_airspeed = try float_value misc_section "MAXIMUM_AIRSPEED" with _ -> nominal_airspeed *. 1.5
let max_power = try float_value misc_section "MAXIMUM_POWER" with _ -> 5. *. maximum_airspeed *. weight
let nominal_airspeed = code_value misc_section "NOMINAL_AIRSPEED"
let maximum_airspeed = try code_value misc_section "MAXIMUM_AIRSPEED" with _ -> nominal_airspeed *. 1.5
let max_power = try code_value misc_section "MAXIMUM_POWER" with _ -> 5. *. maximum_airspeed *. weight
let roll_neutral_default = try float_value infrared_section "ROLL_NEUTRAL_DEFAULT" with _ -> 0.
let pitch_neutral_default = try float_value infrared_section "PITCH_NEUTRAL_DEFAULT" with _ -> 0.
let roll_neutral_default = try code_value infrared_section "ROLL_NEUTRAL_DEFAULT" with _ -> 0.
let pitch_neutral_default = try code_value infrared_section "PITCH_NEUTRAL_DEFAULT" with _ -> 0.
let vert_ctrl_section = try section "VERTICAL CONTROL" with _ -> Xml.Element("",[],[])