AP_RSSI: make RSSI work on ChibiOS

libraries/AP_RSSI/AP_RSSI.cpp

@@ -191,9 +191,11 @@ float AP_RSSI::read_channel_rssi()
     return channel_rssi;    
 }
 
+
 // read the PWM value from a pin
 float AP_RSSI::read_pwm_pin_rssi()
 {
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // check if pin has changed and initialise gpio event callback
     pwm_state.gpio = get_gpio(rssi_analog_pin);
@@ -211,9 +213,34 @@ float AP_RSSI::read_pwm_pin_rssi()
             pwm_state.last_gpio = pwm_state.gpio;
         }
     }
+#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    if (pwm_state.gpio != rssi_analog_pin) {
+        pwm_state.gpio = rssi_analog_pin;
 
+        // remove old gpio event callback if present
+        if (pwm_state.last_gpio != 0) {
+            hal.gpio->attach_interrupt(pwm_state.gpio,
+                                       nullptr,
+                                       HAL_GPIO_INTERRUPT_BOTH);
+            pwm_state.last_gpio = 0;
+        }
+        // install interrupt handler on rising or falling edge of gpio
+        if (pwm_state.gpio != 0) {
+            hal.gpio->pinMode(pwm_state.gpio, HAL_GPIO_INPUT);
+            hal.gpio->attach_interrupt(pwm_state.gpio,
+                                       irq_handler_chibios,
+                                       HAL_GPIO_INTERRUPT_BOTH);
+            pwm_state.gpio = pwm_state.gpio;
+        }
+    }
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // disable interrupts temporarily
     irqstate_t istate = irqsave();
+#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    void *istate = hal.scheduler->disable_interrupts_save();
+#endif
 
     // check for timeout
     float ret;
@@ -225,8 +252,12 @@ float AP_RSSI::read_pwm_pin_rssi()
         ret = scale_and_constrain_float_rssi(pwm_state.value, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
     }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // restore interrupts
     irqrestore(istate);
+#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    hal.scheduler->restore_interrupts(istate);
+#endif
 
     return ret;
 #else
@@ -264,6 +295,7 @@ float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float lo
     return constrain_float(rssi_value_scaled, 0.0f, 1.0f);
 }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 // get gpio id from pin number
 uint32_t AP_RSSI::get_gpio(uint8_t pin_number) const
 {
@@ -289,17 +321,23 @@ uint32_t AP_RSSI::get_gpio(uint8_t pin_number) const
 // interrupt handler for reading pwm value
 int AP_RSSI::irq_handler(int irq, void *context)
 {
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
     // sanity check
     if (pwm_state.gpio == 0) {
         return 0;
     }
 
-    // capture time
-    uint64_t now = AP_HAL::micros64();
-
     // read value of pin
     bool pin_high = stm32_gpioread(pwm_state.gpio);
+    irq_handler_calc_pwm(pin_high);
+    return 0;
+}
+#endif
+
+void AP_RSSI::irq_handler_calc_pwm(bool pin_high)
+{
+    // capture time
+    const uint64_t now = AP_HAL::micros64();
 
     // calculate pwm value
     if (pin_high) {
@@ -311,11 +349,18 @@ int AP_RSSI::irq_handler(int irq, void *context)
         pwm_state.pulse_start_us = 0;
         pwm_state.last_reading_ms = AP_HAL::millis();
     }
-#endif
-
-    return 0;
 }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+// interrupt handler for reading pwm value
+void AP_RSSI::irq_handler_chibios()
+{
+    // read value of pin
+    bool pin_high = hal.gpio->read(pwm_state.gpio);
+    irq_handler_calc_pwm(pin_high);
+}
+#endif
+
 AP_RSSI *AP_RSSI::_s_instance = nullptr;
 
 namespace AP {

libraries/AP_RSSI/AP_RSSI.h

@@ -96,8 +96,16 @@ private:
     float scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range);
 
     // PWM input handling
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     uint32_t get_gpio(uint8_t pin_number) const;
     static int irq_handler(int irq, void *context);
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    static void irq_handler_chibios(void);
+#endif
+
+    static void irq_handler_calc_pwm(bool pin_high);
 };
 
 namespace AP {