Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

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

Browse Source 
* actuators[ ] array in pprz units, conversion to actuator_driver units when sending to the actuator_driver.

actuators becomes an array

* Moved scaling of the Servo to the Set function.

* Fixed switches and pan-tilt to work in PPRZ units now. Simplified code.
This commit is contained in:
Christophe De Wagter
2024-05-25 09:38:51 +02:00
committed by GitHub
parent 98d4ec734e
commit e6bcd60666
16 changed files with 55 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/CDW/cdw_mavtec.xml
+2 -2
View File
@@ -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>
conf/airframes/ENAC/quadrotor/cobra.xml
-2
View File
@@ -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"/>
conf/airframes/ENAC/quadrotor/cobraV2.xml
+2 -2
View File
@@ -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>
conf/airframes/ENAC/quadrotor/maya_indoor.xml
-2
View File
@@ -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"/>
conf/airframes/OPENUAS/openuas_own_mavtec.xml
-2
View File
@@ -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>
conf/airframes/tudelft/mavtec5.xml
+2 -2
View File
@@ -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>
conf/modules/rotorcraft_cam.xml
+1 -1
View File
@@ -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"/>
conf/modules/switch_servo.xml
+4 -4
View File
@@ -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>
@@ -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>
conf/settings/setup_actuators.xml
+8 -8
View File
@@ -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>
sw/airborne/modules/actuators/actuators.c
+7 -2
View File
@@ -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];
sw/airborne/modules/actuators/actuators.h
+8 -1
View File
@@ -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
sw/airborne/modules/cam_control/rotorcraft_cam.c
+7 -13
View File
@@ -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
@@ -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;
@@ -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;
@@ -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
@@ -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
}
@@ -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
}
sw/airborne/modules/cam_control/rotorcraft_cam.h
+1 -1
View File
@@ -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);
sw/airborne/modules/loggers/logger_control_effectiveness.c
+1 -1
View File
@@ -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
sw/airborne/modules/switching/switch_servo.h
+2 -2
View File
@@ -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
sw/tools/generators/gen_airframe.ml
+10 -8
View File
@@ -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));
@@ -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
@@ -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";