Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 19:36:19 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[rpm_actuators] actuators on a bus require indices (#3089)

Browse Source 
* [rpm_actuators] actuators on a bus require indices

* Fix all other ABI RPM messages

* [tools] Add servo driver offset in gen_airframe

* [fix] test actuators_bebop

* [test] disco fix

---------

Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
This commit is contained in:
Christophe De Wagter
2023-09-22 14:10:33 +02:00
committed by GitHub
parent 640d94102d
commit 5672d5fcd6
14 changed files with 69 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+1 -1
View File
@@ -84,7 +84,7 @@
</message>
<message name="RPM" id="15">
<field name="rpm" type="uint16_t *" unit="rpm"/>
<field name="rpm" type="struct rpm_act_t *" unit="rpm"/>
<field name="num_act" type="uint8_t"/>
</message>
conf/modules/actuators_bebop.xml
+1
View File
@@ -23,6 +23,7 @@
<configure name="SRC_BOARD" value="boards/bebop"/>
<define name="BEBOP_ACTUATORS_I2C_DEV" value="i2c1"/>
<define name="USE_I2C1"/>
<define name="SERVOS_DEFAULT_OFFSET" value="0"/>
</test>
</makefile>
</module>
conf/modules/actuators_disco.xml
+1
View File
@@ -21,6 +21,7 @@
<define name="USE_I2C1"/>
<include name="arch/linux"/>
<configure name="SRC_BOARD" value="boards/disco"/>
<define name="SERVOS_DEFAULT_OFFSET" value="0"/>
</test>
</makefile>
</module>
sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
+5 -2
View File
@@ -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);
sw/airborne/boards/bebop/actuators.c
+10 -1
View File
@@ -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)
sw/airborne/boards/disco/actuators.c
+8 -1
View File
@@ -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)
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+3 -3
View File
@@ -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);
}
sw/airborne/modules/actuators/actuators.h
+6
View File
@@ -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;
sw/airborne/modules/actuators/actuators_uavcan.c
+22
View File
@@ -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
}
sw/airborne/modules/core/abi_common.h
+1
View File
@@ -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
*/
sw/airborne/modules/helicopter/throttle_curve.c
+3 -3
View File
@@ -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;
}
sw/airborne/modules/ins/ins_flow.c
+3 -3
View File
@@ -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;
}
}
sw/airborne/modules/sensors/rpm_sensor.c
+3 -2
View File
@@ -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 */
sw/tools/generators/gen_airframe.ml
+2
View File
@@ -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;