From 96d77b007887867e1f611d63aa4e8575a8caff64 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 20 Sep 2023 13:31:45 +0200 Subject: [PATCH 1/5] [rpm_actuators] actuators on a bus require indices --- conf/abi.xml | 2 +- sw/airborne/modules/actuators/actuators.h | 6 ++++++ sw/airborne/modules/actuators/actuators_uavcan.c | 10 ++++++++++ sw/airborne/modules/core/abi_common.h | 1 + 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/conf/abi.xml b/conf/abi.xml index add63110a60..9df5ece23d9 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -84,7 +84,7 @@ - + diff --git a/sw/airborne/modules/actuators/actuators.h b/sw/airborne/modules/actuators/actuators.h index d1b38eba2d1..93e6c052d75 100644 --- a/sw/airborne/modules/actuators/actuators.h +++ b/sw/airborne/modules/actuators/actuators.h @@ -40,6 +40,12 @@ extern void actuators_init(void); extern void actuators_periodic(void); +// Actuator RPM structure for ABI Message +struct rpm_act_t { + uint8_t actuator_idx; + int32_t rpm; +}; + #if ACTUATORS_NB extern uint32_t actuators_delay_time; diff --git a/sw/airborne/modules/actuators/actuators_uavcan.c b/sw/airborne/modules/actuators/actuators_uavcan.c index 51db1456a34..db01dfa3d44 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan.c +++ b/sw/airborne/modules/actuators/actuators_uavcan.c @@ -215,6 +215,16 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR } #endif #endif + +// Create struct for ABI +#ifdef SERVOS_UAVCAN1_NB + struct rpm_act_t rpm_message; + rpm_message.actuator_idx = esc_idx; + rpm_message.rpm = telem[esc_idx].rpm; + + // Send ABI message + AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, SERVOS_UAVCAN1_NB); +#endif } /** diff --git a/sw/airborne/modules/core/abi_common.h b/sw/airborne/modules/core/abi_common.h index afa7ffc9642..5a2d9dcd4e9 100644 --- a/sw/airborne/modules/core/abi_common.h +++ b/sw/airborne/modules/core/abi_common.h @@ -34,6 +34,7 @@ #include "math/pprz_algebra_float.h" #include "modules/gps/gps.h" #include "modules/radio_control/radio_control.h" +#include "modules/actuators/actuators.h" /* Include here headers with structure definition you may want to use with ABI * Ex: '#include "modules/gps/gps.h"' in order to use the GpsState structure */ From e5f8e2f1e140f75beeda1ae9f6ce50418b907659 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 21 Sep 2023 01:59:35 -0700 Subject: [PATCH 2/5] Fix all other ABI RPM messages --- .../arch/chibios/modules/actuators/actuators_dshot_arch.c | 7 +++++-- sw/airborne/boards/bebop/actuators.c | 7 ++++++- sw/airborne/boards/disco/actuators.c | 4 +++- .../rotorcraft/stabilization/stabilization_indi.c | 6 +++--- sw/airborne/modules/actuators/actuators_uavcan.c | 2 +- sw/airborne/modules/helicopter/throttle_curve.c | 6 +++--- sw/airborne/modules/ins/ins_flow.c | 6 +++--- sw/airborne/modules/sensors/rpm_sensor.c | 5 +++-- 8 files changed, 27 insertions(+), 16 deletions(-) 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 42dbad14474..39886f8231e 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c +++ b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c @@ -327,11 +327,14 @@ void actuators_dshot_arch_commit(void) dshotSendFrame(&DSHOTD9); #endif - uint16_t rpm_list[ACTUATORS_DSHOT_NB] = { 0 }; + + struct rpm_act_t rpm_list[ACTUATORS_DSHOT_NB] = { 0 }; for (uint8_t i = 0; i < ACTUATORS_DSHOT_NB; i++) { + rpm_list[i].actuator_idx = i; + rpm_list[i].rpm = 0; if (actuators_dshot_values[i].activated) { const DshotTelemetry *dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel); - rpm_list[i] = dtelem->rpm; + rpm_list[i].rpm = dtelem->rpm; } } AbiSendMsgRPM(RPM_DSHOT_ID, rpm_list, ACTUATORS_DSHOT_NB); diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c index 8198b047afc..2eac5a0f182 100644 --- a/sw/airborne/boards/bebop/actuators.c +++ b/sw/airborne/boards/bebop/actuators.c @@ -138,7 +138,12 @@ void actuators_bebop_commit(void) actuators_bebop.led = led_hw_values & 0x3; } // Send ABI message - AbiSendMsgRPM(RPM_SENSOR_ID, actuators_bebop.rpm_obs, 4); + struct rpm_act_t rpm_message[4]; + for (int i=0;i<4;i++) { + rpm_message[i].actuator_idx = i; + rpm_message[i].rpm = actuators_bebop.rpm_obs[i]; + } + AbiSendMsgRPM(RPM_SENSOR_ID, rpm_message, 4); } static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size) diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c index 182c94a0f9b..76ffd21326c 100644 --- a/sw/airborne/boards/disco/actuators.c +++ b/sw/airborne/boards/disco/actuators.c @@ -169,7 +169,9 @@ void actuators_disco_commit(void) } // Send ABI message - AbiSendMsgRPM(RPM_SENSOR_ID, &actuators_disco.rpm_obs, 1);//FIXME & or not + struct rpm_act_t rpm_message = {0, 0}; + rpm_message.rpm = actuators_disco.rpm_obs; + AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1);//FIXME & or not } static uint8_t actuators_disco_checksum(uint8_t *bytes, uint8_t size) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index dc8cf819d14..9f2788c3113 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -179,7 +179,7 @@ struct Int32Eulers stab_att_sp_euler; struct Int32Quat stab_att_sp_quat; abi_event rpm_ev; -static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act); +static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act); abi_event thrust_ev; static void thrust_cb(uint8_t sender_id, float thrust_increment); @@ -846,12 +846,12 @@ void calc_g1g2_pseudo_inv(void) } } -static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act) +static void rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t UNUSED *rpm_msg, uint8_t UNUSED num_act) { #if INDI_RPM_FEEDBACK int8_t i; for (i = 0; i < num_act; i++) { - act_obs[i] = (rpm[i] - get_servo_min(i)); + act_obs[i] = (rpm_msg.rpm[i] - get_servo_min(i)); act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i))); Bound(act_obs[i], 0, MAX_PPRZ); } diff --git a/sw/airborne/modules/actuators/actuators_uavcan.c b/sw/airborne/modules/actuators/actuators_uavcan.c index db01dfa3d44..5cd478ee99b 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan.c +++ b/sw/airborne/modules/actuators/actuators_uavcan.c @@ -223,7 +223,7 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR rpm_message.rpm = telem[esc_idx].rpm; // Send ABI message - AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, SERVOS_UAVCAN1_NB); + AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1); #endif } diff --git a/sw/airborne/modules/helicopter/throttle_curve.c b/sw/airborne/modules/helicopter/throttle_curve.c index 6379eb7b478..5c42c5e9d3e 100644 --- a/sw/airborne/modules/helicopter/throttle_curve.c +++ b/sw/airborne/modules/helicopter/throttle_curve.c @@ -54,7 +54,7 @@ #define THROTTLE_CURVE_RPM_ACT 0 #endif static abi_event rpm_ev; -static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act); +static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act); /* Initialize the throttle curves from the airframe file */ struct throttle_curve_t throttle_curve = { @@ -99,12 +99,12 @@ void throttle_curve_init(void) /** * RPM callback for RPM based control throttle curves */ -static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t *rpm, uint8_t num_act) +static void rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act) { if(num_act <= THROTTLE_CURVE_RPM_ACT) return; - throttle_curve.rpm_meas = rpm[THROTTLE_CURVE_RPM_ACT]; + throttle_curve.rpm_meas = rpm_msg[THROTTLE_CURVE_RPM_ACT].rpm; throttle_curve.rpm_measured = true; } diff --git a/sw/airborne/modules/ins/ins_flow.c b/sw/airborne/modules/ins/ins_flow.c index 2ea191977d0..4824f5951be 100644 --- a/sw/airborne/modules/ins/ins_flow.c +++ b/sw/airborne/modules/ins/ins_flow.c @@ -120,7 +120,7 @@ static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVe static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); void ins_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence); -static void ins_rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act); +static void ins_rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm, uint8_t num_act); static void aligner_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp __attribute__((unused)), struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, @@ -1467,11 +1467,11 @@ static void set_body_state_from_quat(void) } -static void ins_rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act) +static void ins_rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act) { ins_flow.RPM_num_act = num_act; for (int i = 0; i < num_act; i++) { - ins_flow.RPM[i] = rpm[i]; + ins_flow.RPM[i] = rpm_msg[i].rpm; } } diff --git a/sw/airborne/modules/sensors/rpm_sensor.c b/sw/airborne/modules/sensors/rpm_sensor.c index ae4d2b6c47f..ede7cf045a0 100644 --- a/sw/airborne/modules/sensors/rpm_sensor.c +++ b/sw/airborne/modules/sensors/rpm_sensor.c @@ -60,8 +60,9 @@ void rpm_sensor_init(void) /* RPM periodic */ void rpm_sensor_periodic(void) { - rpm = update_first_order_low_pass(&rpm_lp, rpm_sensor_get_rpm()); - AbiSendMsgRPM(RPM_SENSOR_ID, &rpm, 1); + struct rpm_act_t rpm_msg = {0, 0}; + rpm_msg.rpm = = update_first_order_low_pass(&rpm_lp, rpm_sensor_get_rpm()); + AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_msg, 1); } /* Get the RPM sensor */ From 6ceb7905255a179c78b88e6d3d2350413ac95001 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Thu, 21 Sep 2023 13:18:30 +0200 Subject: [PATCH 3/5] [tools] Add servo driver offset in gen_airframe --- .../modules/actuators/actuators_dshot_arch.c | 2 +- sw/airborne/boards/bebop/actuators.c | 6 ++++- sw/airborne/boards/disco/actuators.c | 7 +++++- .../modules/actuators/actuators_uavcan.c | 24 ++++++++++++++----- sw/tools/generators/gen_airframe.ml | 2 ++ 5 files changed, 32 insertions(+), 9 deletions(-) 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 39886f8231e..77d28ca6df4 100644 --- a/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c +++ b/sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c @@ -330,7 +330,7 @@ void actuators_dshot_arch_commit(void) struct rpm_act_t rpm_list[ACTUATORS_DSHOT_NB] = { 0 }; for (uint8_t i = 0; i < ACTUATORS_DSHOT_NB; i++) { - rpm_list[i].actuator_idx = i; + rpm_list[i].actuator_idx = ACTUATORS_DSHOT_OFFSET + i; rpm_list[i].rpm = 0; if (actuators_dshot_values[i].activated) { const DshotTelemetry *dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel); diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c index 2eac5a0f182..a4d6e39c75d 100644 --- a/sw/airborne/boards/bebop/actuators.c +++ b/sw/airborne/boards/bebop/actuators.c @@ -140,7 +140,11 @@ void actuators_bebop_commit(void) // Send ABI message struct rpm_act_t rpm_message[4]; for (int i=0;i<4;i++) { - rpm_message[i].actuator_idx = i; +#ifdef SERVOS_BEBOP_OFFSET + rpm_message[i].actuator_idx = SERVOS_BEBOP_OFFSET + i; +#else + rpm_message[i].actuator_idx = SERVOS_DEFAULT_OFFSET + i; +#endif rpm_message[i].rpm = actuators_bebop.rpm_obs[i]; } AbiSendMsgRPM(RPM_SENSOR_ID, rpm_message, 4); diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c index 76ffd21326c..aaa74b3db05 100644 --- a/sw/airborne/boards/disco/actuators.c +++ b/sw/airborne/boards/disco/actuators.c @@ -169,7 +169,12 @@ void actuators_disco_commit(void) } // Send ABI message - struct rpm_act_t rpm_message = {0, 0}; + struct rpm_act_t rpm_message = {0}; +#ifdef SERVOS_DISCO_OFFSET + rpm_message.actuator_idx = SERVOS_DISCO_OFFSET; +#else + rpm_message.actuator_idx = SERVOS_DEFAULT_OFFSET; +#endif rpm_message.rpm = actuators_disco.rpm_obs; AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1);//FIXME & or not } diff --git a/sw/airborne/modules/actuators/actuators_uavcan.c b/sw/airborne/modules/actuators/actuators_uavcan.c index 5cd478ee99b..b9ceea867ae 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan.c +++ b/sw/airborne/modules/actuators/actuators_uavcan.c @@ -216,14 +216,26 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR #endif #endif -// Create struct for ABI + // Feedback ABI RPM messages #ifdef SERVOS_UAVCAN1_NB - struct rpm_act_t rpm_message; - rpm_message.actuator_idx = esc_idx; - rpm_message.rpm = telem[esc_idx].rpm; + if (iface == &uavcan1) { + struct rpm_act_t rpm_message; + rpm_message.actuator_idx = SERVOS_UAVCAN1_OFFSET + esc_idx; + rpm_message.rpm = telem[esc_idx].rpm; - // Send ABI message - AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1); + // Send ABI message + AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1); + } +#endif +#ifdef SERVOS_UAVCAN2_NB + if (iface == &uavcan2) { + struct rpm_act_t rpm_message; + rpm_message.actuator_idx = SERVOS_UAVCAN2_OFFSET + esc_idx; + rpm_message.rpm = telem[esc_idx].rpm; + + // Send ABI message + AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1); + } #endif } diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index 7642ab4ca58..44893179c9f 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -315,8 +315,10 @@ let rec parse_section = fun out ac_id s -> let driver = ExtXml.attrib_or_default s "driver" "Default" in let servos = Xml.children s in let nb_servos = List.fold_right (fun s m -> max (int_of_string (ExtXml.attrib s "no")) m) servos min_int + 1 in + let servos_offset = Hashtbl.length servos_drivers in define_out out (sprintf "SERVOS_%s_NB" (String.uppercase_ascii driver)) (string_of_int nb_servos); + define_out out (sprintf "SERVOS_%s_OFFSET" (String.uppercase_ascii driver)) (string_of_int servos_offset); fprintf out "#include \"modules/actuators/actuators_%s.h\"\n" (String.lowercase_ascii driver); fprintf out "\n"; List.iter (parse_servo out driver) servos; From c21cc015f8999524e9eef47793461a4f2fd06866 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 22 Sep 2023 04:07:40 -0700 Subject: [PATCH 4/5] [fix] test actuators_bebop --- conf/modules/actuators_bebop.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/conf/modules/actuators_bebop.xml b/conf/modules/actuators_bebop.xml index ae53df0c9f6..d29d96e480c 100644 --- a/conf/modules/actuators_bebop.xml +++ b/conf/modules/actuators_bebop.xml @@ -23,6 +23,7 @@ + From ccafc296b79001ff7a49a11d284871d755e6001e Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 22 Sep 2023 04:11:00 -0700 Subject: [PATCH 5/5] [test] disco fix --- conf/modules/actuators_disco.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/conf/modules/actuators_disco.xml b/conf/modules/actuators_disco.xml index 6481e4feec6..675313ed85e 100644 --- a/conf/modules/actuators_disco.xml +++ b/conf/modules/actuators_disco.xml @@ -21,6 +21,7 @@ +