actuators[ ] array in pprz units, conversion to actuator_driver units…

… when sending to the actuator_driver.
paparazzi · Dec 14, 2023 · 3536282 · 3536282
1 parent f7a981a
commit 3536282
Showing 1 changed file with 8 additions and 7 deletions.
diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml
@@ -209,9 +209,9 @@ let print_actuators_idx = fun out ->
   Hashtbl.iter (fun s (d, i) ->
     fprintf out "#define SERVO_%s_IDX %d\n" s i;
     (* Set servo macro *)
-    fprintf out "#define Set_%s_Servo(_v) { \\\n" s;
-    fprintf out "  actuators[SERVO_%s_IDX] = Clip(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s;
-    fprintf out "  Actuator%sSet(SERVO_%s, actuators[SERVO_%s_IDX]); \\\n" d s s;
+    fprintf out "#define Set_%s_Servo(_pprzv,_s) { \\\n" s;
+    fprintf out "  actuators[SERVO_%s_IDX] = _pprzv; \\\n" s;
+    fprintf out "  Actuator%sSet(SERVO_%s, Clip(_s, SERVO_%s_MIN, SERVO_%s_MAX)); \\\n" d s s s;
     fprintf out "}\n\n"
   ) servos_drivers;
   define_out out "ACTUATORS_NB" (string_of_int (Hashtbl.length servos_drivers));
@@ -224,11 +224,11 @@ let parse_command_laws = fun out command ->
         let servo = a "servo"
         and value = a "value" in
         let v = preprocess_value value "values" "COMMAND" in
-        fprintf out "  command_value = %s; \\\n" v;
-        fprintf out "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM; \\\n" servo servo;
-        fprintf out "  command_value /= command_value>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" servo servo;
+        fprintf out "  actuator_value_pprz = %s; \\\n" v;
+        fprintf out "  command_value = actuator_value_pprz * (actuator_value_pprz>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM); \\\n" servo servo;
+        fprintf out "  command_value /= actuator_value_pprz>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" servo servo;
         fprintf out "  servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" servo;
-        fprintf out "  Set_%s_Servo(servo_value); \\\n\\\n" servo
+        fprintf out "  Set_%s_Servo(actuator_value_pprz, servo_value); \\\n\\\n" servo
     | "let" ->
       let var = a "var"
       and value = a "value" in
@@ -357,6 +357,7 @@ let rec parse_section = fun out ac_id s ->
       fprintf out "#define SetActuatorsFromCommands(values, AP_MODE) { \\\n";
       fprintf out "  int32_t servo_value;\\\n";
       fprintf out "  int32_t command_value;\\\n\\\n";
+      fprintf out "  int32_t actuator_value_pprz;\\\n\\\n";
       List.iter (parse_command_laws out) (Xml.children s);
       fprintf out "  AllActuatorsCommit(); \\\n";
       fprintf out "}\n\n";