[rpm_sensor] filter and broadcast rpm via ABI (#1830)

conf/abi.xml

@@ -84,6 +84,10 @@
       <field name="eas" type="float" unit="m/s"/>
     </message>
 
+    <message name="RPM" id="15">
+      <field name="rpm" type="uint16_t" unit="rpm"/>
+    </message>
+
   </msg_class>
 
 </protocol>

conf/modules/rpm_sensor.xml

@@ -7,12 +7,14 @@
     </description>
     <configure name="RPM_PWM_CHANNEL" value="PWM_INPUTX" description="Select PWM input channel for RPM sensor"/>
     <define name="RPM_PULSE_PER_RND" value="14" description="Amount of pulses per round"/>
+    <define name="RPM_FILTER_TAU" value="0.3" description="1/cut-off-frequency = filter time"/>
   </doc>
   <depends>pwm_meas</depends>
   <header>
     <file name="rpm_sensor.h"/>
   </header>
   <init fun="rpm_sensor_init()"/>
+  <periodic fun="rpm_sensor_periodic()" autorun="TRUE"/>
   <makefile>
     <file name="rpm_sensor.c"/>
 

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;

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

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 */