deeper and deeper

Makefile.ac

@@ -26,13 +26,14 @@ CONF=$(PAPARAZZI_HOME)/conf
 CONF_XML=$(CONF)/conf.xml
 ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)
 AIRFRAME_H=$(ACINCLUDE)/airframe.h
+CONTROL_H=$(ACINCLUDE)/control.h
 RADIO_H=$(ACINCLUDE)/radio.h
 FLIGHT_PLAN_H=$(ACINCLUDE)/flight_plan.h
 FLIGHT_PLAN_XML=$(ACINCLUDE)/flight_plan.xml
 INFLIGHT_CALIB_H=$(ACINCLUDE)/inflight_calib.h
 MAKEFILE_AC=$(ACINCLUDE)/Makefile.ac
 
-all: $(AIRFRAME_H) $(RADIO_H) $(FLIGHT_PLAN_H) $(FLIGHT_PLAN_XML) $(INFLIGHT_CALIB_H) $(MAKEFILE_AC)
+all: $(AIRFRAME_H) $(RADIO_H) $(CONTROL_H) $(FLIGHT_PLAN_H) $(FLIGHT_PLAN_XML) $(INFLIGHT_CALIB_H) $(MAKEFILE_AC)
 
 
 $(AIRFRAME_H) : $(CONF)/$(AIRFRAME) $(CONF_XML)
@@ -43,6 +44,10 @@ $(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML)
 	$(TOOLS)/gen_radio.out $< > /tmp/radio.h
 	mv /tmp/radio.h $@
 
+$(CONTROL_H) : $(CONF)/$(AIRFRAME) $(CONF_XML)
+	$(TOOLS)/gen_control.out $< > /tmp/control.h
+	mv /tmp/control.h $@
+
 $(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML)
 	$(TOOLS)/gen_flight_plan.out $< > /tmp/fp.h
 	mv /tmp/fp.h $@

conf/airframes/gorrazoptere_esc_3DMG_test.xml

@@ -32,40 +32,50 @@
 
 
 <control>
-  <level name="rates">
-    <loop name="roll_dot" type="P" pgain="100" measure="estimator_roll_dot" setpoint="setpoint_rolldot" output="command_roll_dot"/>
-    <loop name="pitch_dot" type="PD" pgain="100" dgain="200" measure="estimator_pitch_dot" setpoint="setpoint_picthdot" output="command_pitch_dot"/>
+  <level name="rate">
+    <loop name="rolldot" loop_type="P" pgain="100" saturation = "MAX_PPRZ"
+      data_type = "int16_t"
+      measure="estimator_rolldot" setpoint="control_rolldot_setpoint" output="command_roll_dot"/>
+    <loop name="pitchdot" loop_type="PD" pgain="100" dgain="200" saturation = "MAX_PPRZ"
+      data_type = "int16_t"
+      measure="estimator_pitch_dot" setpoint="setpoint_pitchdot" output="command_pitch_dot"/>
   </level>
 
-  <level name="attitudes">
-    <loop name="roll" type="P" pgain="100" measure="estimator_roll" setpoint="setpoint_roll" output="setpoint_rolldot"/>
-    <loop name="pitch" type="PD" pgain="100" dgain="200" measure="estimator_pitch" setpoint="setpoint_picth" output="setpoint_pitchdot"/>
+  <level name="attitude">
+    <loop name="roll" loop_type="P" pgain="100" saturation = "444"
+       data_type = "int16_t"
+       measure="estimator_roll" setpoint="control_roll_setpoint" output="setpoint_rolldot"/>
+    <loop name="pitch" loop_type="PID" pgain="100" dgain="200" igain="300" saturation = "444" integral_saturation = "777"
+       data_type = "int16_t"
+       measure="estimator_pitch" setpoint="control_setpoint_pitch" output="control_setpoint_pitchdot"/>
   </level>
 
-
-
   <mode name="MANUAL">
-    <input input="rc_values[COMMAND_THROTTLE]" output="control_commands[RADIO_THROTTLE]"/>
-    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_rolldot"/>
-    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitchdot"/>
-    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot"/>
+    <input input="rc_values[RADIO_THROTTLE]" output="control_commands[COMMAND_THROTTLE]" range="1"/>
+    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_rolldot" range="(1. * 32768000. / 8500. / 9600.)"/>
+    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitchdot" range="(1. * 32768000. / 8500. / 9600.)"/>
+    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot" range="(1. * 32768000. / 8500. / 9600.)"/>
+    <run name="rate"/>
   </mode>
 
   <mode name="AUTO1">
-    <input input="rc_values[COMMAND_THROTTLE]" output="control_commands[RADIO_THROTTLE]"/>
-    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_roll"/>
-    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitch"/>
-    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot"/>
+    <input input="rc_values[RADIO_THROTTLE]" output="control_commands[COMMAND_THROTTLE]" range="1"/>
+    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_roll" range="1"/>
+    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitch" range="1"/>
+    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot" range="1"/>
+    <run name="rate"/>
+    <run name="attitude"/>
   </mode>
 
   <mode name="AUTO2">
-    <input input="rc_values[COMMAND_THROTTLE]" output="control_commands[RADIO_THROTTLE]"/>
-    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_roll"/>
-    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitch"/>
-    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot"/>
+    <input input="rc_values[RADIO_THROTTLE]" output="control_commands[COMMAND_THROTTLE]" range="1"/>
+    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_roll" range="1"/>
+    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitch" range="1"/>
+    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot" range="1"/>
   </mode>
 
 </control>
-
-
+  <makefile>
+include $(PAPARAZZI_SRC)/conf/autopilot/disc_board.makefile
+ </makefile>
 </airframe>

conf/autopilot/disc_board.makefile

@@ -8,5 +8,5 @@ ap.LOW_FUSE  = a0
 ap.HIGH_FUSE = 99
 ap.EXT_FUSE  = ff
 ap.LOCK_FUSE = ff
-ap.CFLAGS += -DAP -DFBW
-ap.srcs = $(SRC_ARCH)/ppm.c $(SRC_ARCH)/adc_ap.c $(SRC_ARCH)/uart_ap.c $(SRC_ARCH)/esc.c main_fbw.c inter_mcu.c pid.c estimator.c if_calib.c nav.c main_ap.c mainloop.c main.c
+ap.CFLAGS += -DAP -DFBW -DCONFIG=\"config_discboard.h\"
+ap.srcs = sys_time.c main_fbw_2.c main_ap_2.c main.c

sw/include/std.h

@@ -66,6 +66,7 @@ typedef uint8_t bool_t;
 
 #define Bound(_x, _min, _max) { if (_x > _max) _x = _max; if (_x < _min) _x = _min; }
 #define BoundAbs(_x, _max) Bound(_x, -_max, _max)
-#define Chop(x, min, max) ( (x) < (min) ? (min) : (x) > (max) ? (max) : (x) )
+#define Chop(_x, _min, _max) ( (_x) < (_min) ? (_min) : (_x) > (_max) ? (_max) : (_x) )
+#define ChopAbs(x, max) Chop(x, -max, max)
 
 #endif /* STD_H */

sw/tools/Makefile

@@ -3,7 +3,7 @@ OCAMLC=ocamlc -I ../lib/ocaml
 OCAMLLEX=ocamllex
 OCAMLYACC=ocamlyacc
 
-all: gen_aircraft.out gen_airframe.out gen_calib.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out gen_sim_downlink.out gen_dl.out extract_makefile.out
+all: gen_aircraft.out gen_airframe.out gen_calib.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out gen_sim_downlink.out gen_dl.out extract_makefile.out gen_control.out
 
 FP_CMO = fp_syntax.cmo fp_parser.cmo fp_lexer.cmo fp_proc.cmo gen_flight_plan.ml
 ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%)

sw/tools/gen_airframe.ml

@@ -146,7 +146,8 @@ let parse_section = fun s ->
   | "makefile" ->
       ()
       (** Ignoring this section *)
-  | _ -> xml_error (Printf.sprintf "[%s] section|servos|command_laws|conmmands|makefile" (Xml.tag s))
+  | _ -> ignore()
+(*xml_error (Printf.sprintf "[%s] section|servos|command_laws|conmmands|makefile" (Xml.tag s)) *)
       
 
 let h_name = "AIRFRAME_H"

sw/tools/gen_control.ml

@@ -1,41 +1,102 @@
 open Printf
 open Xml2h
 
+let fos = float_of_string
+
+type loop = { 
+    name : string; 
+    loop_type : string; 
+    pgain : float; 
+    dgain : float; 
+    igain : float; 
+    isat : string;
+    data_type : string;  
+    measure : string; 
+    setpoint : string; 
+    output : string;
+    sat : string;
+  }
+
 let parse_input = fun s ->
   match Xml.tag s with
     "input" ->
       let input = ExtXml.attrib s "input"
-      and output = ExtXml.attrib s "output" in
-      printf "  %s = %s;\n" output input 
+      and output = ExtXml.attrib s "output" 
+      and range =  ExtXml.attrib s "range" in
+      printf "  %s = %s * %s;\n" output input range
   | _ -> ignore ()
 
-
-
-let parse_loop = fun s ->
+let parse_loop = fun s list ->
   match Xml.tag s with
     "loop" ->
-      let name = ExtXml.attrib s "name"
-      and loop_type = ExtXml.attrib s "type"
-      and meas = ExtXml.attrib s "type" in
-      printf "name %s\n" name
-  | _ -> ignore ()
+      let na = ExtXml.attrib s "name"
+      and lt = ExtXml.attrib s "loop_type"
+      and pg = fos (ExtXml.attrib s "pgain")
+      and dg = fos (ExtXml.attrib_or_default s "dgain" "0")
+      and ig = fos (ExtXml.attrib_or_default s "igain" "0")
+      and is = ExtXml.attrib_or_default s "integral_saturation" "0"
+      and dt = ExtXml.attrib s "data_type" 
+      and mea = ExtXml.attrib s "measure" 
+      and sp = ExtXml.attrib s "setpoint" 
+      and op = ExtXml.attrib s "output" 
+      and sa = ExtXml.attrib s "saturation" in
+      let param = {name = na; loop_type = lt; 
+		   pgain = pg; dgain = dg; igain = ig; isat = is; 
+		   data_type = dt; measure = mea; setpoint = sp; output = op; sat = sa} in
+      param::list
+  | _ -> list
 
 
+let print_loop_declaration = fun lp ->
+  printf "static extern %s control_%s_setpoint = 0;\n" lp.data_type lp.name;
+  printf "static extern float control_%s_pgain = %f;\n" lp.name lp.pgain;
+  if Str.string_match (Str.regexp ".*D.*") lp.loop_type 0 then
+    begin
+      printf "static extern %s control_%s_last_err = %f;\n" lp.data_type lp.name lp.dgain;
+      printf "static extern float control_%s_dgain = %f;\n" lp.name lp.dgain
+    end;
+  if Str.string_match (Str.regexp ".*I.*") lp.loop_type 0 then
+    begin
+      printf "static extern %s control_%s_sum_err = 0;\n" lp.data_type lp.name;
+      printf "static extern float control_%s_igain = %f;\n" lp.name lp.igain
+    end;
+  nl()
+
+let print_loop_code = fun lp ->
+  printf "  {\n";
+  printf "    %s err = %s - %s;\n" lp.data_type lp.measure lp.setpoint;
+  if Str.string_match (Str.regexp ".*D.*") lp.loop_type 0 then
+    begin
+      printf "    %s d_err = err - control_%s_last_err;\n" lp.data_type lp.name;
+      printf "    control_%s_last_err = err;\n" lp.name
+    end;
+  if Str.string_match (Str.regexp ".*I.*") lp.loop_type 0 then
+      printf "    control_%s_sum_err += err;\n" lp.name;
+  if Str.string_match (Str.regexp "^P$") lp.loop_type 0 then
+    printf "    %s = ChopAbs(%f * err, %s);\n" lp.output lp.pgain lp.sat;
+  if Str.string_match (Str.regexp "^PD$") lp.loop_type 0 then
+    printf "    %s = ChopAbs(%f * (err + %f * d_err), %s);\n" lp.output lp.pgain lp.dgain lp.sat;
+  if Str.string_match (Str.regexp "^PID$") lp.loop_type 0 then
+    printf "    %s =  ChopAbs(%f * (err + %f * d_err + %f * control_%s_sum_err), %s);\n" lp.output lp.pgain lp.dgain lp.igain lp.name lp.sat;
+  printf "  }\n"
 
 let parse_control = fun s ->
   match Xml.tag s with
     "level" ->
+      let loops_params = List.fold_right parse_loop (Xml.children s) [] in
+      List.iter print_loop_declaration loops_params;
+      nl();
       let level_name = ExtXml.attrib s "name" in
       printf "static inline void control_run_%s_loops ( void ) {\n" level_name;
-      List.iter parse_loop (Xml.children s);
+      List.iter print_loop_code loops_params;
       printf "}\n";
-      nl();
+      nl()
   | "mode" ->
       let mode_name = ExtXml.attrib s "name" in
       printf "static inline void control_process_radio_control_%s ( void ) {\n" mode_name;
       List.iter parse_input (Xml.children s);
       printf "}\n";
-      nl();
+      nl()
   | _ -> ignore ()
 
 let parse_section = fun s ->
@@ -53,6 +114,8 @@ let _ =
   let xml_file = Sys.argv.(1) in
   try
     let xml = start_and_begin xml_file h_name in
+    printf "#include \"std.h\"\n";
+    nl();
     List.iter parse_section (Xml.children xml);
     finish h_name
   with