diff --git a/conf/abi.xml b/conf/abi.xml index 2159bab595..7697012757 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -84,6 +84,10 @@ + + + + diff --git a/conf/modules/rpm_sensor.xml b/conf/modules/rpm_sensor.xml index 4bcc3413d1..ffe0e2f8cb 100644 --- a/conf/modules/rpm_sensor.xml +++ b/conf/modules/rpm_sensor.xml @@ -7,12 +7,14 @@ + pwm_meas
+ diff --git a/sw/airborne/modules/sensors/rpm_sensor.c b/sw/airborne/modules/sensors/rpm_sensor.c index 676a8ca1e0..cae6a40781 100644 --- a/sw/airborne/modules/sensors/rpm_sensor.c +++ b/sw/airborne/modules/sensors/rpm_sensor.c @@ -26,14 +26,22 @@ #include "modules/sensors/rpm_sensor.h" #include "mcu_periph/pwm_input.h" #include "subsystems/electrical.h" +#include "subsystems/abi.h" +#include "filters/low_pass_filter.h" + +static struct FirstOrderLowPass rpm_lp; + +#ifndef RPM_FILTER_TAU +#define RPM_FILTER_TAU RPM_SENSOR_PERIODIC_PERIOD +#endif + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" static void rpm_sensor_send_motor(struct transport_tx *trans, struct link_device *dev) { - uint16_t rpm = rpm_sensor_get_rpm(); - + uint16_t rpm = get_first_order_low_pass(&rpm_lp); pprz_msg_send_MOTOR(trans, dev, AC_ID, &rpm, &electrical.current); } #endif @@ -41,11 +49,21 @@ static void rpm_sensor_send_motor(struct transport_tx *trans, struct link_device /* Initialize the RPM measurement by configuring the telemetry */ void rpm_sensor_init(void) { + init_first_order_low_pass(&rpm_lp, RPM_FILTER_TAU, RPM_SENSOR_PERIODIC_PERIOD, 0); + #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_MOTOR, rpm_sensor_send_motor); #endif } +/* RPM periodic */ +void rpm_sensor_periodic(void) +{ + uint16_t rpm = update_first_order_low_pass(&rpm_lp, rpm_sensor_get_rpm()); + AbiSendMsgRPM(RPM_SENSOR_ID, rpm); +} + +/* Get the RPM sensor */ uint16_t rpm_sensor_get_rpm(void) { uint16_t rpm = 0; diff --git a/sw/airborne/modules/sensors/rpm_sensor.h b/sw/airborne/modules/sensors/rpm_sensor.h index c46378d9c3..135975b41b 100644 --- a/sw/airborne/modules/sensors/rpm_sensor.h +++ b/sw/airborne/modules/sensors/rpm_sensor.h @@ -34,6 +34,7 @@ #endif extern void rpm_sensor_init(void); +extern void rpm_sensor_periodic(void); extern uint16_t rpm_sensor_get_rpm(void); #endif diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 1d9967025c..483fdee1a9 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -252,5 +252,11 @@ #define RSSI_BLUEGIGA_ID 1 #endif +/* + * IDs of RPM sensors (message 15) + */ +#ifndef RPM_SENSOR_ID +#define RPM_SENSOR_ID 1 +#endif #endif /* ABI_SENDER_IDS_H */