rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

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.

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/modules/Actuator/actuator.c
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)
{
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)
{

28
flight/pios/common/pios_servo.c
View File

@ -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];

1
flight/pios/inc/pios_servo.h
View File

@ -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);

3
flight/targets/boards/oplinkmini/firmware/pios_board.c
View File

@ -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();