rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-19 09:54:15 +01:00
Code Issues Releases Activity

CC-14 PIOS/Servo: Set more than two bank speeds

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2658 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-01 02:17:55 +00:00 committed by peabody124
parent 200f9a7247
commit c9207b3754
4 changed files with 38 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
flight/Modules/Actuator/actuator.c
View File

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

36
flight/PiOS/STM32F10x/pios_servo.c
View File

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

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

@ -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 */

2
shared/uavobjectdefinition/actuatorsettings.xml
View File

@ -15,7 +15,7 @@
<field name="VTOLMotorSW" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/>
<field name="VTOLMotorW" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/>
<field name="VTOLMotorNW" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/>
<field name="ChannelUpdateFreq" units="Hz" type="int16" elements="2" defaultvalue="50"/>
<field name="ChannelUpdateFreq" units="Hz" type="uint16" elements="4" defaultvalue="50"/>
<field name="ChannelMax" units="us" type="int16" elements="8" defaultvalue="1000"/>
<field name="ChannelNeutral" units="us" type="int16" elements="8" defaultvalue="1000"/>
<field name="ChannelMin" units="us" type="int16" elements="8" defaultvalue="1000"/>