8 changes: 8 additions & 0 deletions sw/airborne/modules/ctrl/eff_scheduling_generic.c
View file
Expand Up @@ -37,19 +37,27 @@
#error "You need to define an RC channel to switch between simple and advanced scheduling"
#endif

#ifdef FWD_G1
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = FWD_G1;
#else
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {
FWD_G1_ROLL,
FWD_G1_PITCH,
FWD_G1_YAW,
FWD_G1_THRUST
};
#endif

#ifdef STABILIZATION_INDI_G1
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
#else
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH,
STABILIZATION_INDI_G1_YAW,
STABILIZATION_INDI_G1_THRUST
};
#endif

static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING

Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
View file
Expand Up @@ -72,7 +72,11 @@ float backwing_pitch_eff_scaling = 1.0;

static float g_forward[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL_FWD, STABILIZATION_INDI_G1_PITCH_FWD, STABILIZATION_INDI_G1_YAW_FWD, STABILIZATION_INDI_G1_THRUST_FWD};

#ifdef STABILIZATION_INDI_G1
static float g_hover[4][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
#else
static float g_hover[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST};
#endif

// Functions to schedule switching on and of of tip props on front wing
float sched_ratio_tip_props = 1.0;
Expand Down
17 changes: 15 additions & 2 deletions sw/lib/ocaml/module.ml
View file
Expand Up @@ -61,23 +61,36 @@ let parse_configure = fun xml ->
{ cname = find_name xml; cvalue = get "value"; case = get "case";
default = get "default"; cdescription = get "description" }

type field = {
finame: string option;
fivalue: string option;
fitype: string option
}

type define = {
dname: string;
dvalue: string option;
integer: int option;
dunit: string option;
dtype: string option;
ddescription: string option;
cond: string option
cond: string option;
fields: field list
}

let parse_field = fun xml ->
{ finame = ExtXml.attrib_opt xml "name";
fivalue = ExtXml.attrib_opt xml "value";
fitype = ExtXml.attrib_opt xml "type" }

let parse_define = fun xml ->
let get = fun x -> ExtXml.attrib_opt xml x in
{ dname = find_name xml; dvalue = get "value";
integer = begin match get "integer" with
| None -> None | Some i -> Some (int_of_string i) end;
dunit = get "unit"; dtype = get "type";
ddescription = get "description"; cond = get "cond" }
ddescription = get "description"; cond = get "cond";
fields = List.map parse_field (Xml.children xml) }

type incl = { element: string; condition: string option }

Expand Down
18 changes: 11 additions & 7 deletions sw/tools/calibration/calibration_utils.py
View file
Expand Up @@ -192,14 +192,18 @@ def print_xml(p, sensor, sensor_id, res):
y_sens = continious_frac(p[4]*2**res)
z_sens = continious_frac(p[5]*2**res)

struct = "{{.abi_id="+sensor_id+", .calibrated={.neutral=true, .scale=true},"
struct += ".neutral={"+str(int(round(p[0])))+","+str(int(round(p[1])))+","+str(int(round(p[2])))+"}, "
struct += ".scale={{"+str(x_sens[0])+","+str(y_sens[0])+","+str(z_sens[0])+"},"
struct += "{"+str(x_sens[1])+","+str(y_sens[1])+","+str(z_sens[1])+"}}"
struct += "}}"

print("")
print("<define name=\"IMU_"+sensor+"_CALIB\" value=\""+struct+"\"/>")
print('<define name="IMU_'+sensor+'_CALIB" type="array"/>')
print(' <field type="struct"/>')
print(' <field name="abi_id" value="'+sensor_id+'"/>')
print(' <field name="calibrated" type="struct">')
print(' <field name="neutral" value="true"/>')
print(' <field name="scale" value="true"/>')
print(' </field>')
print(' <field name="neutral" value="'+str(int(round(p[0])))+','+str(int(round(p[1])))+','+str(int(round(p[2])))+'" type="int[]"/>')
print(' <field name="scale" value="{{'+str(x_sens[0])+','+str(y_sens[0])+','+str(z_sens[0])+'},{'+str(x_sens[1])+','+str(y_sens[1])+','+str(z_sens[1])+'}}"/>')
print(' </field>')
print('</define>')
print("")
print("<define name=\""+sensor+"_X_NEUTRAL\" value=\""+str(int(round(p[0])))+"\"/>")
print("<define name=\""+sensor+"_Y_NEUTRAL\" value=\""+str(int(round(p[1])))+"\"/>")
Expand Down
113 changes: 83 additions & 30 deletions sw/tools/generators/gen_airframe.ml
View file
Expand Up @@ -94,51 +94,104 @@ let convert_value_with_code_unit_coef_of_xml = function xml ->
with _ -> prerr_endline (sprintf "Error: Unit conversion of parameter %s impossible because '%s' is not a float" (Xml.attrib xml "name") (Xml.attrib xml "value")); flush stderr; exit 1 in
v *. conv


type val_t =
| Value of string
| XmlFields of Xml.xml list

let get_value_converted = fun x ->
(* fail if units conversion is not found and just copy value instead,
this is important for integer values, you can't just multiply them with 1.0 *)
try string_of_float (convert_value_with_code_unit_coef_of_xml x)
with _ -> ExtXml.display_entities (ExtXml.attrib x "value")

let array_sep = Str.regexp "[,;]"
let rec string_from_type = fun name v t ->
let sprint_array = fun v t ->
let vs = Str.split array_sep v in
let sl = List.map (fun vl -> string_from_type name vl t) vs in
"{ "^(String.concat " , " sl)^" }"
in

let rec string_from_type = fun name t x ->
(* error function *)
let error_and_exit = fun e -> prerr_endline e; flush stderr; exit 1 in
(* utility function to remove spaces *)
let rm_leading_trailing_spaces = fun s ->
let s = Str.global_replace (Str.regexp "^ *") "" s in
Str.global_replace (Str.regexp " *$") "" s
in
match t with
| "float" ->
begin
try
string_of_float (float_of_string (rm_leading_trailing_spaces v))
with _ -> prerr_endline (sprintf "Define value %s = %s is not compatible with type float" name v); flush stderr; exit 1
end
| "int" ->
begin
(* test valid cases and extract value or fields *)
let v = try Some (Xml.attrib x "value") with _ -> None in
let c = if List.length (Xml.children x) = 0 then None else Some (Xml.children x) in
let _val = match v, c with
| None, None -> error_and_exit (sprintf "Define %s requires 'value' attribute or 'field' node" name)
| Some _, Some _ -> error_and_exit (sprintf "Define %s can't support both 'value' attribute or 'field' node" name)
| Some _v, None -> Value (get_value_converted x)
| None, Some _c -> XmlFields _c
in
(* extract values as string *)
let rec value_from_type = fun t _val ->
(* printers for array-like and structures *)
let sprint_array t = function
| Value v ->
let vs = Str.split array_sep v in
let sl = List.map (fun vl -> value_from_type t (Value vl)) vs in
"{ "^(String.concat " , " sl)^" }"
| XmlFields xfs ->
let sl = List.map (fun x -> value_from_type t (XmlFields (Xml.children x))) xfs in
"{ "^(String.concat " , " sl)^" }"
in
let sprint_matrix = fun l ->
let fl = List.map (fun f -> string_from_type name (ExtXml.attrib_or_default f "type" "") f) l in
"{ "^(String.concat " , " fl)^" }"
in
let sprint_struct = fun l ->
let fl = List.map (fun f ->
try
string_of_int (int_of_string (rm_leading_trailing_spaces v))
with _ -> prerr_endline (sprintf "Define value %s = %s is not compatible with type int" name v); flush stderr; exit 1
let n = ExtXml.attrib f "name" in
sprintf ".%s = %s" n (string_from_type name (ExtXml.attrib_or_default f "type" "") f)
with _ ->
sprintf "%s" (string_from_type name (ExtXml.attrib_or_default f "type" "") f)
) l in
"{ "^(String.concat " , " fl)^" }"
in
(* match correct types *)
match _val with
| XmlFields xfs -> begin
match t with
| "matrix" | "array" -> sprint_matrix (Xml.children x)
| "struct" -> sprint_struct (Xml.children x)
| _ -> error_and_exit (sprintf "Unknown define type for fields of %s (specify 'matrix', 'array' or 'struct')" name)
end
| "string" -> "\""^(rm_leading_trailing_spaces v)^"\""
| "array" -> sprint_array v ""
| "float[]" -> sprint_array v "float"
| "int[]" -> sprint_array v "int"
| "string[]" -> sprint_array v "string"
| _ -> v
| Value v -> begin
match t with
| "float" ->
begin
try
string_of_float (float_of_string (rm_leading_trailing_spaces v))
with _ -> error_and_exit (sprintf "Define value %s = %s is not compatible with type float" name v);
end
| "int" ->
begin
try
string_of_int (int_of_string (rm_leading_trailing_spaces v))
with _ -> error_and_exit (sprintf "Define value %s = %s is not compatible with type int" name v);
end
| "string" -> "\""^(rm_leading_trailing_spaces v)^"\""
| "array" -> sprint_array "" (Value v)
| "float[]" -> sprint_array "float" (Value v)
| "int[]" -> sprint_array "int" (Value v)
| "string[]" -> sprint_array "string" (Value v)
| "matrix" -> sprint_matrix (Xml.children x)
| "struct" -> sprint_struct (Xml.children x)
| _ -> v
end
in
value_from_type t _val


let parse_element = fun out prefix s ->
match Xml.tag s with
"define" -> begin
try
(* fail if units conversion is not found and just copy value instead,
this is important for integer values, you can't just multiply them with 1.0 *)
let value =
try string_of_float (convert_value_with_code_unit_coef_of_xml s)
with _ -> ExtXml.display_entities (ExtXml.attrib s "value")
in
let name = (prefix^ExtXml.attrib s "name") in
let t = ExtXml.attrib_or_default s "type" "" in
define_out out name (string_from_type name value t);
define_out out name (string_from_type name t s);
define_integer out name (ExtXml.float_attrib s "value") (ExtXml.int_attrib s "integer");
with _ -> ();
end
Expand Down