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/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 @@
+
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 @@
+
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..77d28ca6df4 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 = 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);
- 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..a4d6e39c75d 100644
--- a/sw/airborne/boards/bebop/actuators.c
+++ b/sw/airborne/boards/bebop/actuators.c
@@ -138,7 +138,16 @@ 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++) {
+#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);
}
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..aaa74b3db05 100644
--- a/sw/airborne/boards/disco/actuators.c
+++ b/sw/airborne/boards/disco/actuators.c
@@ -169,7 +169,14 @@ 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};
+#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
}
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.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..b9ceea867ae 100644
--- a/sw/airborne/modules/actuators/actuators_uavcan.c
+++ b/sw/airborne/modules/actuators/actuators_uavcan.c
@@ -214,6 +214,28 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
electrical.current += uavcan2_telem[i].current;
}
#endif
+#endif
+
+ // Feedback ABI RPM messages
+#ifdef SERVOS_UAVCAN1_NB
+ 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);
+ }
+#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/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
*/
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 */
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;