mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
OP-1683 - Increase resolution for OneShot125 running timer at 8MHz, setup automatically rates for OneShot
This commit is contained in:
parent
26fcec09e3
commit
632051c0ac
@ -53,20 +53,22 @@ static int8_t counter;
|
||||
#endif
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 2
|
||||
#define MAX_QUEUE_SIZE 2
|
||||
|
||||
#if defined(PIOS_ACTUATOR_STACK_SIZE)
|
||||
#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE
|
||||
#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 1312
|
||||
#define STACK_SIZE_BYTES 1312
|
||||
#endif
|
||||
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
|
||||
#define FAILSAFE_TIMEOUT_MS 100
|
||||
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
|
||||
#define FAILSAFE_TIMEOUT_MS 100
|
||||
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
|
||||
|
||||
#define CAMERA_BOOT_DELAY_MS 7000
|
||||
#define CAMERA_BOOT_DELAY_MS 7000
|
||||
|
||||
#define ACTUATOR_ONESHOT125_CLOCK 8000000
|
||||
#define ACTUATOR_PWM_CLOCK 1000000
|
||||
// Private types
|
||||
|
||||
|
||||
@ -735,16 +737,8 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
|
||||
|
||||
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
|
||||
{
|
||||
uint8_t mode = pinsMode[actuatorSettings->ChannelAddr[mixer_channel]];
|
||||
switch (mode) {
|
||||
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
|
||||
// Quick way to remap of 1000-2000 range to 125-250
|
||||
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value / 8);
|
||||
break;
|
||||
default:
|
||||
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
|
||||
break;
|
||||
}
|
||||
// OneShot125 is remapped from 1000-2000 range to 125-250 due to timer running at 8MHz
|
||||
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
|
||||
}
|
||||
return true;
|
||||
|
||||
@ -790,7 +784,7 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
|
||||
sizeof(prevBankMode));
|
||||
|
||||
uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
|
||||
|
||||
uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
|
||||
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
|
||||
PIOS_Servo_SetBankMode(i,
|
||||
actuatorSettings->BankMode[i] ==
|
||||
@ -798,13 +792,22 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
|
||||
PIOS_SERVO_BANK_MODE_PWM :
|
||||
PIOS_SERVO_BANK_MODE_SINGLE_PULSE
|
||||
);
|
||||
if (actuatorSettings->BankMode[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125) {
|
||||
freq[i] = 1000000 / 255; // force a total period of 255uSec
|
||||
} else {
|
||||
freq[i] = actuatorSettings->BankUpdateFreq[i];
|
||||
switch (actuatorSettings->BankMode[i]) {
|
||||
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
|
||||
freq[i] = 1000;
|
||||
clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 8MHz timer clock
|
||||
break;
|
||||
case ACTUATORSETTINGS_BANKMODE_ONESHOT:
|
||||
freq[i] = (PIOS_SENSOR_RATE + PIOS_SENSOR_RATE / 10); // use Sensor rate + a 10% margin to prevent missing updates.
|
||||
clock[i] = ACTUATOR_PWM_CLOCK;
|
||||
break;
|
||||
default: // PWM
|
||||
freq[i] = actuatorSettings->BankUpdateFreq[i];
|
||||
clock[i] = ACTUATOR_PWM_CLOCK;
|
||||
break;
|
||||
}
|
||||
}
|
||||
PIOS_Servo_SetHz(freq, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
|
||||
PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
|
||||
|
||||
// retrieve mode from related bank
|
||||
for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
|
||||
|
@ -36,7 +36,7 @@ enum pios_servo_bank_mode {
|
||||
PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 1
|
||||
};
|
||||
/* Public Functions */
|
||||
extern void PIOS_Servo_SetHz(const uint16_t *update_rates, 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_Update();
|
||||
extern void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode);
|
||||
|
@ -43,13 +43,13 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS];
|
||||
*/
|
||||
void PIOS_Servo_Init(void)
|
||||
{}
|
||||
|
||||
/**
|
||||
* Set the servo update rate (Max 500Hz)
|
||||
* \param[in] onetofour Rate for outputs 1 to 4 (Hz)
|
||||
* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz)
|
||||
* \param[in] array of rates in Hz
|
||||
* \param[in] array of timer clocks in Hz
|
||||
* \param[in] maximum number of banks
|
||||
*/
|
||||
void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks)
|
||||
void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
|
||||
{}
|
||||
|
||||
/**
|
||||
|
@ -43,11 +43,12 @@ static const struct pios_servo_cfg *servo_cfg;
|
||||
// static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 };
|
||||
|
||||
// timer associated to each bank
|
||||
// static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
|
||||
static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
|
||||
|
||||
// index of bank used for each pin
|
||||
static uint8_t *pios_servo_pin_bank;
|
||||
|
||||
#define PIOS_SERVO_TIMER_CLOCK 1000000
|
||||
|
||||
/**
|
||||
* Initialise Servos
|
||||
@ -81,7 +82,7 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
|
||||
pios_servo_pin_bank[j] = bank;
|
||||
}
|
||||
}
|
||||
// pios_servo_bank_timer[i] = chan->timer;
|
||||
pios_servo_bank_timer[bank] = chan->timer;
|
||||
|
||||
PIOS_Assert(bank < PIOS_SERVO_BANKS);
|
||||
|
||||
@ -90,11 +91,6 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
|
||||
pios_servo_pin_bank[j] = bank;
|
||||
}
|
||||
}
|
||||
/*
|
||||
TIM_ARRPreloadConfig(chan->timer, ENABLE);
|
||||
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
|
||||
TIM_Cmd(chan->timer, DISABLE);
|
||||
*/
|
||||
bank++;
|
||||
}
|
||||
|
||||
@ -129,10 +125,12 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
|
||||
/**
|
||||
* Set the servo update rate (Max 500Hz)
|
||||
* \param[in] array of rates in Hz
|
||||
* \param[in] array of timer clocks in Hz
|
||||
* \param[in] maximum number of banks
|
||||
*/
|
||||
void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
|
||||
void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
|
||||
{
|
||||
PIOS_Assert(banks <= PIOS_SERVO_BANKS);
|
||||
if (!servo_cfg) {
|
||||
return;
|
||||
}
|
||||
@ -140,23 +138,18 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
|
||||
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
|
||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
|
||||
|
||||
uint8_t set = 0;
|
||||
for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) {
|
||||
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
|
||||
if (timer) {
|
||||
uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK;
|
||||
if (clock[i]) {
|
||||
new_clock = clock[i];
|
||||
}
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / new_clock) - 1;
|
||||
TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1);
|
||||
|
||||
for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
|
||||
bool new = true;
|
||||
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
|
||||
|
||||
/* See if any previous channels use that same timer */
|
||||
for (uint8_t j = 0; (j < i) && new; j++) {
|
||||
new &= chan->timer != servo_cfg->channels[j].timer;
|
||||
}
|
||||
|
||||
if (new) {
|
||||
TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
|
||||
TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
|
||||
set++;
|
||||
TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -193,9 +193,10 @@ void PIOS_Servo_Update()
|
||||
/**
|
||||
* Set the servo update rate (Max 500Hz)
|
||||
* \param[in] array of rates in Hz
|
||||
* \param[in] array of timer clocks in Hz
|
||||
* \param[in] maximum number of banks
|
||||
*/
|
||||
void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
|
||||
void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks)
|
||||
{
|
||||
PIOS_Assert(banks <= PIOS_SERVO_BANKS);
|
||||
if (!servo_cfg) {
|
||||
@ -206,17 +207,21 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
|
||||
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
|
||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||
|
||||
for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
|
||||
for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) {
|
||||
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
|
||||
if (timer) {
|
||||
uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK;
|
||||
if (clock[i]) {
|
||||
new_clock = clock[i];
|
||||
}
|
||||
// Choose the correct prescaler value for the APB the timer is attached
|
||||
if (timer == TIM1 || timer == TIM8 || timer == TIM9 || timer == TIM10 || timer == TIM11) {
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / PIOS_SERVO_TIMER_CLOCK) - 1;
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / new_clock) - 1;
|
||||
} else {
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / PIOS_SERVO_TIMER_CLOCK) - 1;
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / new_clock) - 1;
|
||||
}
|
||||
|
||||
TIM_TimeBaseStructure.TIM_Period = ((PIOS_SERVO_TIMER_CLOCK / speeds[i]) - 1);
|
||||
TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1);
|
||||
TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure);
|
||||
}
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user