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

OP-288 Slight simplification of the ManualControl module

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2548 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
FredericG 2011-01-23 18:38:00 +00:00 committed by FredericG
parent fadcbbbe6c
commit f7e6f998ca
1 changed files with 14 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
flight/Modules/ManualControl/manualcontrol.c
View File

@ -159,6 +159,8 @@ static void manualControlTask(void *parameters)
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
while (1) { while (1) {
float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
// Wait until next update // Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
@ -180,6 +182,7 @@ static void manualControlTask(void *parameters)
} }
if (!ManualControlCommandReadOnly(&cmd)) { if (!ManualControlCommandReadOnly(&cmd)) {
// Check settings, if error raise alarm // Check settings, if error raise alarm
if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
@ -204,46 +207,31 @@ static void manualControlTask(void *parameters)
#elif defined(PIOS_INCLUDE_SPEKTRUM) #elif defined(PIOS_INCLUDE_SPEKTRUM)
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
#endif #endif
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
} }
// Calculate roll command in range +1 to -1 // Scale channels to -1 -> +1 range
cmd.Roll = scaleChannel(cmd.Channel[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Roll = scaledChannel[settings.Roll];
settings.ChannelMin[settings.Roll], settings.ChannelNeutral[settings.Roll]); cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
// Calculate pitch command in range +1 to -1 cmd.Throttle = scaledChannel[settings.Throttle];
cmd.Pitch = scaleChannel(cmd.Channel[settings.Pitch], settings.ChannelMax[settings.Pitch], flightMode = scaledChannel[settings.FlightMode];
settings.ChannelMin[settings.Pitch], settings.ChannelNeutral[settings.Pitch]);
// Calculate yaw command in range +1 to -1
cmd.Yaw = scaleChannel(cmd.Channel[settings.Yaw], settings.ChannelMax[settings.Yaw],
settings.ChannelMin[settings.Yaw], settings.ChannelNeutral[settings.Yaw]);
// Calculate throttle command in range +1 to -1
cmd.Throttle = scaleChannel(cmd.Channel[settings.Throttle], settings.ChannelMax[settings.Throttle],
settings.ChannelMin[settings.Throttle], settings.ChannelNeutral[settings.Throttle]);
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
cmd.Accessory1 = scaleChannel(cmd.Channel[settings.Accessory1], settings.ChannelMax[settings.Accessory1], cmd.Accessory1 = scaledChannel[settings.Accessory1];
settings.ChannelMin[settings.Accessory1], settings.ChannelNeutral[settings.Accessory1]);
else else
cmd.Accessory1 = 0; cmd.Accessory1 = 0;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
cmd.Accessory2 = scaleChannel(cmd.Channel[settings.Accessory2], settings.ChannelMax[settings.Accessory2], cmd.Accessory2 = scaledChannel[settings.Accessory2];
settings.ChannelMin[settings.Accessory2], settings.ChannelNeutral[settings.Accessory2]);
else else
cmd.Accessory2 = 0; cmd.Accessory2 = 0;
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
cmd.Accessory3 = scaleChannel(cmd.Channel[settings.Accessory3], settings.ChannelMax[settings.Accessory3], cmd.Accessory3 = scaledChannel[settings.Accessory3];
settings.ChannelMin[settings.Accessory3], settings.ChannelNeutral[settings.Accessory3]);
else else
cmd.Accessory3 = 0; cmd.Accessory3 = 0;
// Update flight mode
flightMode = scaleChannel(cmd.Channel[settings.FlightMode], settings.ChannelMax[settings.FlightMode],
settings.ChannelMin[settings.FlightMode], settings.ChannelNeutral[settings.FlightMode]);
if (flightMode < -FLIGHT_MODE_LIMIT) { if (flightMode < -FLIGHT_MODE_LIMIT) {
// Position 1 // Position 1
for(int i = 0; i < 3; i++) { for(int i = 0; i < 3; i++) {
@ -327,8 +315,7 @@ static void manualControlTask(void *parameters)
if (connection_state == CONNECTED) { if (connection_state == CONNECTED) {
// Should use RC input only if RX is connected // Should use RC input only if RX is connected
armStickLevel = scaleChannel(cmd.Channel[channel], settings.ChannelMax[channel], armStickLevel = scaledChannel[channel];
settings.ChannelMin[channel], settings.ChannelNeutral[channel]);
if (reverse) if (reverse)
armStickLevel =-armStickLevel; armStickLevel =-armStickLevel;