Skip to content

Commit

Permalink
actuators[ ] array in pprz units, conversion to actuator_driver units…
Browse files Browse the repository at this point in the history 
… when sending to the actuator_driver.

actuators becomes an array
  • Loading branch information
@dewagter
dewagter committed Dec 18, 2023
1 parent 94c79d4 commit e495727
Show file tree
Hide file tree
Showing 5 changed files with 34 additions and 20 deletions.
16 changes: 8 additions & 8 deletions conf/settings/setup_actuators.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,14 +5,14 @@
<settings target="setup_actuators">
<dl_settings>
<dl_settings NAME="Actuators" >
<dl_setting VAR="actuators[0]" MIN="900" STEP="1" MAX="2100" module="modules/actuators/actuators" shortname="chan0"/>
<dl_setting VAR="actuators[1]" MIN="900" STEP="1" MAX="2100" shortname="chan1"/>
<dl_setting VAR="actuators[2]" MIN="900" STEP="1" MAX="2100" shortname="chan2"/>
<dl_setting VAR="actuators[3]" MIN="900" STEP="1" MAX="2100" shortname="chan3"/>
<dl_setting VAR="actuators[4]" MIN="900" STEP="1" MAX="2100" shortname="chan4"/>
<dl_setting VAR="actuators[5]" MIN="900" STEP="1" MAX="2100" shortname="chan5"/>
<dl_setting VAR="actuators[6]" MIN="900" STEP="1" MAX="2100" shortname="chan6"/>
<dl_setting VAR="actuators[7]" MIN="900" STEP="1" MAX="2100" shortname="chan7"/>
<dl_setting VAR="actuators[0].driver_val" MIN="900" STEP="1" MAX="2100" module="modules/actuators/actuators" shortname="chan0"/>
<dl_setting VAR="actuators[1].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan1"/>
<dl_setting VAR="actuators[2].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan2"/>
<dl_setting VAR="actuators[3].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan3"/>
<dl_setting VAR="actuators[4].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan4"/>
<dl_setting VAR="actuators[5].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan5"/>
<dl_setting VAR="actuators[6].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan6"/>
<dl_setting VAR="actuators[7].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan7"/>
</dl_settings>
</dl_settings>
</settings>
9 changes: 7 additions & 2 deletions sw/airborne/modules/actuators/actuators.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,11 +42,16 @@

static void send_actuators(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
// Downlink the actuators raw driver values
int16_t v[ACTUATORS_NB] = {0};
for (int i = 0; i < ACTUATORS_NB; i++) {
v[i] = actuators[i].driver_val;
}
pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, v);
}
#endif

int16_t actuators[ACTUATORS_NB];
struct actuator_t actuators[ACTUATORS_NB];

// Can be used to directly control each actuator from the control algorithm
int16_t actuators_pprz[ACTUATORS_NB];
Expand Down
11 changes: 9 additions & 2 deletions sw/airborne/modules/actuators/actuators.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,17 @@ struct act_feedback_t {
extern uint32_t actuators_delay_time;
extern bool actuators_delay_done;

// Actuator feedback structure for ABI Message
struct actuator_t {
pprz_t pprz_val; ///< Actuator value in PPRZ units
int16_t driver_val; ///< Actuator value in driver units (scaling from servo in airframe.h)
};


/** Actuators array.
* Temporary storage (for debugging purpose, downlinked via telemetry)
* */
extern int16_t actuators[ACTUATORS_NB];
extern struct actuator_t actuators[ACTUATORS_NB];

/** PPRZ command to each actuator
* Can be used to directly control actuators from the control algorithm
Expand All @@ -72,7 +79,7 @@ extern int16_t actuators_pprz[ACTUATORS_NB];
* @param _n actuators name as given in airframe file, servos section
* @param _v new actuator's value
*/
#define _ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v)
#define _ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v,_v)
#define ActuatorSet(_n, _v) _ActuatorSet(_n, _v)

#endif /* ACTUATORS_NB */
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/loggers/logger_control_effectiveness.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,7 +129,7 @@ void logger_control_effectiveness_periodic(void)
// log actuators
#if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
for (unsigned int i = 0; i < ACTUATORS_NB; i++) {
sdLogWriteLog(pprzLogFile, ",%d", actuators[i]);
sdLogWriteLog(pprzLogFile, ",%d", actuators[i].pprz_val);
}
#endif

Expand Down
16 changes: 9 additions & 7 deletions sw/tools/generators/gen_airframe.ml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,9 +218,10 @@ let preprocess_value = fun s v prefix ->
let print_actuators_idx = fun out ->
Hashtbl.iter (fun s (d, 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_DRIVER_NO, actuators[SERVO_%s_IDX]); \\\n" d s s;
fprintf out "#define Set_%s_Servo(_pprzv,_s) { \\\n" s;
fprintf out " actuators[SERVO_%s_IDX].pprz_val = ClipAbs( _pprzv, MAX_PPRZ); \\\n" s;
fprintf out " actuators[SERVO_%s_IDX].driver_val = Clip(_s, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s;
fprintf out " Actuator%sSet(SERVO_%s_DRIVER_NO, actuators[SERVO_%s_IDX].driver_val); \\\n" d s s;
fprintf out "}\n\n"
) servos_drivers;
define_out out "ACTUATORS_NB" (string_of_int (Hashtbl.length servos_drivers));
Expand All @@ -233,11 +234,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
Expand Down Expand Up @@ -364,6 +365,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";
Expand Down

0 comments on commit e495727

Please sign in to comment.