mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-18 08:54:15 +01:00
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
This commit is contained in:
parent
200f9a7247
commit
c9207b3754
@ -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;
|
||||
|
@ -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
|
||||
}
|
||||
|
@ -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 */
|
||||
|
@ -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"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user