LP-544 Add PIOS_Servo_SetActive() API to communicate active servo channels from Actuator module to pios_servo driver. pios_servo is not allowed to touch inactive outputs.

Author: mindnever

flight/modules/Actuator/actuator.c

@@ -1062,6 +1062,25 @@ static void actuator_update_rate_if_changed(bool force_update)
     }
 }
 
+static void update_servo_active()
+{
+    /* For each mixer output that is not disabled,
+     * figure out servo address and send allocation map to pios_servo driver.
+     * We need to execute this when either ActuatorSettings or MixerSettings change.
+     */
+    uint32_t servo_active = 0;
+
+    Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
+
+    for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
+        if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
+            servo_active |= 1 << actuatorSettings.ChannelAddr[ct];
+        }
+    }
+
+    PIOS_Servo_SetActive(servo_active);
+}
+
 static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     ActuatorSettingsGet(&actuatorSettings);
@@ -1070,6 +1089,8 @@ static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
         spinWhileArmed = false;
     }
     actuator_update_rate_if_changed(false);
+
+    update_servo_active();
 }
 
 static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
@@ -1082,6 +1103,8 @@ static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
             mixer_settings_count++;
         }
     }
+
+    update_servo_active();
 }
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {

flight/pios/common/pios_servo.c

@@ -72,7 +72,8 @@ static uint32_t pios_dshot_t0h_raw;
 static uint32_t pios_dshot_t1h_raw;
 static uint32_t pios_dshot_t_raw;
 
-static bool pios_servo_enabled = true;
+static bool pios_servo_enabled    = true;
+static uint32_t pios_servo_active = 0; // No active outputs by default
 
 #define PIOS_SERVO_TIMER_CLOCK 1000000
 #define PIOS_SERVO_SAFE_MARGIN 50
@@ -83,6 +84,20 @@ static bool pios_servo_enabled = true;
 #define DSHOT_T1H_DIV          1333
 #define DSHOT_NUM_BITS         16
 
+extern void PIOS_Servo_SetActive(uint32_t active)
+{
+    bool enabled = pios_servo_enabled;
+
+    if (enabled) {
+        PIOS_Servo_Disable();
+    }
+
+    pios_servo_active = active;
+
+    if (enabled) {
+        PIOS_Servo_Enable();
+    }
+}
 
 extern void PIOS_Servo_Disable()
 {
@@ -142,6 +157,10 @@ static void PIOS_Servo_SetupBank(uint8_t bank_nr)
             continue;
         }
 
+        if (!(pios_servo_active & (1L << i))) { // This output is not active
+            continue;
+        }
+
         GPIO_InitTypeDef init = chan->pin.init;
 
         switch (bank->mode) {
@@ -345,6 +364,10 @@ static void PIOS_Servo_DShot_Update()
             continue;
         }
 
+        if (!(pios_servo_active & (1L << i))) { // This output is not active
+            continue;
+        }
+
         has_dshot = true;
 
         uint16_t payload = pin->value;
@@ -451,7 +474,8 @@ void PIOS_Servo_Update()
     }
 
     for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
-        if (pios_servo_pins[i].bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
+        if ((pios_servo_active & (1L << i))
+            && (pios_servo_pins[i].bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE)) {
             /* Update the position */
             const struct pios_tim_channel *chan = &servo_cfg->channels[i];
 

flight/pios/inc/pios_servo.h

@@ -41,6 +41,7 @@ enum pios_servo_bank_mode {
 /* Public Functions */
 extern void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks);
 extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position);
+extern void PIOS_Servo_SetActive(uint32_t Active);
 extern void PIOS_Servo_Update();
 extern void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode);
 extern void PIOS_Servo_DSHot_Rate(uint32_t rate_in_khz);

flight/targets/boards/oplinkmini/firmware/pios_board.c

@@ -336,9 +336,10 @@ void PIOS_Board_Init(void)
         PIOS_Servo_Init(&pios_servo_flexi_cfg);
     }
 
-    // Set bank modes
+    // Set bank modes and activate output. We need to do this here because oplm does not run Actuator module.
     PIOS_Servo_SetBankMode(0, PIOS_SERVO_BANK_MODE_PWM);
     PIOS_Servo_SetBankMode(1, PIOS_SERVO_BANK_MODE_PWM);
+    PIOS_Servo_SetActive(0b11);
 #endif
 
     PIOS_BOARD_IO_Configure_RFM22B();