From c9207b37547e69c84ad158c46010239b471eab1d Mon Sep 17 00:00:00 2001 From: peabody124 Date: Tue, 1 Feb 2011 02:17:55 +0000 Subject: [PATCH] CC-14 PIOS/Servo: Set more than two bank speeds git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2658 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/Modules/Actuator/actuator.c | 25 ++++++++++--- flight/PiOS/STM32F10x/pios_servo.c | 36 +++++++++---------- flight/PiOS/inc/pios_servo.h | 2 +- .../uavobjectdefinition/actuatorsettings.xml | 2 +- 4 files changed, 38 insertions(+), 27 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index e6e1f7fc8..7018101bf 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -63,6 +63,7 @@ static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; // Private functions static void actuatorTask(void* parameters); +static void actuator_update_rate(UAVObjEvent *); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); static void setFailsafe(); static float MixerCurve(const float throttle, const float* curve); @@ -90,7 +91,10 @@ int32_t ActuatorInitialize() // Listen for ExampleObject1 updates ActuatorDesiredConnectQueue(queue); - + + // If settings change, update the output rate + ActuatorSettingsConnectCallback(actuator_update_rate); + // Start main task xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle); @@ -99,9 +103,8 @@ int32_t ActuatorInitialize() return 0; } - /** - * @brieft Main Actuator module task + * @brief Main Actuator module task * * Universal matrix based mixer for VTOL, helis and fixed wing. * Converts desired roll,pitch,yaw and throttle to servo/ESC outputs. @@ -129,8 +132,7 @@ static void actuatorTask(void* parameters) ManualControlCommandData manualControl; ActuatorSettingsGet(&settings); - // Set servo update frequency (done only on start-up) - PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]); + PIOS_Servo_SetHz(&settings.ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); float * status = (float *)&mixerStatus; //access status objects as an array of floats @@ -410,6 +412,19 @@ static void setFailsafe() ActuatorCommandSet(&command); } + +/** + * @brief Update the servo update rate + */ +static void actuator_update_rate(UAVObjEvent * ev) +{ + ActuatorSettingsData settings; + if ( ev->obj == ActuatorSettingsHandle() ) { + ActuatorSettingsGet(&settings); + PIOS_Servo_SetHz(&settings.ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); + } +} + #if defined(ARCH_POSIX) || defined(ARCH_WIN32) static bool set_channel(uint8_t mixer_channel, uint16_t value) { return true; diff --git a/flight/PiOS/STM32F10x/pios_servo.c b/flight/PiOS/STM32F10x/pios_servo.c index 52dea026f..04c731a5d 100644 --- a/flight/PiOS/STM32F10x/pios_servo.c +++ b/flight/PiOS/STM32F10x/pios_servo.c @@ -127,10 +127,10 @@ 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] maximum number of banks */ -void PIOS_Servo_SetHz(uint16_t onetofour, uint16_t fivetoeight) +void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t banks) { #ifndef PIOS_ENABLE_DEBUG_PINS #if defined(PIOS_INCLUDE_SERVO) @@ -139,26 +139,22 @@ void PIOS_Servo_SetHz(uint16_t onetofour, uint16_t fivetoeight) TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; - for (uint8_t i = 0; i < pios_servo_cfg.num_channels; i++) { + uint8_t set = 0; + + for(uint8_t i = 0; (i < pios_servo_cfg.num_channels) && (set < banks); i++) { + bool new = true; struct pios_servo_channel channel = pios_servo_cfg.channels[i]; - /* Enable time base */ - switch((int32_t)channel.timer) { - case (int32_t)TIM1: - case (int32_t)TIM2: - case (int32_t)TIM3: - case (int32_t)TIM4: - TIM_TimeBaseStructure.TIM_Period = ((1000000 / onetofour) - 1); - break; - case (int32_t)TIM5: - case (int32_t)TIM6: - case (int32_t)TIM7: - case (int32_t)TIM8: - TIM_TimeBaseStructure.TIM_Period = ((1000000 / fivetoeight) - 1); - break; + /* See if any previous channels use that same timer */ + for(uint8_t j = 0; (j < i) && new; j++) + new &= channel.timer != pios_servo_cfg.channels[j].timer; + + if(new) { + TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); + TIM_TimeBaseInit(channel.timer, &TIM_TimeBaseStructure); + set++; } - TIM_TimeBaseInit(channel.timer, &TIM_TimeBaseStructure); - } + } #endif // PIOS_INCLUDE_SERVO #endif // PIOS_ENABLE_DEBUG_PINS } diff --git a/flight/PiOS/inc/pios_servo.h b/flight/PiOS/inc/pios_servo.h index a9624b692..2d621cb25 100644 --- a/flight/PiOS/inc/pios_servo.h +++ b/flight/PiOS/inc/pios_servo.h @@ -32,7 +32,7 @@ /* Public Functions */ extern void PIOS_Servo_Init(void); -extern void PIOS_Servo_SetHz(uint16_t onetofour, uint16_t fivetoeight); +extern void PIOS_Servo_SetHz(uint16_t * update_rates, uint8_t channels); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); #endif /* PIOS_SERVO_H */ diff --git a/shared/uavobjectdefinition/actuatorsettings.xml b/shared/uavobjectdefinition/actuatorsettings.xml index bdafbe834..b4756405b 100644 --- a/shared/uavobjectdefinition/actuatorsettings.xml +++ b/shared/uavobjectdefinition/actuatorsettings.xml @@ -15,7 +15,7 @@ - +