Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

actuators[ ] array of structs with pprz units and driver units #3205

Merged
merged 3 commits into from
May 25, 2024
Merged

actuators[ ] array of structs with pprz units and driver units #3205

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7b77caa
actuators[ ] array in pprz units, conversion to actuator_driver units…
dewagter Dec 14, 2023
c40eea7
Moved scaling of the Servo to the Set function.
dewagter May 24, 2024
00f490f
Fixed switches and pan-tilt to work in PPRZ units now. Simplified code.
dewagter May 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 2 additions & 2 deletions conf/airframes/CDW/cdw_mavtec.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,8 @@

<section name="SWITCH_SERVO">
<define name="SWITCH_SERVO_SERVO" value="DROP"/>
<define name="SWITCH_SERVO_ON_VALUE" value="SERVO_DROP_MIN"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_DROP_MAX"/>
<define name="SWITCH_SERVO_ON_VALUE" value="MIN_PPRZ"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="MAX_PPRZ"/>
<define name="DropOpen()" value="SwitchServoOn()"/>
<define name="DropClose()" value="SwitchServoOff()"/>
</section>
Expand Down
2 changes: 0 additions & 2 deletions conf/airframes/ENAC/quadrotor/cobra.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,6 @@
<module name="actuators" type="dshot"/>
<module name="actuators" type="pwm"/>
<module name="switch" type="servo"/>
<define name="SWITCH_SERVO_ON_VALUE" value="SERVO_SWITCH_MAX"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_SWITCH_MIN"/>

<module name="board" type="tawaki">
<define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_256HZ"/>
Expand Down
4 changes: 2 additions & 2 deletions conf/airframes/ENAC/quadrotor/cobraV2.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,8 @@
</module>
<module name="actuators" type="pwm"/>
<module name="switch" type="servo"/>
<define name="SWITCH_SERVO_ON_VALUE" value="SERVO_SWITCH_MAX"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_SWITCH_MIN"/>
<define name="SWITCH_SERVO_ON_VALUE" value="MAX_PPRZ"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="MIN_PPRZ"/>

<module name="board" type="tawaki_2.0">
</module>
Expand Down
2 changes: 0 additions & 2 deletions conf/airframes/ENAC/quadrotor/maya_indoor.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,6 @@
<module name="actuators" type="dshot"/>
<module name="actuators" type="pwm"/>
<module name="switch" type="servo"/>
<define name="SWITCH_SERVO_ON_VALUE" value="SERVO_SWITCH_MAX"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_SWITCH_MIN"/>

<module name="board" type="tawaki">
<define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_256HZ"/>
Expand Down
2 changes: 0 additions & 2 deletions conf/airframes/OPENUAS/openuas_own_mavtec.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,6 @@

<section name="SWITCH_SERVO">
<define name="SWITCH_SERVO_SERVO" value="DROP"/>
<define name="SWITCH_SERVO_ON_VALUE" value="SERVO_DROP_MIN"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_DROP_MAX"/>
<define name="DropOpen()" value="SwitchServoOn()"/>
<define name="DropClose()" value="SwitchServoOff()"/>
</section>
Expand Down
4 changes: 2 additions & 2 deletions conf/airframes/tudelft/mavtec5.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,8 +74,8 @@

<section name="SWITCH_SERVO">
<define name="SWITCH_SERVO_SERVO" value="DROP"/>
<define name="SWITCH_SERVO_ON_VALUE" value="SERVO_DROP_MIN"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_DROP_MAX"/>
<define name="SWITCH_SERVO_ON_VALUE" value="MIN_PPRZ"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="MAX_PPRZ"/>
<define name="DropOpen()" value="SwitchServoOn()"/>
<define name="DropClose()" value="SwitchServoOff()"/>
</section>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/rotorcraft_cam.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ On boards with CAM_SWITCH, ROTORCRAFT_CAM_SWITCH_GPIO can be defined to CAM_SWIT
<key_press key="F1" value="0"/>
<key_press key="F2" value="1"/>
</dl_setting>
<dl_setting var="rotorcraft_cam_tilt_pwm" min="1000" step="1" max="2500" shortname="tilt_pwm"/>
<dl_setting var="rotorcraft_cam_tilt_pprz" min="MIN_PPRZ" step="1" max="MAX_PPRZ" shortname="tilt_pprz"/>
<dl_setting var="rotorcraft_cam_tilt" min="-90" step="1" max="0" shortname="tilt" alt_unit="deg" alt_unit_coef="0.0139882">
<strip_button name="Look Foreward" icon="lookfore.png" value="0" group="cam_look"/>
<strip_button name="Look Down" icon="lookdown.png" value="-6434" group="cam_look"/>
Expand Down
8 changes: 4 additions & 4 deletions conf/modules/switch_servo.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
<module name="switch_servo" dir="switching">
<doc>
<description>Swicht using a Servo</description>
<define name="SWITCH_SERVO_ON_VALUE" value="pwm" description="servo value in usec"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="pwm" description="servo value in usec"/>
<define name="SWITCH_SERVO_ON_VALUE" value="pprz_cmd" description="servo value in pprz units"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="pprz_cmd" description="servo value in pprz units"/>
<define name="SWITCH_SERVO_SERVO" value="nb" description="Id of the servo (default: SWITCH)"/>
</doc>
<settings>
Expand All @@ -27,8 +27,8 @@
<!-- these parameters should be set for that module in the airframe file unless you want the defaults
Servo value in usec
<load name="switch_servo.xml">
<define name="SWITCH_SERVO_ON_VALUE" value="2000"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="1000"/>
<define name="SWITCH_SERVO_ON_VALUE" value="MAX_PPRZ"/>
<define name="SWITCH_SERVO_OFF_VALUE" value="MIN_PPRZ"/>
<define name="SWITCH_SERVO_SERVO" value="SWITCH"/>
</load>

Expand Down
16 changes: 8 additions & 8 deletions conf/settings/setup_actuators.xml
View file
Open in desktop
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"/>
dewagter marked this conversation as resolved.
Show resolved Hide resolved
<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
Open in desktop
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
9 changes: 8 additions & 1 deletion sw/airborne/modules/actuators/actuators.h
View file
Open in desktop
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)
};
dewagter marked this conversation as resolved.
Show resolved Hide resolved


/** 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 Down
20 changes: 7 additions & 13 deletions sw/airborne/modules/cam_control/rotorcraft_cam.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,12 +73,8 @@ uint8_t rotorcraft_cam_mode;

// Tilt definition
int16_t rotorcraft_cam_tilt;
int16_t rotorcraft_cam_tilt_pwm;
int16_t rotorcraft_cam_tilt_pprz;
#if ROTORCRAFT_CAM_USE_TILT
#define ROTORCRAFT_CAM_TILT_NEUTRAL SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, NEUTRAL)
#define ROTORCRAFT_CAM_TILT_MIN SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MIN)
#define ROTORCRAFT_CAM_TILT_MAX SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MAX)
#define D_TILT (ROTORCRAFT_CAM_TILT_MAX - ROTORCRAFT_CAM_TILT_MIN)
#define CT_MIN Min(CAM_TA_MIN, CAM_TA_MAX)
#define CT_MAX Max(CAM_TA_MIN, CAM_TA_MAX)
#endif
Expand Down Expand Up @@ -112,11 +108,9 @@ void rotorcraft_cam_init(void)
gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO);
#endif
rotorcraft_cam_set_mode(ROTORCRAFT_CAM_DEFAULT_MODE);
rotorcraft_cam_tilt_pprz = 0;
#if ROTORCRAFT_CAM_USE_TILT
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
#else
rotorcraft_cam_tilt_pwm = 1500;
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pprz);
#endif
rotorcraft_cam_tilt = 0;
rotorcraft_cam_pan = 0;
Expand All @@ -130,7 +124,7 @@ void rotorcraft_cam_periodic(void)
switch (rotorcraft_cam_mode) {
case ROTORCRAFT_CAM_MODE_NONE:
#if ROTORCRAFT_CAM_USE_TILT
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
rotorcraft_cam_tilt_pprz = 0;
#endif
#if ROTORCRAFT_CAM_USE_PAN
rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
Expand All @@ -142,7 +136,7 @@ void rotorcraft_cam_periodic(void)
case ROTORCRAFT_CAM_MODE_HEADING:
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
rotorcraft_cam_tilt_pprz = MIN_PPRZ + 2 * MAX_PPRZ * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
#if ROTORCRAFT_CAM_USE_PAN
Expand All @@ -164,7 +158,7 @@ void rotorcraft_cam_periodic(void)
height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
rotorcraft_cam_tilt = int32_atan2(height, dist);
Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
rotorcraft_cam_tilt_pprz = MIN_PPRZ + 2 * MAX_PPRZ * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
}
Expand All @@ -174,6 +168,6 @@ void rotorcraft_cam_periodic(void)
break;
}
#if ROTORCRAFT_CAM_USE_TILT
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pprz);
#endif
}
2 changes: 1 addition & 1 deletion sw/airborne/modules/cam_control/rotorcraft_cam.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ extern uint8_t rotorcraft_cam_mode;

extern int16_t rotorcraft_cam_tilt;
extern int16_t rotorcraft_cam_pan;
extern int16_t rotorcraft_cam_tilt_pwm;
extern int16_t rotorcraft_cam_tilt_pprz;

extern void rotorcraft_cam_init(void);
extern void rotorcraft_cam_periodic(void);
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/loggers/logger_control_effectiveness.c
View file
Open in desktop
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
4 changes: 2 additions & 2 deletions sw/airborne/modules/switching/switch_servo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,10 @@ extern bool switch_servo_on;
extern int16_t switch_servo_value;

#ifndef SWITCH_SERVO_ON_VALUE
#define SWITCH_SERVO_ON_VALUE 2000
#define SWITCH_SERVO_ON_VALUE MAX_PPRZ
#endif
#ifndef SWITCH_SERVO_OFF_VALUE
#define SWITCH_SERVO_OFF_VALUE 1000
#define SWITCH_SERVO_OFF_VALUE MIN_PPRZ
#endif
#ifndef SWITCH_SERVO_SERVO
#define SWITCH_SERVO_SERVO SWITCH
Expand Down
18 changes: 10 additions & 8 deletions sw/tools/generators/gen_airframe.ml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -271,9 +271,13 @@ 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(actuator_value_pprz) { \\\n" s;
fprintf out " actuators[SERVO_%s_IDX].pprz_val = ClipAbs( actuator_value_pprz, MAX_PPRZ); \\\n" s;
fprintf out " command_value = actuator_value_pprz * (actuator_value_pprz>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM); \\\n" s s;
fprintf out " command_value /= actuator_value_pprz>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" s s;
fprintf out " servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" s;
fprintf out " actuators[SERVO_%s_IDX].driver_val = Clip(servo_value, 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 @@ -286,11 +290,8 @@ 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 " servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" servo;
fprintf out " Set_%s_Servo(servo_value); \\\n\\\n" servo
fprintf out " actuator_value_pprz = %s; \\\n" v;
fprintf out " Set_%s_Servo(actuator_value_pprz); \\\n\\\n" servo
| "let" ->
let var = a "var"
and value = a "value" in
Expand Down Expand Up @@ -435,6 +436,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