rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-193: Output mixer should read from up to 6 accessory channels (may add more

Browse Source 
in future).
This commit is contained in:
James Cotton 2011-06-23 01:47:55 -05:00
parent 493fcaef2a
commit 368d5b649a
1 changed files with 37 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -104,15 +104,15 @@ 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_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
return 0;
}
@ -158,7 +158,7 @@ static void actuatorTask(void* parameters)
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
{
PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
@ -196,7 +196,7 @@ static void actuatorTask(void* parameters)
nMixers ++;
}
}
if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers.
if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers.
{
setFailsafe(); // So that channels like PWM buzzer keep working
continue;
@ -207,7 +207,7 @@ static void actuatorTask(void* parameters)
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00;
bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
//The source for the secondary curve is selectable
float curve2 = 0;
@ -233,11 +233,11 @@ static void actuatorTask(void* parameters)
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0)
curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2);
else
curve2 = 0;
else
curve2 = 0;
break;
}
for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
{
if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
@ -246,41 +246,46 @@ static void actuatorTask(void* parameters)
command.Channel[ct] = 0;
continue;
}
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO))
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
else
status[ct] = -1;
// Motors have additional protection for when to be on
if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
// If not armed or motors aren't meant to spin all the time
if( !armed ||
(!spinWhileArmed && !positiveThrottle))
{
filterAccumulator[ct] = 0;
lastResult[ct] = 0;
lastResult[ct] = 0;
status[ct] = -1; //force min throttle
}
// If armed meant to keep spinning,
}
// If armed meant to keep spinning,
else if ((spinWhileArmed && !positiveThrottle) ||
(status[ct] < 0) )
status[ct] = 0;
status[ct] = 0;
}
// If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped
// directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
// these also will not be updated in failsafe mode. I'm not sure what
// these also will not be updated in failsafe mode. I'm not sure what
// the correct behavior is since it seems domain specific. I don't love
// this code
if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY2))
if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5))
{
if(AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0,&accessory) == 0)
status[ct] = accessory.AccessoryVal;
else
status[ct] = -1;
}
command.Channel[ct] = scaleChannel(status[ct],
ChannelMax[ct],
ChannelMin[ct],
@ -294,7 +299,7 @@ static void actuatorTask(void* parameters)
command.UpdateTime = 1000*dT;
if(1000*dT > command.MaxUpdateTime)
command.MaxUpdateTime = 1000*dT;
// Update output object
ActuatorCommandSet(&command);
// Update in case read only (eg. during servo configuration)
@ -302,7 +307,7 @@ static void actuatorTask(void* parameters)
// Update servo outputs
bool success = true;
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
{
success &= set_channel(n, command.Channel[n]);
@ -311,7 +316,7 @@ static void actuatorTask(void* parameters)
if(!success) {
command.NumFailedUpdates++;
ActuatorCommandSet(&command);
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
}
}
@ -465,7 +470,7 @@ static void setFailsafe()
// Reset ActuatorCommand to safe values
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
{
if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR)
{
Channel[n] = ChannelMin[n];
@ -512,10 +517,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
}
#else
static bool set_channel(uint8_t mixer_channel, uint16_t value) {
ActuatorSettingsData settings;
ActuatorSettingsGet(&settings);
switch(settings.ChannelType[mixer_channel]) {
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: {
// This is for buzzers that take a PWM input
@ -567,7 +572,7 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
PIOS_Servo_Set(settings.ChannelAddr[mixer_channel], value);
return true;
#if defined(PIOS_INCLUDE_I2C_ESC)
case ACTUATORSETTINGS_CHANNELTYPE_MK:
case ACTUATORSETTINGS_CHANNELTYPE_MK:
return PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value);
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
return PIOS_SetAstec4Speed(settings.ChannelAddr[mixer_channel],value);
@ -575,10 +580,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
#endif
default:
return false;
}
}
return false;
}
#endif