1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00

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.

This commit is contained in:
Vladimir Zidar 2017-10-17 13:06:05 +02:00
parent 2f1e7cd72d
commit cc29d520a9
4 changed files with 52 additions and 3 deletions

View File

@ -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) static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
ActuatorSettingsGet(&actuatorSettings); ActuatorSettingsGet(&actuatorSettings);
@ -1070,6 +1089,8 @@ static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
spinWhileArmed = false; spinWhileArmed = false;
} }
actuator_update_rate_if_changed(false); actuator_update_rate_if_changed(false);
update_servo_active();
} }
static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
@ -1082,6 +1103,8 @@ static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
mixer_settings_count++; mixer_settings_count++;
} }
} }
update_servo_active();
} }
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {

View File

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

View File

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

View File

@ -336,9 +336,10 @@ void PIOS_Board_Init(void)
PIOS_Servo_Init(&pios_servo_flexi_cfg); 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(0, PIOS_SERVO_BANK_MODE_PWM);
PIOS_Servo_SetBankMode(1, PIOS_SERVO_BANK_MODE_PWM); PIOS_Servo_SetBankMode(1, PIOS_SERVO_BANK_MODE_PWM);
PIOS_Servo_SetActive(0b11);
#endif #endif
PIOS_BOARD_IO_Configure_RFM22B(); PIOS_BOARD_IO_Configure_RFM22B();