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

OP-1683 - Increase resolution for OneShot125 running timer at 8MHz, setup automatically rates for OneShot

Browse Source
This commit is contained in:
Alessio Morale 2015-01-29 21:07:21 +01:00
parent 26fcec09e3
commit 632051c0ac
5 changed files with 57 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
flight/modules/Actuator/actuator.c
View File

@ -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++) {

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

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

8
flight/pios/posix/pios_servo.c
View File

@ -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)
{}
/**

39
flight/pios/stm32f10x/pios_servo.c
View File

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

15
flight/pios/stm32f4xx/pios_servo.c
View File

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