diff --git a/conf/modules/actuators.xml b/conf/modules/actuators.xml index aff78576c8..d32ec360cb 100644 --- a/conf/modules/actuators.xml +++ b/conf/modules/actuators.xml @@ -16,7 +16,11 @@ - + + + + + diff --git a/conf/modules/actuators_dshot.xml b/conf/modules/actuators_dshot.xml index 8903ab8517..599e0986e5 100644 --- a/conf/modules/actuators_dshot.xml +++ b/conf/modules/actuators_dshot.xml @@ -28,6 +28,7 @@ + diff --git a/conf/modules/actuators_nps.xml b/conf/modules/actuators_nps.xml index 614178227c..29776ea999 100644 --- a/conf/modules/actuators_nps.xml +++ b/conf/modules/actuators_nps.xml @@ -11,6 +11,7 @@ actuators + diff --git a/conf/modules/actuators_pwm.xml b/conf/modules/actuators_pwm.xml index 0296318845..30b8f874f5 100644 --- a/conf/modules/actuators_pwm.xml +++ b/conf/modules/actuators_pwm.xml @@ -13,9 +13,11 @@
- + + + diff --git a/sw/airborne/arch/chibios/common_board.h b/sw/airborne/arch/chibios/common_board.h index fa95657895..3858836ef1 100644 --- a/sw/airborne/arch/chibios/common_board.h +++ b/sw/airborne/arch/chibios/common_board.h @@ -256,12 +256,27 @@ /* * PWM defines */ +#if defined(LINE_SERVO0) +#ifndef USE_PWM0 +#define USE_PWM0 1 +#endif +#if USE_PWM1 +#define PWM_SERVO_0 0 +#define PWM_SERVO_0_GPIO PAL_PORT(LINE_SERVO0) +#define PWM_SERVO_0_PIN PAL_PAD(LINE_SERVO0) +#define PWM_SERVO_0_AF AF_LINE_SERVO0 +#define PWM_SERVO_0_DRIVER CONCAT_BOARD_PARAM(PWMD, SERVO0_TIM) +#define PWM_SERVO_0_CHANNEL (SERVO0_TIM_CH-1) +#define PWM_SERVO_0_CONF CONCAT_BOARD_PARAM(pwmcfg, SERVO0_TIM) +#endif +#endif + #if defined(LINE_SERVO1) #ifndef USE_PWM1 #define USE_PWM1 1 #endif #if USE_PWM1 -#define PWM_SERVO_1 0 +#define PWM_SERVO_1 1 #define PWM_SERVO_1_GPIO PAL_PORT(LINE_SERVO1) #define PWM_SERVO_1_PIN PAL_PAD(LINE_SERVO1) #define PWM_SERVO_1_AF AF_LINE_SERVO1 @@ -276,7 +291,7 @@ #define USE_PWM2 1 #endif #if USE_PWM2 -#define PWM_SERVO_2 1 +#define PWM_SERVO_2 2 #define PWM_SERVO_2_GPIO PAL_PORT(LINE_SERVO2) #define PWM_SERVO_2_PIN PAL_PAD(LINE_SERVO2) #define PWM_SERVO_2_AF AF_LINE_SERVO2 @@ -291,7 +306,7 @@ #define USE_PWM3 1 #endif #if USE_PWM3 -#define PWM_SERVO_3 2 +#define PWM_SERVO_3 3 #define PWM_SERVO_3_GPIO PAL_PORT(LINE_SERVO3) #define PWM_SERVO_3_PIN PAL_PAD(LINE_SERVO3) #define PWM_SERVO_3_AF AF_LINE_SERVO3 @@ -306,7 +321,7 @@ #define USE_PWM4 1 #endif #if USE_PWM4 -#define PWM_SERVO_4 3 +#define PWM_SERVO_4 4 #define PWM_SERVO_4_GPIO PAL_PORT(LINE_SERVO4) #define PWM_SERVO_4_PIN PAL_PAD(LINE_SERVO4) #define PWM_SERVO_4_AF AF_LINE_SERVO4 @@ -321,7 +336,7 @@ #define USE_PWM5 1 #endif #if USE_PWM5 -#define PWM_SERVO_5 4 +#define PWM_SERVO_5 5 #define PWM_SERVO_5_GPIO PAL_PORT(LINE_SERVO5) #define PWM_SERVO_5_PIN PAL_PAD(LINE_SERVO5) #define PWM_SERVO_5_AF AF_LINE_SERVO5 @@ -336,7 +351,7 @@ #define USE_PWM6 1 #endif #if USE_PWM6 -#define PWM_SERVO_6 5 +#define PWM_SERVO_6 6 #define PWM_SERVO_6_GPIO PAL_PORT(LINE_SERVO6) #define PWM_SERVO_6_PIN PAL_PAD(LINE_SERVO6) #define PWM_SERVO_6_AF AF_LINE_SERVO6 @@ -351,7 +366,7 @@ #define USE_PWM7 1 #endif #if USE_PWM7 -#define PWM_SERVO_7 6 +#define PWM_SERVO_7 7 #define PWM_SERVO_7_GPIO PAL_PORT(LINE_SERVO7) #define PWM_SERVO_7_PIN PAL_PAD(LINE_SERVO7) #define PWM_SERVO_7_AF AF_LINE_SERVO7 @@ -366,7 +381,7 @@ #define USE_PWM8 1 #endif #if USE_PWM8 -#define PWM_SERVO_8 7 +#define PWM_SERVO_8 8 #define PWM_SERVO_8_GPIO PAL_PORT(LINE_SERVO8) #define PWM_SERVO_8_PIN PAL_PAD(LINE_SERVO8) #define PWM_SERVO_8_AF AF_LINE_SERVO8 @@ -793,7 +808,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c index b3a816af80..a69a1c7b5b 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c +++ b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c @@ -49,7 +49,6 @@ struct dshot_private { uint32_t channel; }; -struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB]; struct dshot_private actuators_dshot_private[ACTUATORS_DSHOT_NB]; #if DSHOT_CONF_TIM1 diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.c b/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.c index d3a1bf0890..360b1c1bd0 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.c @@ -89,8 +89,6 @@ PRINT_CONFIG_VAR(SERVO_HZ) #define PWM_CMD_TO_US(_t) (PWM_FREQUENCY * _t / 1000000) #endif -int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; - /** * PWM callback function * @@ -374,7 +372,7 @@ void actuators_pwm_arch_init(void) } -void actuators_pwm_commit(void) +void actuators_pwm_arch_commit(void) { #ifdef PWM_SERVO_0 pwmEnableChannel(&PWM_SERVO_0_DRIVER, PWM_SERVO_0_CHANNEL, PWM_CMD_TO_US(actuators_pwm_values[PWM_SERVO_0])); @@ -428,3 +426,4 @@ void actuators_pwm_commit(void) pwmEnableChannel(&PWM_SERVO_16_DRIVER, PWM_SERVO_16_CHANNEL, PWM_CMD_TO_US(actuators_pwm_values[PWM_SERVO_16])); #endif } + diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.h b/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.h index 7abf214ae2..f77e596560 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.h +++ b/sw/airborne/arch/chibios/modules/actuators/actuators_pwm_arch.h @@ -38,15 +38,4 @@ #include BOARD_CONFIG -#ifndef ACTUATORS_PWM_NB -#define ACTUATORS_PWM_NB 8 -#endif - -extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; - -extern void actuators_pwm_commit(void); - -#define ActuatorPwmSet(_i, _v) { actuators_pwm_values[_i] = _v; } -#define ActuatorsPwmCommit actuators_pwm_commit - #endif /* ACTUATORS_PWM_ARCH_H */ diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.c b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.c index ae90ed4697..b900c6f946 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.c +++ b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.c @@ -100,3 +100,9 @@ void actuators_t4_commit(void) { // Send ABI message AbiSendMsgACTUATORS_T4_OUT(ABI_ACTUATORS_T4_OUT_ID, &actuators_t4_out_local, &actuators_t4_extra_data_out_local[0]); } + +void actuators_t4_set(uint8_t idx, int16_t value) +{ + actuators_t4_values[idx] = value; +} + diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.h b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.h index 69ee68e340..2af9260dc5 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.h +++ b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.h @@ -35,8 +35,9 @@ extern int32_t actuators_t4_values[ACTUATORS_T4_NB]; extern void actuators_t4_commit(void); +extern void actuators_t4_set(uint8_t idx, int16_t value); -#define ActuatorT4Set(_i, _v) { actuators_t4_values[_i] = _v; } +#define ActuatorT4Set actuators_t4_set #define ActuatorsT4Commit actuators_t4_commit #endif /* ACTUATORS_T4_ARCH_H */ diff --git a/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.c b/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.c index aad1cea25c..8670231efe 100644 --- a/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.c @@ -26,8 +26,7 @@ #include "modules/actuators/actuators_pwm_arch.h" #include "modules/actuators/actuators_pwm.h" -void actuators_pwm_arch_init(void) -{ +void actuators_pwm_arch_init(void) {} -} +void actuators_pwm_arch_commit(void) {} diff --git a/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.h b/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.h index 04dc9df5fb..7b2a074edf 100644 --- a/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.h +++ b/sw/airborne/arch/linux/modules/actuators/actuators_pwm_arch.h @@ -28,9 +28,4 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorPwmSet(_i, _v) {} -#define ActuatorsPwmCommit() {} - -//extern void actuators_pwm_arch_init(void); - #endif /* ACTUATORS_PWM_ARCH_H */ diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.c b/sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.c index 7b947e16a7..e45d2786ec 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.c +++ b/sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.c @@ -27,8 +27,6 @@ #include "modules/actuators/actuators_dshot.h" -struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB]; - void actuators_dshot_arch_init(void) {} void actuators_dshot_arch_commit(void) {} diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.c b/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.c index 2db488f080..f4033a54a6 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.c +++ b/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.c @@ -25,8 +25,7 @@ #include "modules/actuators/actuators_dualpwm_arch.h" -void actuators_dualpwm_arch_init(void) -{ +void actuators_dualpwm_arch_init(void) {} -} +void actuators_dualpwm_set(uint8_t idx UNUSED, int16_t value UNUSED) {} diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.h b/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.h index 7d0ee978c2..4379276962 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.h +++ b/sw/airborne/arch/sim/modules/actuators/actuators_dualpwm_arch.h @@ -28,9 +28,10 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorDualpwmSet(_i, _v) {} +#define ActuatorDualpwmSet actuators_dualpwm_set #define ActuatorsDualPwmCommit() {} extern void actuators_dualpwm_arch_init(void); +extern void actuators_dualpwm_set(uint8_t idx, int16_t value); #endif /* ACTUATORS_DUALPWM_ARCH_H */ diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.c b/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.c index b7e2b10d50..642cfd78a8 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.c @@ -23,10 +23,9 @@ * dummy servos handling for sim */ -#include "modules/actuators/actuators_pwm_arch.h" +#include "modules/actuators/actuators_pwm.h" -void actuators_pwm_arch_init(void) -{ +void actuators_pwm_arch_init(void) {} -} +void actuators_pwm_arch_commit(void) {} diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.h b/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.h index 57ad96c4f8..22b1a4b079 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.h +++ b/sw/airborne/arch/sim/modules/actuators/actuators_pwm_arch.h @@ -28,9 +28,4 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorPwmSet(_i, _v) {} -#define ActuatorsPwmCommit() {} - -extern void actuators_pwm_arch_init(void); - #endif /* ACTUATORS_PWM_ARCH_H */ diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.c b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.c index 36cbf163fc..84c0fa363c 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.c +++ b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.c @@ -101,3 +101,9 @@ void actuators_t4_commit(void) { // Send ABI message AbiSendMsgACTUATORS_T4_OUT(ABI_ACTUATORS_T4_OUT_ID, &actuators_t4_out_local, &actuators_t4_extra_data_out_local[0]); } + +void actuators_t4_set(uint8_t idx, int16_t value) +{ + actuators_t4_values[idx] = value; +} + diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.h b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.h index 8621dd673f..7e752faddf 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.h +++ b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.h @@ -35,8 +35,9 @@ extern int32_t actuators_t4_values[ACTUATORS_T4_NB]; extern void actuators_t4_commit(void); +extern void actuators_t4_set(uint8_t idx, int16_t value); -#define ActuatorT4Set(_i, _v) { actuators_t4_values[_i] = _v; } +#define ActuatorT4Set actuators_t4_set #define ActuatorsT4Commit actuators_t4_commit #endif /* ACTUATORS_T4_ARCH_H */ diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.c b/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.c index dc6ada8af0..9dc1d58d72 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.c +++ b/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.c @@ -28,4 +28,5 @@ void actuators_uavcan_arch_init(void) { -} \ No newline at end of file +} + diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.h b/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.h index 3918093ec8..01c68c88de 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.h +++ b/sw/airborne/arch/sim/modules/actuators/actuators_uavcan_arch.h @@ -28,7 +28,7 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorUavcanSet(_i, _v) {} +#define ActuatorUavcanSet NULL #define ActuatorsUavcanCommit() {} extern void actuators_uavcan_arch_init(void); diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.c b/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.c index 2d9ba271a0..18150ed244 100644 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.c +++ b/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.c @@ -167,6 +167,10 @@ void dual_pwm_isr(void) } } +void actuators_dualpwm_set(uint8_t idx, int16_t value) +{ + actuators_dualpwm_values[idx] = value; +} /** Set pulse widths from actuator values, assumed to be in us */ diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.h b/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.h index 34b3c8868c..11c5bf0359 100644 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.h +++ b/sw/airborne/arch/stm32/modules/actuators/actuators_dualpwm_arch.h @@ -38,6 +38,8 @@ extern uint32_t actuators_dualpwm_values[ACTUATORS_DUALPWM_NB]; +extern void actuators_dualpwm_set(uint8_t idx, int16_t value); + extern void actuators_dualpwm_commit(void); extern void dual_pwm_isr(void); @@ -50,7 +52,7 @@ extern void set_dual_pwm_timer_s_oc(uint32_t oc_value, uint32_t oc_value2); #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorDualpwmSet(_i, _v) { actuators_dualpwm_values[_i] = _v; } +#define ActuatorDualpwmSet actuators_dualpwm_set #define ActuatorsDualpwmCommit actuators_dualpwm_commit #endif /* ACTUATORS_dualpwm_ARCH_H */ diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.c index 7c53d71fc5..692df593fc 100644 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.c @@ -36,9 +36,6 @@ #include "mcu_periph/gpio_arch.h" -int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; - - /** PWM arch init called by generic pwm driver */ void actuators_pwm_arch_init(void) @@ -155,7 +152,7 @@ void actuators_pwm_arch_init(void) /** Set pulse widths from actuator values, assumed to be in us */ -void actuators_pwm_commit(void) +void actuators_pwm_arch_commit(void) { #ifdef PWM_SERVO_0 timer_set_oc_value(PWM_SERVO_0_TIMER, PWM_SERVO_0_OC, actuators_pwm_values[PWM_SERVO_0]); @@ -195,3 +192,4 @@ void actuators_pwm_commit(void) #endif } + diff --git a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.h b/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.h index 2e54c4f7df..fe8271d303 100644 --- a/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.h +++ b/sw/airborne/arch/stm32/modules/actuators/actuators_pwm_arch.h @@ -30,17 +30,6 @@ #include BOARD_CONFIG -#ifndef ACTUATORS_PWM_NB -#define ACTUATORS_PWM_NB 8 -#endif - -extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; - -extern void actuators_pwm_commit(void); - #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorPwmSet(_i, _v) { actuators_pwm_values[_i] = _v; } -#define ActuatorsPwmCommit actuators_pwm_commit - #endif /* ACTUATORS_PWM_ARCH_H */ diff --git a/sw/airborne/boards/apogee/chibios/v1.0/board.cfg b/sw/airborne/boards/apogee/chibios/v1.0/board.cfg index c5a515cc15..36c2473979 100644 --- a/sw/airborne/boards/apogee/chibios/v1.0/board.cfg +++ b/sw/airborne/boards/apogee/chibios/v1.0/board.cfg @@ -494,7 +494,7 @@ HEADER */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/apogee/chibios/v1.0/board.h b/sw/airborne/boards/apogee/chibios/v1.0/board.h index a92bdcd0dc..bddc45d25a 100644 --- a/sw/airborne/boards/apogee/chibios/v1.0/board.h +++ b/sw/airborne/boards/apogee/chibios/v1.0/board.h @@ -508,7 +508,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h index 05d60ac6a2..b5927f86dd 100644 --- a/sw/airborne/boards/apogee_1.0.h +++ b/sw/airborne/boards/apogee_1.0.h @@ -102,7 +102,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/ardrone/actuators.c b/sw/airborne/boards/ardrone/actuators.c index 17feefc239..ee09ee02ae 100644 --- a/sw/airborne/boards/ardrone/actuators.c +++ b/sw/airborne/boards/ardrone/actuators.c @@ -225,6 +225,13 @@ void actuators_ardrone_commit(void) RunOnceEvery(100, actuators_ardrone_motor_status()); } +void actuators_ardrone_set(uint8_t idx, int16_t value) +{ + if (idx < ACTUATORS_ARDRONE_NB) { + actuators_pwm_values[idx] = value; + } +} + /** * Write motor speed command * cmd = 001aaaaa aaaabbbb bbbbbccc ccccccdd ddddddd0 diff --git a/sw/airborne/boards/ardrone/actuators.h b/sw/airborne/boards/ardrone/actuators.h index b044004d0d..2df9b1a7d8 100644 --- a/sw/airborne/boards/ardrone/actuators.h +++ b/sw/airborne/boards/ardrone/actuators.h @@ -39,9 +39,9 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorArdroneSet(_i, _v) { actuators_pwm_values[_i] = _v; } -#define ActuatorsArdroneCommit() actuators_ardrone_commit(); -#define ActuatorsArdroneInit() actuators_ardrone_init(); +#define ActuatorArdroneSet actuators_ardrone_set +#define ActuatorsArdroneCommit actuators_ardrone_commit +#define ActuatorsArdroneInit actuators_ardrone_init #define MOT_LEDOFF 0 #define MOT_LEDRED 1 @@ -52,6 +52,7 @@ extern uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB]; extern void actuators_ardrone_commit(void); extern void actuators_ardrone_init(void); +extern void actuators_ardrone_set(uint8_t idx, int16_t value); int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen); diff --git a/sw/airborne/boards/ardrone2.h b/sw/airborne/boards/ardrone2.h index eb5d8835c5..972639347a 100644 --- a/sw/airborne/boards/ardrone2.h +++ b/sw/airborne/boards/ardrone2.h @@ -19,7 +19,7 @@ extern struct video_config_t front_camera; /* Default actuators driver */ #define DEFAULT_ACTUATORS "boards/ardrone/actuators.h" -#define ActuatorDefaultSet(_x,_y) ActuatorArdroneSet(_x,_y) +#define ActuatorDefaultSet ActuatorArdroneSet #define ActuatorsDefaultInit() ActuatorsArdroneInit() #define ActuatorsDefaultCommit() ActuatorsArdroneCommit() diff --git a/sw/airborne/boards/bebop.h b/sw/airborne/boards/bebop.h index 3ea8945bf9..a63ec94001 100644 --- a/sw/airborne/boards/bebop.h +++ b/sw/airborne/boards/bebop.h @@ -52,7 +52,7 @@ /* Default actuators driver */ #ifndef DEFAULT_ACTUATORS #define DEFAULT_ACTUATORS "boards/bebop/actuators.h" -#define ActuatorDefaultSet(_x,_y) ActuatorsBebopSet(_x,_y) +#define ActuatorDefaultSet ActuatorsBebopSet #define ActuatorsDefaultInit() ActuatorsBebopInit() #define ActuatorsDefaultCommit() ActuatorsBebopCommit() #endif diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c index 08c2422232..526304f796 100644 --- a/sw/airborne/boards/bebop/actuators.c +++ b/sw/airborne/boards/bebop/actuators.c @@ -70,6 +70,11 @@ void actuators_bebop_init(void) #endif } +void actuators_bebop_set(uint8_t idx, int16_t value) +{ + actuators_bebop.rpm_ref[idx] = value; +} + void actuators_bebop_commit(void) { // Receive the status diff --git a/sw/airborne/boards/bebop/actuators.h b/sw/airborne/boards/bebop/actuators.h index 95bcaff9ff..a6cf4c4dd1 100644 --- a/sw/airborne/boards/bebop/actuators.h +++ b/sw/airborne/boards/bebop/actuators.h @@ -52,12 +52,13 @@ struct ActuatorsBebop { uint8_t led; ///< Current led status }; -#define ActuatorsBebopSet(_i, _v) { actuators_bebop.rpm_ref[_i] = _v; } +#define ActuatorsBebopSet actuators_bebop_set #define ActuatorsBebopCommit() actuators_bebop_commit(); #define ActuatorsBebopInit() actuators_bebop_init(); extern struct ActuatorsBebop actuators_bebop; extern void actuators_bebop_commit(void); extern void actuators_bebop_init(void); +extern void actuators_bebop_set(uint8_t idx, int16_t val); #endif /* ACTUATORS_BEBOP_RAW_H_ */ diff --git a/sw/airborne/boards/cc3d.h b/sw/airborne/boards/cc3d.h index 64655eb943..3cad47d0dd 100644 --- a/sw/airborne/boards/cc3d.h +++ b/sw/airborne/boards/cc3d.h @@ -244,7 +244,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/chimera/chibios/v1.0/chimera.h b/sw/airborne/boards/chimera/chibios/v1.0/chimera.h index d86dc8c774..ffc9b6a0ce 100644 --- a/sw/airborne/boards/chimera/chibios/v1.0/chimera.h +++ b/sw/airborne/boards/chimera/chibios/v1.0/chimera.h @@ -580,7 +580,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/cjmcu.h b/sw/airborne/boards/cjmcu.h index a3287a2821..95d1c73dec 100644 --- a/sw/airborne/boards/cjmcu.h +++ b/sw/airborne/boards/cjmcu.h @@ -45,7 +45,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/crazybee_f4_1.0.h b/sw/airborne/boards/crazybee_f4_1.0.h index 7e835331d6..cafd74c67b 100644 --- a/sw/airborne/boards/crazybee_f4_1.0.h +++ b/sw/airborne/boards/crazybee_f4_1.0.h @@ -229,7 +229,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/crazyflie/chibios/v2.1/crazyflie.h b/sw/airborne/boards/crazyflie/chibios/v2.1/crazyflie.h index e8edc69267..47384ba7bd 100644 --- a/sw/airborne/boards/crazyflie/chibios/v2.1/crazyflie.h +++ b/sw/airborne/boards/crazyflie/chibios/v2.1/crazyflie.h @@ -316,7 +316,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/disco.h b/sw/airborne/boards/disco.h index edefcbcb73..3b8d24e868 100644 --- a/sw/airborne/boards/disco.h +++ b/sw/airborne/boards/disco.h @@ -54,7 +54,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "boards/disco/actuators.h" -#define ActuatorDefaultSet(_x,_y) ActuatorsDiscoSet(_x,_y) +#define ActuatorDefaultSet ActuatorsDiscoSet #define ActuatorsDefaultInit() ActuatorsDiscoInit() #define ActuatorsDefaultCommit() ActuatorsDiscoCommit() diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c index 45b4cc850a..1e973c4ab3 100644 --- a/sw/airborne/boards/disco/actuators.c +++ b/sw/airborne/boards/disco/actuators.c @@ -98,15 +98,15 @@ void actuators_disco_init(void) } } -void actuators_disco_set(uint8_t idx, uint16_t val) +void actuators_disco_set(uint8_t idx, int16_t val) { if (idx == ACTUATORS_DISCO_MOTOR_IDX) { - actuators_disco.motor_rpm = val; + actuators_disco.motor_rpm = (uint16_t)val; } else if (idx > ACTUATORS_DISCO_PWM_NB) { // wrong index, do nothing } else { // val is a PWM value in ms, convert to ns - pwm_sysfs_set_duty(&actuators_disco.pwm[idx-1], val * 1000); + pwm_sysfs_set_duty(&actuators_disco.pwm[idx-1], (uint16_t)val * 1000); } } diff --git a/sw/airborne/boards/disco/actuators.h b/sw/airborne/boards/disco/actuators.h index 9e05754080..a90ad6e682 100644 --- a/sw/airborne/boards/disco/actuators.h +++ b/sw/airborne/boards/disco/actuators.h @@ -60,12 +60,12 @@ struct ActuatorsDisco { bool rpm_saturated; ///< RPM saturation flag (bit 15 in obs data) }; -#define ActuatorsDiscoSet(_i, _v) actuators_disco_set(_i, _v) +#define ActuatorsDiscoSet actuators_disco_set #define ActuatorsDiscoCommit() actuators_disco_commit() #define ActuatorsDiscoInit() actuators_disco_init() extern struct ActuatorsDisco actuators_disco; -extern void actuators_disco_set(uint8_t idx, uint16_t val); +extern void actuators_disco_set(uint8_t idx, int16_t val); extern void actuators_disco_commit(void); extern void actuators_disco_init(void); diff --git a/sw/airborne/boards/elle0_common.h b/sw/airborne/boards/elle0_common.h index 2540041530..d608f3953c 100644 --- a/sw/airborne/boards/elle0_common.h +++ b/sw/airborne/boards/elle0_common.h @@ -84,7 +84,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/holybro/kakute_f7/holybro_kakute_f7.h b/sw/airborne/boards/holybro/kakute_f7/holybro_kakute_f7.h index 78bf7dfeb0..dfde8fc7ab 100644 --- a/sw/airborne/boards/holybro/kakute_f7/holybro_kakute_f7.h +++ b/sw/airborne/boards/holybro/kakute_f7/holybro_kakute_f7.h @@ -412,7 +412,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index 9322525cbc..5d9d1e4b0b 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -71,7 +71,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lia/chibios/v1.1/board.h b/sw/airborne/boards/lia/chibios/v1.1/board.h index f57deb843a..adf12a61f0 100644 --- a/sw/airborne/boards/lia/chibios/v1.1/board.h +++ b/sw/airborne/boards/lia/chibios/v1.1/board.h @@ -487,7 +487,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lia_1.1.h b/sw/airborne/boards/lia_1.1.h index 02c040d5e7..3c9b24f2bd 100644 --- a/sw/airborne/boards/lia_1.1.h +++ b/sw/airborne/boards/lia_1.1.h @@ -120,7 +120,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index ca64c3d649..39bd70f9a8 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -174,7 +174,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index 7c5fbaa6fa..431d53823c 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -78,7 +78,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h index 5a1e55cbac..f968c541f7 100644 --- a/sw/airborne/boards/lisa_m_2.0.h +++ b/sw/airborne/boards/lisa_m_2.0.h @@ -126,7 +126,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_m_2.1.h b/sw/airborne/boards/lisa_m_2.1.h index ee45787a7c..07ca27901f 100644 --- a/sw/airborne/boards/lisa_m_2.1.h +++ b/sw/airborne/boards/lisa_m_2.1.h @@ -118,7 +118,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_mx/chibios/v2.1/board.h b/sw/airborne/boards/lisa_mx/chibios/v2.1/board.h index 4a11594eb7..c475b0c6b2 100644 --- a/sw/airborne/boards/lisa_mx/chibios/v2.1/board.h +++ b/sw/airborne/boards/lisa_mx/chibios/v2.1/board.h @@ -1363,7 +1363,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_mx_common.h b/sw/airborne/boards/lisa_mx_common.h index d9fd1f18ec..49e3a54b15 100644 --- a/sw/airborne/boards/lisa_mx_common.h +++ b/sw/airborne/boards/lisa_mx_common.h @@ -131,7 +131,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h b/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h index d47d623e6b..ecd195bb63 100644 --- a/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h +++ b/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h @@ -1365,7 +1365,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/lisa_s_1.0.h b/sw/airborne/boards/lisa_s_1.0.h index 2c850a7acc..183c06c125 100644 --- a/sw/airborne/boards/lisa_s_1.0.h +++ b/sw/airborne/boards/lisa_s_1.0.h @@ -62,7 +62,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/matek_f405_wing_v1.h b/sw/airborne/boards/matek_f405_wing_v1.h index 9571da65f2..9da3e4d867 100644 --- a/sw/airborne/boards/matek_f405_wing_v1.h +++ b/sw/airborne/boards/matek_f405_wing_v1.h @@ -73,7 +73,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/mateksys/F765-WING/matekF765-WING.h b/sw/airborne/boards/mateksys/F765-WING/matekF765-WING.h index 8959c54bc2..5ca3298ab4 100644 --- a/sw/airborne/boards/mateksys/F765-WING/matekF765-WING.h +++ b/sw/airborne/boards/mateksys/F765-WING/matekF765-WING.h @@ -575,7 +575,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/navstik_1.0.h b/sw/airborne/boards/navstik_1.0.h index 51663f1034..7d675934b3 100644 --- a/sw/airborne/boards/navstik_1.0.h +++ b/sw/airborne/boards/navstik_1.0.h @@ -60,7 +60,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/naze32_common.h b/sw/airborne/boards/naze32_common.h index 2f9fe81ad1..ff23282242 100644 --- a/sw/airborne/boards/naze32_common.h +++ b/sw/airborne/boards/naze32_common.h @@ -296,7 +296,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/nucleo/144_f767zi/nucleo144_f767zi.h b/sw/airborne/boards/nucleo/144_f767zi/nucleo144_f767zi.h index 26e782b0ee..a43722fdba 100644 --- a/sw/airborne/boards/nucleo/144_f767zi/nucleo144_f767zi.h +++ b/sw/airborne/boards/nucleo/144_f767zi/nucleo144_f767zi.h @@ -720,7 +720,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/opa_ftd_1.0.h b/sw/airborne/boards/opa_ftd_1.0.h index 4ba75f08b9..3f40dda1a3 100644 --- a/sw/airborne/boards/opa_ftd_1.0.h +++ b/sw/airborne/boards/opa_ftd_1.0.h @@ -110,7 +110,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/openpilot_revo_1.0.h b/sw/airborne/boards/openpilot_revo_1.0.h index 4c31321d68..90d9c35764 100644 --- a/sw/airborne/boards/openpilot_revo_1.0.h +++ b/sw/airborne/boards/openpilot_revo_1.0.h @@ -56,7 +56,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/openpilot_revo_nano.h b/sw/airborne/boards/openpilot_revo_nano.h index ebde2cfba5..c057ee6ae2 100644 --- a/sw/airborne/boards/openpilot_revo_nano.h +++ b/sw/airborne/boards/openpilot_revo_nano.h @@ -62,7 +62,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h index 71c9025473..90d73cfcc2 100644 --- a/sw/airborne/boards/pc_sim.h +++ b/sw/airborne/boards/pc_sim.h @@ -4,7 +4,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/px4fmu/chibios/v2.4/board.h b/sw/airborne/boards/px4fmu/chibios/v2.4/board.h index 9ef64625ca..fb1dfcf3bb 100644 --- a/sw/airborne/boards/px4fmu/chibios/v2.4/board.h +++ b/sw/airborne/boards/px4fmu/chibios/v2.4/board.h @@ -981,7 +981,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/px4fmu/chibios/v4.0/px4fmu.h b/sw/airborne/boards/px4fmu/chibios/v4.0/px4fmu.h index 85d15c852c..f46b2149b1 100644 --- a/sw/airborne/boards/px4fmu/chibios/v4.0/px4fmu.h +++ b/sw/airborne/boards/px4fmu/chibios/v4.0/px4fmu.h @@ -436,7 +436,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/px4fmu_1.7.h b/sw/airborne/boards/px4fmu_1.7.h index 165647a7d4..e59356e42b 100644 --- a/sw/airborne/boards/px4fmu_1.7.h +++ b/sw/airborne/boards/px4fmu_1.7.h @@ -207,7 +207,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/px4fmu_2.4.h b/sw/airborne/boards/px4fmu_2.4.h index 0368f343b7..80f7c3e981 100644 --- a/sw/airborne/boards/px4fmu_2.4.h +++ b/sw/airborne/boards/px4fmu_2.4.h @@ -246,7 +246,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/px4fmu_4.0.h b/sw/airborne/boards/px4fmu_4.0.h index 81855a4215..199997f3f1 100644 --- a/sw/airborne/boards/px4fmu_4.0.h +++ b/sw/airborne/boards/px4fmu_4.0.h @@ -321,7 +321,7 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/px4io_2.4.h b/sw/airborne/boards/px4io_2.4.h index 4a85a6b1a5..fe66e95ecd 100644 --- a/sw/airborne/boards/px4io_2.4.h +++ b/sw/airborne/boards/px4io_2.4.h @@ -242,7 +242,7 @@ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/tawaki/chibios/common/tawaki.h b/sw/airborne/boards/tawaki/chibios/common/tawaki.h index c122ba1482..7f416b8a7b 100644 --- a/sw/airborne/boards/tawaki/chibios/common/tawaki.h +++ b/sw/airborne/boards/tawaki/chibios/common/tawaki.h @@ -751,7 +751,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h b/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h index 31388733dd..bb7f8ffa3f 100644 --- a/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h +++ b/sw/airborne/boards/tawaki/chibios/v2.0/tawaki_v2.0.h @@ -780,7 +780,7 @@ */ /* Default actuators driver */ #define DEFAULT_ACTUATORS "modules/actuators/actuators_pwm.h" -#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorDefaultSet ActuatorPwmSet #define ActuatorsDefaultInit() ActuatorsPwmInit() #define ActuatorsDefaultCommit() ActuatorsPwmCommit() diff --git a/sw/airborne/modules/actuators/actuators.c b/sw/airborne/modules/actuators/actuators.c index 572273c259..d2f6936329 100644 --- a/sw/airborne/modules/actuators/actuators.c +++ b/sw/airborne/modules/actuators/actuators.c @@ -61,7 +61,7 @@ static void send_actuators(struct transport_tx *trans, struct link_device *dev) } #endif -struct actuator_t actuators[ACTUATORS_NB]; +struct actuator_t actuators[ACTUATORS_NB] = ACTUATORS_CONFIG; // Can be used to directly control each actuator from the control algorithm int16_t actuators_pprz[ACTUATORS_NB]; @@ -69,6 +69,95 @@ int16_t actuators_pprz[ACTUATORS_NB]; uint32_t actuators_delay_time; bool actuators_delay_done; +#if USE_SHELL +#include "modules/core/shell.h" +#include "printf.h" +#include "string.h" + +static void show_actuator(shell_stream_t *sh, uint8_t i) { + chprintf(sh, "Actuator %d: driver no %d, servo idx %d\r\n", i, + actuators[i].config.driver_no, actuators[i].config.servo_idx); + chprintf(sh, " min: %d, max: %d, neutral %d\r\n", + actuators[i].config.min, actuators[i].config.max, actuators[i].config.neutral); + if (actuators[i].set == NULL) { + chprintf(sh, " no set function registered"); + } + chprintf(sh, " pprz value : %d\r\n", actuators[i].pprz_val); + chprintf(sh, " driver value: %d\r\n", actuators[i].driver_val); +} + +static void cmd_actuator(shell_stream_t *sh, int argc, const char *const argv[]) +{ + (void) argv; + if (argc == 0) { + chprintf(sh, "Usage: actuator [command] [options]\r\n"); + chprintf(sh, " show [pprz_idx] Print list of actuators with their config or a single one\r\n"); + chprintf(sh, " get_pprz pprz_idx Get current actuator value in pprz unit\r\n"); + chprintf(sh, " get_driver driver_no servo_idx Get current actuator value in driver unit\r\n"); + chprintf(sh, " set_pprz pprz_idx value Set actuator value in pprz unit (values are bounded to [-9600; 9600])\r\n"); + chprintf(sh, " set_driver driver_no servo_idx value Set actuator value in driver unit (warning: values are not bounded)\r\n"); + chprintf(sh, " set_conf pprz_idx min max neutral Set actuator min, max and neutral values in driver unit (warning: values are not bounded)\r\n"); + return; + } + + if (argc >= 1) { + if (strcmp(argv[0], "show") == 0) { + if (argc == 1) { + for (int i = 0; i < ACTUATORS_NB; i++) { + show_actuator(sh, i); + } + } else { + uint8_t idx = atoi(argv[1]); + show_actuator(sh, idx); + } + } else if (strcmp(argv[0], "get_pprz") == 0) { + if (argc >= 2) { + uint8_t idx = atoi(argv[1]); + chprintf(sh, "%d\r\n", actuators[idx].pprz_val); + } + } else if (strcmp(argv[0], "get_driver") == 0) { + if (argc >= 3) { + uint8_t d_idx = atoi(argv[1]); + uint8_t s_idx = atoi(argv[2]); + chprintf(sh, "%d\r\n", actuators[actuator_get_idx(d_idx, s_idx)].driver_val); + } + } else if (strcmp(argv[0], "set_pprz") == 0) { + if (argc >= 3) { + uint8_t idx = atoi(argv[1]); + int16_t val = atoi(argv[2]); + actuator_set(idx, val); + } + } else if (strcmp(argv[0], "set_driver") == 0) { + if (argc >= 4) { + uint8_t d_idx = atoi(argv[1]); + uint8_t s_idx = atoi(argv[2]); + uint8_t p_idx = actuator_get_idx(d_idx, s_idx); + int16_t val = atoi(argv[3]); + actuators[p_idx].driver_val = val; + if (actuators[p_idx].set != NULL) { + actuators[p_idx].set(actuators[p_idx].config.servo_idx, actuators[p_idx].driver_val); + } + } + } else if (strcmp(argv[0], "set_conf") == 0) { + if (argc >= 5) { + uint8_t idx = atoi(argv[1]); + int32_t min = atoi(argv[2]); + int32_t max = atoi(argv[3]); + int32_t neutral = atoi(argv[4]); + uint8_t d_idx = actuators[idx].config.driver_no; + uint8_t s_idx = actuators[idx].config.servo_idx; + actuator_set_min(d_idx, s_idx, min); + actuator_set_max(d_idx, s_idx, max); + actuator_set_neutral(d_idx, s_idx, neutral); + } + } else { + chprintf(sh, "unknown actuator command\r\n"); + } + } +} + +#endif // USE_SHELL + void actuators_init(void) { @@ -95,6 +184,11 @@ void actuators_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS_RAW, send_actuators_raw); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators); #endif + +#if USE_SHELL + shell_add_entry("actuator", cmd_actuator); +#endif + } /** Actuators periodic @@ -133,6 +227,98 @@ void actuators_periodic(void) #endif // USE_COMMANDS } +void actuator_set(uint8_t pprz_idx, pprz_t pprz_value) +{ + int32_t servo_value; + int32_t command_value; + + actuators[pprz_idx].pprz_val = ClipAbs(pprz_value, MAX_PPRZ); + command_value = actuators[pprz_idx].pprz_val * (pprz_value>0 ? actuators[pprz_idx].config.travel_up : actuators[pprz_idx].config.travel_down); + servo_value = actuators[pprz_idx].config.neutral + command_value; + actuators[pprz_idx].driver_val = Clip(servo_value, actuators[pprz_idx].config.min, actuators[pprz_idx].config.max); + if (actuators[pprz_idx].set != NULL) { + actuators[pprz_idx].set(actuators[pprz_idx].config.servo_idx, actuators[pprz_idx].driver_val); + } +} + +int16_t actuator_get(uint8_t pprz_idx) +{ + return actuators[pprz_idx].driver_val; +} + +/** Get pprz index from driver number and servo index + * @return servo index or ACTUATORS_NB if not found + */ +uint8_t actuator_get_idx(uint8_t driver_no, uint8_t servo_idx) +{ + uint8_t i = 0; + for (i = 0; i < ACTUATORS_NB; i++) { + if (actuators[i].config.driver_no == driver_no && + actuators[i].config.servo_idx == servo_idx) { + break; + } + } + return i; // returns ACTUATORS_NB if not found +} + +int32_t actuator_get_min(uint8_t driver_no, uint8_t servo_idx) +{ + uint8_t idx = actuator_get_idx(driver_no, servo_idx); + if (idx != ACTUATORS_NB) { + return actuators[idx].config.min; + } + return 0; +} + +int32_t actuator_get_max(uint8_t driver_no, uint8_t servo_idx) +{ + uint8_t idx = actuator_get_idx(driver_no, servo_idx); + if (idx != ACTUATORS_NB) { + return actuators[idx].config.max; + } + return 0; +} + +int32_t actuator_get_neutral(uint8_t driver_no, uint8_t servo_idx) +{ + uint8_t idx = actuator_get_idx(driver_no, servo_idx); + if (idx != ACTUATORS_NB) { + return actuators[idx].config.neutral; + } + return 0; +} + +void actuator_set_min(uint8_t driver_no, uint8_t servo_idx, int32_t min) +{ + uint8_t idx = actuator_get_idx(driver_no, servo_idx); + if (idx == ACTUATORS_NB) return; // wrong index + actuators[idx].config.min = min; + int32_t delta = actuators[idx].config.neutral - min; + actuators[idx].config.travel_down = (float) delta / MAX_PPRZ; +} + +void actuator_set_max(uint8_t driver_no, uint8_t servo_idx, int32_t max) +{ + uint8_t idx = actuator_get_idx(driver_no, servo_idx); + if (idx == ACTUATORS_NB) return; // wrong index + actuators[idx].config.max = max; + int32_t delta = max - actuators[idx].config.neutral; + actuators[idx].config.travel_up = (float) delta / MAX_PPRZ; +} + +void actuator_set_neutral(uint8_t driver_no, uint8_t servo_idx, int32_t neutral) +{ + uint8_t idx = actuator_get_idx(driver_no, servo_idx); + if (idx == ACTUATORS_NB) return; // wrong index + actuators[idx].config.neutral = neutral; + int32_t delta_max = actuators[idx].config.max - neutral; + actuators[idx].config.travel_up = (float) delta_max / MAX_PPRZ; + int32_t delta_min = neutral - actuators[idx].config.min; + actuators[idx].config.travel_down = (float) delta_min / MAX_PPRZ; +} + + + #else // No command_laws section or no actuators void actuators_init(void) {} diff --git a/sw/airborne/modules/actuators/actuators.h b/sw/airborne/modules/actuators/actuators.h index 718e600a35..4b2d43a12a 100644 --- a/sw/airborne/modules/actuators/actuators.h +++ b/sw/airborne/modules/actuators/actuators.h @@ -52,6 +52,24 @@ struct act_feedback_t { float position; ///< In radians }; +// Actuator configuration +struct act_config_t { + uint8_t driver_no; ///< Driver number + uint8_t servo_idx; ///< Driver servo index + int32_t min; ///< Driver min value (or motor stop position) + int32_t max; ///< Driver max value (or motor max position) + int32_t neutral; ///< Driver neutral value (or motor idle position) + float travel_up; ///< Precomputed travel up coef + float travel_down; ///< Precomputed travel down coef +}; + +/** Actuator set callback + * + * @param driver_idx actuator index in driver frame + * @param driver_value actuator value in driver unit + */ +typedef void (*act_set_t)(uint8_t driver_idx, int16_t driver_value); + #if ACTUATORS_NB extern uint32_t actuators_delay_time; @@ -59,13 +77,13 @@ 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) + pprz_t pprz_val; ///< Actuator value in PPRZ units + int16_t driver_val; ///< Actuator value in driver units (scaling from servo in airframe.h) + struct act_config_t config; ///< Actuator config + act_set_t set; ///< Actuator set callback function }; - /** Actuators array. - * Temporary storage (for debugging purpose, downlinked via telemetry) * */ extern struct actuator_t actuators[ACTUATORS_NB]; @@ -76,11 +94,48 @@ extern struct actuator_t actuators[ACTUATORS_NB]; extern int16_t actuators_pprz[ACTUATORS_NB]; /** Set actuators. + * @param pprz_idx general actuators index + * @param pprz_value actuator's value in pprz unit + */ +extern void actuator_set(uint8_t pprz_idx, pprz_t pprz_value); + +/** Backward compatibility * @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) _ActuatorSet(_n, _v) +#define ActuatorSet(_n, _v) actuator_set(SERVO_ ## _n ## _IDX, _v) + +/** Get current actuator value in driver unit + */ +extern int16_t actuator_get(uint8_t pprz_idx); + +/** Get global index from driver index + */ +extern uint8_t actuator_get_idx(uint8_t driver_no, uint8_t servo_idx); + +/** Get min actuator value from driver index + */ +extern int32_t actuator_get_min(uint8_t driver_no, uint8_t servo_idx); + +/** Get max actuator value from driver index + */ +extern int32_t actuator_get_max(uint8_t driver_no, uint8_t servo_idx); + +/** Get neutral actuator value from driver index + */ +extern int32_t actuator_get_neutral(uint8_t driver_no, uint8_t servo_idx); + +/** Set min actuator value from driver index + */ +extern void actuator_set_min(uint8_t driver_no, uint8_t servo_idx, int32_t min); + +/** Set max actuator value from driver index + */ +extern void actuator_set_max(uint8_t driver_no, uint8_t servo_idx, int32_t max); + +/** Set neutral actuator value from driver index + */ +extern void actuator_set_neutral(uint8_t driver_no, uint8_t servo_idx, int32_t neutral); #endif /* ACTUATORS_NB */ diff --git a/sw/airborne/modules/actuators/actuators_asctec_v2.c b/sw/airborne/modules/actuators/actuators_asctec_v2.c index 4419702116..7e17605c33 100644 --- a/sw/airborne/modules/actuators/actuators_asctec_v2.c +++ b/sw/airborne/modules/actuators/actuators_asctec_v2.c @@ -48,7 +48,7 @@ void actuators_asctec_v2_init(void) } -void actuators_asctec_v2_set(void) +void actuators_asctec_v2_commit(void) { #if defined ACTUATORS_START_DELAY && ! defined SITL if (!actuators_delay_done) { @@ -130,3 +130,8 @@ void actuators_asctec_v2_set(void) } +void actuators_asctec_v2_set(uint8_t idx, int16_t value) +{ + actuators_asctec_v2.cmds[idx] = value; +} + diff --git a/sw/airborne/modules/actuators/actuators_asctec_v2.h b/sw/airborne/modules/actuators/actuators_asctec_v2.h index 3a68372a3f..dffb45ffeb 100644 --- a/sw/airborne/modules/actuators/actuators_asctec_v2.h +++ b/sw/airborne/modules/actuators/actuators_asctec_v2.h @@ -67,11 +67,12 @@ extern struct ActuatorsAsctecV2 actuators_asctec_v2; } extern void actuators_asctec_v2_init(void); -extern void actuators_asctec_v2_set(void); +extern void actuators_asctec_v2_commit(void); +extern void actuators_asctec_v2_set(uint8_t idx, int16_t value); -#define ActuatorAsctec_v2Set(_i, _v) { actuators_asctec_v2.cmds[_i] = _v; } +#define ActuatorAsctec_v2Set actuators_asctec_v2_set #define ActuatorsAsctec_v2Init() actuators_asctec_v2_init() -#define ActuatorsAsctec_v2Commit() actuators_asctec_v2_set() +#define ActuatorsAsctec_v2Commit() actuators_asctec_v2_commit() #endif /* ACTUATORS_ASCTEC_H */ diff --git a/sw/airborne/modules/actuators/actuators_dshot.c b/sw/airborne/modules/actuators/actuators_dshot.c new file mode 100644 index 0000000000..27b0cfb424 --- /dev/null +++ b/sw/airborne/modules/actuators/actuators_dshot.c @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2025 Gautier Hattenberger + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file "modules/actuators/actuators_dshot.c" + * @author Gautier Hattenberger + */ + +#include "modules/actuators/actuators_dshot.h" + +struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB]; + +void actuators_dshot_set(uint8_t idx, int16_t value) +{ + if (value == 0) { + actuators_dshot_values[idx].cmd = 0; + } else { + actuators_dshot_values[idx].cmd = value + ACTUATORS_DSHOT_OFFSET; + } +} + diff --git a/sw/airborne/modules/actuators/actuators_dshot.h b/sw/airborne/modules/actuators/actuators_dshot.h index 99fcaf2b9e..c45bb7bb15 100644 --- a/sw/airborne/modules/actuators/actuators_dshot.h +++ b/sw/airborne/modules/actuators/actuators_dshot.h @@ -28,7 +28,6 @@ #define ACTUATORS_DSHOT_H #include "std.h" -#include "modules/actuators/actuators_dshot_arch.h" /** In normal DSHOT, first 48 values are special commands * this offset allow to use 0 as the no-throttle command @@ -62,14 +61,12 @@ extern struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB]; */ extern void actuators_dshot_arch_init(void); extern void actuators_dshot_arch_commit(void); +extern void actuators_dshot_set(uint8_t idx, int16_t value); /* Actuator macros */ -#define ActuatorDShotSet(_i, _v) { \ - if (_v == 0) { actuators_dshot_values[_i].cmd = 0; } \ - else { actuators_dshot_values[_i].cmd = _v + ACTUATORS_DSHOT_OFFSET; } \ -} -#define ActuatorsDShotInit() actuators_dshot_arch_init() -#define ActuatorsDShotCommit() actuators_dshot_arch_commit() +#define ActuatorDShotSet actuators_dshot_set +#define ActuatorsDShotInit actuators_dshot_arch_init +#define ActuatorsDShotCommit actuators_dshot_arch_commit #endif diff --git a/sw/airborne/modules/actuators/actuators_esc32.c b/sw/airborne/modules/actuators/actuators_esc32.c index 21a4236995..195473fe20 100644 --- a/sw/airborne/modules/actuators/actuators_esc32.c +++ b/sw/airborne/modules/actuators/actuators_esc32.c @@ -70,12 +70,12 @@ static inline void actuators_esc32_grant_idx(uint8_t *data); static inline void actuators_esc32_proc_telem(uint8_t *data); /** Set the commands (either RPM or duty cycle) */ -void actuators_esc32_set(uint8_t i, uint16_t v) +void actuators_esc32_set(uint8_t i, int16_t v) { #ifdef ACTUAOTRS_ESC32_RPM #else // In the airframe file the ESC's duty cycle is defined as 10*precentage - actuators_esc32.cmds[actuators_esc32.escs_sorted[i]] = v * (65535 / 1000); + actuators_esc32.cmds[actuators_esc32.escs_sorted[i]] = (uint16_t)v * (65535 / 1000); #endif } diff --git a/sw/airborne/modules/actuators/actuators_esc32.h b/sw/airborne/modules/actuators/actuators_esc32.h index 17ca10d4aa..af6b560675 100644 --- a/sw/airborne/modules/actuators/actuators_esc32.h +++ b/sw/airborne/modules/actuators/actuators_esc32.h @@ -199,10 +199,10 @@ extern struct ESC32 actuators_esc32; extern void actuators_esc32_init(void); extern void actuators_esc32_commit(void); -extern void actuators_esc32_set(uint8_t i, uint16_t v); +extern void actuators_esc32_set(uint8_t i, int16_t v); extern void actuators_esc32_config_cmd(uint8_t i); -#define ActuatorESC32Set(_i, _v) { actuators_esc32_set(_i, _v); } +#define ActuatorESC32Set actuators_esc32_set #define ActuatorsESC32Init() actuators_esc32_init() #define ActuatorsESC32Commit() actuators_esc32_commit() diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.c b/sw/airborne/modules/actuators/actuators_faulhaber.c index c585e91b21..73d3772fd9 100644 --- a/sw/airborne/modules/actuators/actuators_faulhaber.c +++ b/sw/airborne/modules/actuators/actuators_faulhaber.c @@ -470,3 +470,11 @@ void actuators_faulhaber_SetMode(uint8_t mode) faulhaber.mode = mode; faulhaber.state = 0; } + +void actuators_faulhaber_set(uint8_t idx UNUSED, int16_t value UNUSED) +{ +#if !USE_NPS + faulhaber.setpoint_position = ((get_servo_max_FAULHABER(0) - value) + + get_servo_min_FAULHABER(0))*ACTUATORS_FAULHABER_COMMAND_SCALE; +#endif +} diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.h b/sw/airborne/modules/actuators/actuators_faulhaber.h index 164b1bff95..75a1701db1 100644 --- a/sw/airborne/modules/actuators/actuators_faulhaber.h +++ b/sw/airborne/modules/actuators/actuators_faulhaber.h @@ -40,7 +40,7 @@ enum faulhaber_modes_t { struct faulhaber_t { enum faulhaber_modes_t mode; ///< Current mode of the controller uint8_t state; ///< The state of the mode - + float p_gain; ///< The proportional gain of the velocity controller int32_t max_velocity; ///< The maximum velocity of the controller @@ -58,18 +58,13 @@ extern void actuators_faulhaber_init(void); extern void actuators_faulhaber_periodic(void); extern void actuators_faulhaber_event(void); extern void actuators_faulhaber_SetMode(uint8_t mode); +extern void actuators_faulhaber_set(uint8_t idx, int16_t value); #define ACTUATORS_FAULHABER_COMMAND_SCALE 1000.0f -#if USE_NPS -#define ActuatorsFaulhaberInit() {} -#define ActuatorFaulhaberSet(_i, _v) {} -#define ActuatorsFaulhaberCommit() {} -#else +#define ActuatorFaulhaberSet actuators_faulhaber_set #define ActuatorsFaulhaberInit() actuators_faulhaber_init() -#define ActuatorFaulhaberSet(_i, _v) { faulhaber.setpoint_position = ((get_servo_max_FAULHABER(0)-_v) + get_servo_min_FAULHABER(0))*ACTUATORS_FAULHABER_COMMAND_SCALE; } -#define ActuatorsFaulhaberCommit() {} -#endif +#define ActuatorsFaulhaberCommit() {} #endif /* ACTUATORS_FAULHABER_H */ diff --git a/sw/airborne/modules/actuators/actuators_hitl.c b/sw/airborne/modules/actuators/actuators_hitl.c index 6b124d5660..b6c6188bf0 100644 --- a/sw/airborne/modules/actuators/actuators_hitl.c +++ b/sw/airborne/modules/actuators/actuators_hitl.c @@ -33,7 +33,7 @@ #endif -#ifndef HITL_DEVICE +#ifndef HITL_DEVICE #error "HITL_DEVICE must be defined" #endif diff --git a/sw/airborne/modules/actuators/actuators_md25.c b/sw/airborne/modules/actuators/actuators_md25.c index ea362a6dbc..66d9a01a2a 100644 --- a/sw/airborne/modules/actuators/actuators_md25.c +++ b/sw/airborne/modules/actuators/actuators_md25.c @@ -105,7 +105,7 @@ void actuators_md25_periodic(void) } } -void actuators_md25_set(void) +void actuators_md25_commit(void) { if (actuators_md25.initialized && actuators_md25.trans_cmd.status == I2CTransDone) { actuators_md25.trans_cmd.buf[0] = MD25_REG_SPEED1; @@ -115,6 +115,11 @@ void actuators_md25_set(void) } } +void actuators_md25_set(uint8_t idx, int16_t value) +{ + actuators_md25.cmds[idx] = value; +} + #define Int32FromBuf(_buf,_idx) ((int32_t)(((uint32_t)_buf[_idx]<<24) | ((uint32_t)_buf[_idx+1]<<16) | ((uint32_t)_buf[_idx+2]<<8) | _buf[_idx+3])) void actuators_md25_event(void) diff --git a/sw/airborne/modules/actuators/actuators_md25.h b/sw/airborne/modules/actuators/actuators_md25.h index 946165ea21..1c7456eb84 100644 --- a/sw/airborne/modules/actuators/actuators_md25.h +++ b/sw/airborne/modules/actuators/actuators_md25.h @@ -48,10 +48,11 @@ extern struct ActuatorsMD25 actuators_md25; extern void actuators_md25_init(void); extern void actuators_md25_periodic(void); extern void actuators_md25_event(void); -extern void actuators_md25_set(void); +extern void actuators_md25_commit(void); +extern void actuators_md25_set(uint8_t idx, int16_t value); /* Actuator macros */ -#define ActuatorMD25Set(_i, _v) { actuators_md25.cmds[_i] = _v; } +#define ActuatorMD25Set actuators_md25_set #define ActuatorsMD25Init() actuators_md25_init() #define ActuatorsMD25Commit() actuators_md25_set() diff --git a/sw/airborne/modules/actuators/actuators_ostrich.c b/sw/airborne/modules/actuators/actuators_ostrich.c index 636e5255f2..10217a61ba 100644 --- a/sw/airborne/modules/actuators/actuators_ostrich.c +++ b/sw/airborne/modules/actuators/actuators_ostrich.c @@ -112,4 +112,8 @@ void actuators_ostrich_periodic() uart_put_buffer(ostrich_dev, 0, raw_message.bytes, 9); } +void actuators_ostrich_set(uint8_t idx, int16_t value) +{ + actuators_ostrich.cmds[idx] = value; +} diff --git a/sw/airborne/modules/actuators/actuators_ostrich.h b/sw/airborne/modules/actuators/actuators_ostrich.h index 24d50bb3ce..e28c0ce52f 100644 --- a/sw/airborne/modules/actuators/actuators_ostrich.h +++ b/sw/airborne/modules/actuators/actuators_ostrich.h @@ -39,9 +39,10 @@ extern struct ActuatorsOstrich actuators_ostrich; extern void actuators_ostrich_init(void); extern void actuators_ostrich_periodic(void); +extern void actuators_ostrich_set(uint8_t idx, int16_t value); /* Actuator macros */ -#define ActuatorOSTRICHSet(_i, _v) { actuators_ostrich.cmds[_i] = _v; } +#define ActuatorOSTRICHSet actuators_ostrich_set #define ActuatorsOSTRICHInit() actuators_ostrich_init() #define ActuatorsOSTRICHCommit() {} diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.h b/sw/airborne/modules/actuators/actuators_pwm.c similarity index 69% rename from sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.h rename to sw/airborne/modules/actuators/actuators_pwm.c index 9436b70f07..63ffdf7c05 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.h +++ b/sw/airborne/modules/actuators/actuators_pwm.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Gautier Hattenberger + * Copyright (C) 2025 Gautier Hattenberger * * This file is part of paparazzi * @@ -19,18 +19,19 @@ */ /** - * @file "modules/actuators/actuators_dshot_arch.h" + * @file "modules/actuators/actuators_pwm.c" * @author Gautier Hattenberger - * Driver for DSHOT speed controller protocol - * Arch dependent part */ -#ifndef ACTUATORS_DSHOT_H -#define ACTUATORS_DSHOT_H +#include "modules/actuators/actuators_pwm.h" -#include "std.h" -#include +int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; + +void actuators_pwm_set(uint8_t idx, int16_t value) +{ + if (idx < ACTUATORS_PWM_NB) { + actuators_pwm_values[idx] = value; + } +} -#include BOARD_CONFIG -#endif /* ACTUATORS_DSHOT_ARCH_H */ diff --git a/sw/airborne/modules/actuators/actuators_pwm.h b/sw/airborne/modules/actuators/actuators_pwm.h index 9d832751f3..c37a374c21 100644 --- a/sw/airborne/modules/actuators/actuators_pwm.h +++ b/sw/airborne/modules/actuators/actuators_pwm.h @@ -22,13 +22,32 @@ #ifndef ACTUATORS_PWM_H #define ACTUATORS_PWM_H +#include "std.h" #include "modules/actuators/actuators_pwm_arch.h" -/** Arch dependent init file. +#ifndef ACTUATORS_PWM_NB +#define ACTUATORS_PWM_NB 8 +#endif + +extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; + +/** Arch dependent init function. * implemented in arch files */ extern void actuators_pwm_arch_init(void); -#define ActuatorsPwmInit() actuators_pwm_arch_init() +/** Arch dependent commit function. + */ +extern void actuators_pwm_arch_commit(void); + +/** Set actuator value in array + */ +extern void actuators_pwm_set(uint8_t idx, int16_t value); + +/** Compatibility macros + */ +#define ActuatorPwmSet actuators_pwm_set +#define ActuatorsPwmInit actuators_pwm_arch_init +#define ActuatorsPwmCommit actuators_pwm_arch_commit #endif /* ACTUATORS_PWM_H */ diff --git a/sw/airborne/modules/actuators/actuators_sbus.c b/sw/airborne/modules/actuators/actuators_sbus.c index a5e57c83c9..9d4ba8af0b 100644 --- a/sw/airborne/modules/actuators/actuators_sbus.c +++ b/sw/airborne/modules/actuators/actuators_sbus.c @@ -61,7 +61,7 @@ void actuators_sbus_init(void) /* * Transmit the sbus output at 1 / 7ms */ -void actuators_sbus_set(void) +void actuators_sbus_commit(void) { static uint8_t cnt = 0; @@ -73,6 +73,11 @@ void actuators_sbus_set(void) } } +void actuators_sbus_set(uint8_t idx, int16_t value) +{ + actuators_sbus.cmds[idx] = value; +} + /* * Actually transmit the sbus output on a link device * diff --git a/sw/airborne/modules/actuators/actuators_sbus.h b/sw/airborne/modules/actuators/actuators_sbus.h index d45feb2018..cd2c19a877 100644 --- a/sw/airborne/modules/actuators/actuators_sbus.h +++ b/sw/airborne/modules/actuators/actuators_sbus.h @@ -41,12 +41,13 @@ struct ActuatorsSbus { /* Functions used in actuator macros */ extern struct ActuatorsSbus actuators_sbus; extern void actuators_sbus_init(void); -extern void actuators_sbus_set(void); +extern void actuators_sbus_commit(void); +extern void actuators_sbus_set(uint8_t idx, int16_t value); /* Actuator macros */ -#define ActuatorSbusSet(_i, _v) { actuators_sbus.cmds[_i] = _v; } +#define ActuatorSbusSet actuators_sbus_set #define ActuatorsSbusInit() actuators_sbus_init() -#define ActuatorsSbusCommit() actuators_sbus_set() +#define ActuatorsSbusCommit() actuators_sbus_commit() #endif /* ACTUATORS_SBUS_H */ diff --git a/sw/airborne/modules/actuators/actuators_spektrum.c b/sw/airborne/modules/actuators/actuators_spektrum.c index 4056b27d61..8c758a4c95 100644 --- a/sw/airborne/modules/actuators/actuators_spektrum.c +++ b/sw/airborne/modules/actuators/actuators_spektrum.c @@ -60,7 +60,7 @@ void actuators_spektrum_init(void) /* * Transmit the spektrum output at ~90Hz */ -void actuators_spektrum_set(void) +void actuators_spektrum_commit(void) { static uint8_t cnt = 0; @@ -77,6 +77,15 @@ void actuators_spektrum_set(void) } } +/* + * Set actuator value in array + */ +void actuators_spektrum_set(uint8_t idx, int16_t value) +{ + actuators_spektrum.cmds[idx] = value; +} + + /* * Actually transmit the spektrum output on a link device */ diff --git a/sw/airborne/modules/actuators/actuators_spektrum.h b/sw/airborne/modules/actuators/actuators_spektrum.h index d62ce526f7..79723acf06 100644 --- a/sw/airborne/modules/actuators/actuators_spektrum.h +++ b/sw/airborne/modules/actuators/actuators_spektrum.h @@ -42,12 +42,13 @@ struct ActuatorsSpektrum { /* Functions used in actuator macros */ extern struct ActuatorsSpektrum actuators_spektrum; extern void actuators_spektrum_init(void); -extern void actuators_spektrum_set(void); +extern void actuators_spektrum_commit(void); +extern void actuators_spektrum_set(uint8_t idx, int16_t value); /* Actuator macros */ -#define ActuatorSpektrumSet(_i, _v) { actuators_spektrum.cmds[_i] = _v; } +#define ActuatorSpektrumSet actuators_spektrum_set #define ActuatorsSpektrumInit() actuators_spektrum_init() -#define ActuatorsSpektrumCommit() actuators_spektrum_set() +#define ActuatorsSpektrumCommit() actuators_spektrum_commit() #endif /* ACTUATORS_SPEKTRUM_H */ diff --git a/sw/airborne/modules/actuators/actuators_sts3032.c b/sw/airborne/modules/actuators/actuators_sts3032.c index 8743dc880b..a04a1dab54 100644 --- a/sw/airborne/modules/actuators/actuators_sts3032.c +++ b/sw/airborne/modules/actuators/actuators_sts3032.c @@ -120,6 +120,12 @@ void actuators_sts3032_periodic(void) } +void actuators_sts3032_set(uint8_t idx, int16_t value) +{ + if (sts3032_enabled) { + sts3032_write_pos(&sts, sts.ids[idx], value); + } +} static void sts3032_event(struct sts3032 *sts) diff --git a/sw/airborne/modules/actuators/actuators_sts3032.h b/sw/airborne/modules/actuators/actuators_sts3032.h index d221fe3111..a07e715dcc 100644 --- a/sw/airborne/modules/actuators/actuators_sts3032.h +++ b/sw/airborne/modules/actuators/actuators_sts3032.h @@ -68,11 +68,12 @@ extern struct sts3032 sts; extern void actuators_sts3032_init(void); extern void actuators_sts3032_periodic(void); extern void actuators_sts3032_event(void); -uint8_t id_idx(struct sts3032 *sts, uint8_t id); -void sts3032_write_pos(struct sts3032 *sts, uint8_t id, int16_t position); +extern void actuators_sts3032_set(uint8_t idx, int16_t value); +extern uint8_t id_idx(struct sts3032 *sts, uint8_t id); +extern void sts3032_write_pos(struct sts3032 *sts, uint8_t id, int16_t position); /* Actuator macros */ -#define ActuatorSTS3032Set(_i, _v) { if(sts3032_enabled) {sts3032_write_pos(&sts, sts.ids[_i], _v);} } +#define ActuatorSTS3032Set actuators_sts3032_set #define ActuatorsSTS3032Init() actuators_sts3032_init() #define ActuatorsSTS3032Commit() {} diff --git a/sw/airborne/modules/actuators/actuators_uavcan.c b/sw/airborne/modules/actuators/actuators_uavcan.c index 39f6fcd24c..c379178a79 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan.c +++ b/sw/airborne/modules/actuators/actuators_uavcan.c @@ -59,15 +59,35 @@ struct actuators_uavcan_telem_t { /* The transmitted actuator values */ #ifdef SERVOS_UAVCAN1_NB int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB]; + +void actuators_uavcan1_set(uint8_t idx, int16_t value) +{ + actuators_uavcan1_values[idx] = value; +} #endif #ifdef SERVOS_UAVCAN2_NB int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB]; + +void actuators_uavcan2_set(uint8_t idx, int16_t value) +{ + actuators_uavcan2_values[idx] = value; +} #endif #ifdef SERVOS_UAVCAN1CMD_NB int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB]; + +void actuators_uavcan1cmd_set(uint8_t idx, int16_t value) +{ + actuators_uavcan1cmd_values[idx] = value; +} #endif #ifdef SERVOS_UAVCAN2CMD_NB int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB]; + +void actuators_uavcan2cmd_set(uint8_t idx, int16_t value) +{ + actuators_uavcan2cmd_values[idx] = value; +} #endif /* Set the actual telemetry length (ID's from actuators can't collide with the command version) */ diff --git a/sw/airborne/modules/actuators/actuators_uavcan1.h b/sw/airborne/modules/actuators/actuators_uavcan1.h index 9c3b53918f..9563b80182 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan1.h +++ b/sw/airborne/modules/actuators/actuators_uavcan1.h @@ -27,14 +27,16 @@ /** Stub file needed per uavcan interface because of generator */ extern int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB]; +extern void actuators_uavcan1_set(uint8_t idx, int16_t value); + #if USE_NPS +#define ActuatorUavcan1Set NULL #define ActuatorsUavcan1Init() {} -#define ActuatorUavcan1Set(_i, _v) {} -#define ActuatorsUavcan1Commit() {} +#define ActuatorsUavcan1Commit() {} #else +#define ActuatorUavcan1Set actuators_uavcan1_set #define ActuatorsUavcan1Init() actuators_uavcan_init(&uavcan1) -#define ActuatorUavcan1Set(_i, _v) { actuators_uavcan1_values[_i] = _v; } #define ActuatorsUavcan1Commit() RunOnceEvery(ACTUATORS_UAVCAN_RAW_DIV,actuators_uavcan_commit(&uavcan1, actuators_uavcan1_values, SERVOS_UAVCAN1_NB)) #endif -#endif /* ACTUATORS_UAVCAN1_H */ \ No newline at end of file +#endif /* ACTUATORS_UAVCAN1_H */ diff --git a/sw/airborne/modules/actuators/actuators_uavcan1cmd.h b/sw/airborne/modules/actuators/actuators_uavcan1cmd.h index 3cf1ba86d6..48d144d165 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan1cmd.h +++ b/sw/airborne/modules/actuators/actuators_uavcan1cmd.h @@ -27,14 +27,16 @@ /** Stub file needed per uavcan interface because of generator */ extern int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB]; +extern void actuators_uavcan1cmd_set(uint8_t idx, int16_t value); + #if USE_NPS +#define ActuatorUavcan1CmdSet NULL #define ActuatorsUavcan1CmdInit() {} -#define ActuatorUavcan1CmdSet(_i, _v) {} #define ActuatorsUavcan1CmdCommit() {} #else +#define ActuatorUavcan1CmdSet actuators_uavcan1cmd_set #define ActuatorsUavcan1CmdInit() actuators_uavcan_init(&uavcan1) -#define ActuatorUavcan1CmdSet(_i, _v) { actuators_uavcan1cmd_values[_i] = _v; } #define ActuatorsUavcan1CmdCommit() RunOnceEvery(ACTUATORS_UAVCAN_CMD_DIV,actuators_uavcan_cmd_commit(&uavcan1, actuators_uavcan1cmd_values, SERVOS_UAVCAN1CMD_NB)) #endif -#endif /* ACTUATORS_UAVCAN1_CMD_H */ \ No newline at end of file +#endif /* ACTUATORS_UAVCAN1_CMD_H */ diff --git a/sw/airborne/modules/actuators/actuators_uavcan2.h b/sw/airborne/modules/actuators/actuators_uavcan2.h index b873356d52..b54b006ba4 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan2.h +++ b/sw/airborne/modules/actuators/actuators_uavcan2.h @@ -27,14 +27,16 @@ /** Stub file needed per interface because of generator */ extern int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB]; +extern void actuators_uavcan2_set(uint8_t idx, int16_t value); + #if USE_NPS +#define ActuatorUavcan2Set NULL #define ActuatorsUavcan2Init() {} -#define ActuatorUavcan2Set(_i, _v) {} #define ActuatorsUavcan2Commit() {} #else +#define ActuatorUavcan2Set actuators_uavcan2_set #define ActuatorsUavcan2Init() actuators_uavcan_init(&uavcan2) -#define ActuatorUavcan2Set(_i, _v) { actuators_uavcan2_values[_i] = _v; } #define ActuatorsUavcan2Commit() RunOnceEvery(ACTUATORS_UAVCAN_RAW_DIV,actuators_uavcan_commit(&uavcan2, actuators_uavcan2_values, SERVOS_UAVCAN2_NB)) #endif -#endif /* ACTUATORS_UAVCAN2_H */ \ No newline at end of file +#endif /* ACTUATORS_UAVCAN2_H */ diff --git a/sw/airborne/modules/actuators/actuators_uavcan2cmd.h b/sw/airborne/modules/actuators/actuators_uavcan2cmd.h index df76d7a862..8eca371886 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan2cmd.h +++ b/sw/airborne/modules/actuators/actuators_uavcan2cmd.h @@ -27,14 +27,16 @@ /** Stub file needed per uavcan interface because of generator */ extern int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB]; +extern void actuators_uavcan2cmd_set(uint8_t idx, int16_t value); + #if USE_NPS +#define ActuatorUavcan2CmdSet NULL #define ActuatorsUavcan2CmdInit() {} -#define ActuatorUavcan2CmdSet(_i, _v) {} #define ActuatorsUavcan2CmdCommit() {} #else +#define ActuatorUavcan2CmdSet actuators_uavcan2cmd_set #define ActuatorsUavcan2CmdInit() actuators_uavcan_init(&uavcan2) -#define ActuatorUavcan2CmdSet(_i, _v) { actuators_uavcan2cmd_values[_i] = _v; } #define ActuatorsUavcan2CmdCommit() RunOnceEvery(ACTUATORS_UAVCAN_CMD_DIV,actuators_uavcan_cmd_commit(&uavcan2, actuators_uavcan2cmd_values, SERVOS_UAVCAN2CMD_NB)) #endif -#endif /* ACTUATORS_UAVCAN2_CMD_H */ \ No newline at end of file +#endif /* ACTUATORS_UAVCAN2_CMD_H */ diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index ff61f27dc2..30bd909207 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -37,6 +37,7 @@ let fos = float_of_string let sof = fun x -> if mod_float x 1. = 0. then Printf.sprintf "%.0f" x else string_of_float x let servos_drivers = Hashtbl.create 3 +let drivers_no = Hashtbl.create 3 let get_servo_driver = fun servo_name -> try @@ -204,7 +205,7 @@ let print_reverse_servo_table = fun out driver servos -> fprintf out " switch (_idx) {\n"; List.iter (fun c -> let name = ExtXml.attrib c "name" in - fprintf out " case SERVO_%s_DRIVER_NO: return SERVO_%s_MIN;\n" name name; + fprintf out " case SERVO_%s_DRIVER_IDX: return SERVO_%s_MIN;\n" name name; ) servos; fprintf out " default: return 0;\n"; fprintf out " };\n"; @@ -213,7 +214,7 @@ let print_reverse_servo_table = fun out driver servos -> fprintf out " switch (_idx) {\n"; List.iter (fun c -> let name = ExtXml.attrib c "name" in - fprintf out " case SERVO_%s_DRIVER_NO: return SERVO_%s_MAX;\n" name name; + fprintf out " case SERVO_%s_DRIVER_IDX: return SERVO_%s_MAX;\n" name name; ) servos; fprintf out " default: return 0;\n"; fprintf out " };\n"; @@ -222,7 +223,7 @@ let print_reverse_servo_table = fun out driver servos -> fprintf out " switch (_idx) {\n"; List.iter (fun c -> let name = ExtXml.attrib c "name" in - fprintf out " case SERVO_%s_DRIVER_NO: return SERVO_%s_IDX;\n" name name; + fprintf out " case SERVO_%s_DRIVER_IDX: return SERVO_%s_IDX;\n" name name; ) servos; fprintf out " default: return 0;\n"; fprintf out " };\n"; @@ -233,8 +234,11 @@ let parse_servo = fun out driver c -> let name = "SERVO_"^shortname and no_servo = int_of_string (ExtXml.attrib c "no") in let global_idx = Hashtbl.length servos_drivers in + if not (Hashtbl.mem drivers_no driver) then + Hashtbl.add drivers_no driver (Hashtbl.length drivers_no); - define_out out (name^"_DRIVER_NO") (string_of_int no_servo); + define_out out (name^"_DRIVER_NO") (string_of_int (Hashtbl.find drivers_no driver)); + define_out out (name^"_DRIVER_IDX") (string_of_int no_servo); define_out out (name^"_IDX") (string_of_int global_idx); let s_min = fos (ExtXml.attrib c "min" ) @@ -266,19 +270,27 @@ let preprocess_value = fun s v prefix -> let s = Str.global_replace pprz_value (sprintf "%s[%s_\\1]" v prefix) s in Str.global_replace var_value "_var_\\1" s -let print_actuators_idx = fun out -> +let print_actuators_config = fun out -> + let actuators = Array.make (Hashtbl.length servos_drivers) "" in Hashtbl.iter (fun s (d, i) -> - (* Set servo macro *) - fprintf out "#define Set_%s_Servo(actuator_value_pprz) { \\\n" s; - fprintf out " int32_t servo_value;\\\n"; - fprintf out " int32_t command_value;\\\n\\\n"; - 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 : SERVO_%s_TRAVEL_DOWN); \\\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" + actuators.(i) <- sprintf " { \\ + .pprz_val = 0, \\ + .driver_val = 0, \\ + .config = { \\ + .driver_no = SERVO_%s_DRIVER_NO, \\ + .servo_idx = SERVO_%s_DRIVER_IDX, \\ + .min = SERVO_%s_MIN, \\ + .max = SERVO_%s_MAX, \\ + .neutral = SERVO_%s_NEUTRAL, \\ + .travel_up = SERVO_%s_TRAVEL_UP, \\ + .travel_down = SERVO_%s_TRAVEL_DOWN \\ + }, \\ + .set = Actuator%sSet \\ + }" s s s s s s s d; ) servos_drivers; + fprintf out "#define ACTUATORS_CONFIG { \\\n"; + fprintf out "%s" (String.concat ", \\\n" (Array.to_list actuators)); + fprintf out "\\\n }\n\n"; define_out out "ACTUATORS_NB" (string_of_int (Hashtbl.length servos_drivers)); fprintf out "\n" @@ -290,7 +302,7 @@ let parse_command_laws = fun out command -> and value = a "value" in let v = preprocess_value value "values" "COMMAND" in fprintf out " actuator_value_pprz = %s; \\\n" v; - fprintf out " Set_%s_Servo(actuator_value_pprz); \\\n\\\n" servo + fprintf out " ActuatorSet(%s, actuator_value_pprz); \\\n\\\n" servo | "let" -> let var = a "var" and value = a "value" in @@ -421,8 +433,8 @@ let rec parse_section = fun out ac_id s -> List.iter (parse_ap_only_commands out) (Xml.children s); fprintf out "}\n\n" | "command_laws" -> - (* print actuators index and set macros *) - print_actuators_idx out; + (* print actuators config *) + print_actuators_config out; (* print init and commit actuators macros *) let drivers = get_list_of_drivers () in fprintf out "#define AllActuatorsInit() { \\\n"; @@ -433,7 +445,7 @@ let rec parse_section = fun out ac_id s -> fprintf out "}\n\n"; (* print actuators from commands macro *) fprintf out "#define SetActuatorsFromCommands(values, AP_MODE) { \\\n"; - fprintf out " int32_t actuator_value_pprz;\\\n\\\n"; + if List.length (Xml.children s) > 0 then 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"; diff --git a/tests/modules/test_arch/modules/actuators/actuators_dualpwm_arch.h b/tests/modules/test_arch/modules/actuators/actuators_dualpwm_arch.h index 16661ca7a8..0df5bf0a09 100644 --- a/tests/modules/test_arch/modules/actuators/actuators_dualpwm_arch.h +++ b/tests/modules/test_arch/modules/actuators/actuators_dualpwm_arch.h @@ -28,7 +28,7 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorDualpwmSet(_i, _v) {} +#define ActuatorDualpwmSet NULL #define ActuatorsDualPwmCommit() {} extern void actuators_dualpwm_arch_init(void); diff --git a/tests/modules/test_arch/modules/actuators/actuators_pwm_arch.h b/tests/modules/test_arch/modules/actuators/actuators_pwm_arch.h index b599aa72da..ecf55de24a 100644 --- a/tests/modules/test_arch/modules/actuators/actuators_pwm_arch.h +++ b/tests/modules/test_arch/modules/actuators/actuators_pwm_arch.h @@ -28,9 +28,6 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define ActuatorPwmSet(_i, _v) {} -#define ActuatorsPwmCommit() {} - extern void actuators_pwm_arch_init(void); #endif /* ACTUATORS_PWM_ARCH_H */ diff --git a/tests/modules/test_arch/modules/actuators/servos_4015_MAT_hw.h b/tests/modules/test_arch/modules/actuators/servos_4015_MAT_hw.h index f3e00e1822..d561a13312 100644 --- a/tests/modules/test_arch/modules/actuators/servos_4015_MAT_hw.h +++ b/tests/modules/test_arch/modules/actuators/servos_4015_MAT_hw.h @@ -25,7 +25,7 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define Actuator4015Set(_i, _v) {} +#define Actuator4015Set NULL #define Actuators4015Init() {} #define Actuators4015Commit() {} diff --git a/tests/modules/test_arch/modules/actuators/servos_4017_hw.h b/tests/modules/test_arch/modules/actuators/servos_4017_hw.h index fad3ed1c7f..34d98d862d 100644 --- a/tests/modules/test_arch/modules/actuators/servos_4017_hw.h +++ b/tests/modules/test_arch/modules/actuators/servos_4017_hw.h @@ -25,7 +25,7 @@ #define SERVOS_TICS_OF_USEC(_v) (_v) -#define Actuator4017Set(_i, _v) {} +#define Actuator4017Set NULL #define Actuators4017Init() {} #define Actuators4017Commit() {} diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.h b/tests/modules/test_arch/modules/core/shell_arch.h similarity index 68% rename from sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.h rename to tests/modules/test_arch/modules/core/shell_arch.h index 8b3531c1d2..fe6f9f248b 100644 --- a/sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.h +++ b/tests/modules/test_arch/modules/core/shell_arch.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Gautier Hattenberger + * Copyright (C) Alexandre Bustico * * This file is part of paparazzi * @@ -18,15 +18,16 @@ * . */ -/** - * @file "modules/actuators/actuators_dshot_arch.h" - * @author Gautier Hattenberger - * Simulation driver for DSHOT speed controller protocol - * Arch dependent part +/** @file "modules/core/shell_arch.h" + * Dummy test file */ -#ifndef ACTUATORS_DSHOT_H -#define ACTUATORS_DSHOT_H +#ifndef SHELL_ARCH_H +#define SHELL_ARCH_H +typedef void shell_stream_t; +extern void shell_init_arch(void); -#endif /* ACTUATORS_DSHOT_ARCH_H */ +extern void chprintf(shell_stream_t *lchp, const char *fmt, ...) ; + +#endif // SHELL_ARCH_H