From 2f6f76d57851483c33b412e46a7536a8b4b2e891 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Wed, 29 Sep 2010 18:51:34 +0000 Subject: [PATCH] Flight/Actuator: Remove old mixer code git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1799 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/OpenPilot/Modules/Actuator/actuator.c | 638 ------------------ .../OpenPilot/Modules/Actuator/inc/actuator.h | 42 -- 2 files changed, 680 deletions(-) delete mode 100644 flight/OpenPilot/Modules/Actuator/actuator.c delete mode 100644 flight/OpenPilot/Modules/Actuator/inc/actuator.h diff --git a/flight/OpenPilot/Modules/Actuator/actuator.c b/flight/OpenPilot/Modules/Actuator/actuator.c deleted file mode 100644 index 0de26e03d..000000000 --- a/flight/OpenPilot/Modules/Actuator/actuator.c +++ /dev/null @@ -1,638 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup ActuatorModule Actuator Module - * @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type. - * This is where all the mixing of channels is computed. - * @{ - * - * @file actuator.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Actuator module. Drives the actuators (servos, motors etc). - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "openpilot.h" -#include "actuator.h" -#include "actuatorsettings.h" -#include "vtolsettings.h" -#include "vtolstatus.h" -#include "systemsettings.h" -#include "actuatordesired.h" -#include "actuatorcommand.h" -#include "manualcontrolcommand.h" - -//yaw stabilisation for helis... -#include "stabilizationsettings.h" -#include "attitudeactual.h" -//yaw stabilisation for helis... - -// Private constants -#define MAX_QUEUE_SIZE 2 -#define STACK_SIZE configMINIMAL_STACK_SIZE -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define FAILSAFE_TIMEOUT_MS 100 - -// Private types - //yaw stabilisation for helis... - //currently just coppied from simple stabilisation -#define YAW_INTEGRAL_LIMIT 0.5 -AttitudeActualData attitudeActual; -StabilizationSettingsData stabSettings; -float ccpmYawError, ccpmYawErrorLast; -float ccpmYawDerivative; -float ccpmYawIntegral, ccpmYawIntegralLimit; -float desiredYaw; -static float bound(float val, float min, float max); - //yaw stabilisation for helis... - -// Private variables -static xQueueHandle queue; -static xTaskHandle taskHandle; - -// Private functions -static void actuatorTask(void *parameters); -static int32_t mixerFixedWing(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd); -static int32_t mixerFixedWingElevon(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd); -static int32_t mixerVTOL(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd); -static int32_t InterpolateCCPMCurve(float *Output, float input, const float *Curve); -static int32_t mixerCCPM(ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd); -static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); -static void setFailsafe(); - -static float bound(float val, float min, float max); - -/** - * @brief Module initialization - * @return 0 - */ -int32_t ActuatorInitialize() -{ - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - - // Listen for ExampleObject1 updates - ActuatorDesiredConnectQueue(queue); - - ccpmYawErrorLast = 0.0; - - // Start main task - xTaskCreate(actuatorTask, (signed char *)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); - - return 0; -} - -/** - * @brief Main module task - */ -static void actuatorTask(void *parameters) -{ -// UAVObjEvent ev; - ActuatorSettingsData settings; - SystemSettingsData sysSettings; - ActuatorDesiredData desired; - ActuatorCommandData cmd; - portTickType lastSysTime; - - // Set servo update frequency (done only on start-up) - ActuatorSettingsGet(&settings); - PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]); - - // Go to the neutral (failsafe) values until an ActuatorDesired update is received - setFailsafe(); - - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) { - // Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe - /*if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) - { - setFailsafe(); - continue; - } */ - - // Read settings - ActuatorSettingsGet(&settings); - SystemSettingsGet(&sysSettings); - - // Reset ActuatorCommand to neutral values - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - cmd.Channel[n] = settings.ChannelNeutral[n]; - } - - // Read input object - ActuatorDesiredGet(&desired); - - // Call appropriate mixer depending on the airframe configuration - if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) { - if (mixerFixedWing(&settings, &desired, &cmd) == -1) { - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - } - } else if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) { - if (mixerFixedWingElevon(&settings, &desired, &cmd) == -1) { - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - } - } else if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) { - if (mixerVTOL(&settings, &desired, &cmd) == -1) { - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - } - } else if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) { - if (mixerCCPM(&settings, &desired, &cmd) == -1) { - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - } - } - // Update output object - ActuatorCommandSet(&cmd); - // Update in case read only (eg. during servo configuration) - ActuatorCommandGet(&cmd); - - // Update servo outputs - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - PIOS_Servo_Set(n, cmd.Channel[n]); - } - - // Wait until next update - vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS); - - } -} - -/** - * Mixer for Fixed Wing airframes. Converts desired roll,pitch,yaw and throttle to servo outputs. - * @return -1 if error, 0 if success - */ -static int32_t mixerFixedWing(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd) -{ - - // Check settings - if (settings->FixedWingPitch1 == ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE || - settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE || - settings->FixedWingThrottle == ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE) { - return -1; - } - - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - - // Set pitch servo command - cmd->Channel[settings->FixedWingPitch1] = scaleChannel(desired->Pitch, settings->ChannelMax[settings->FixedWingPitch1], - settings->ChannelMin[settings->FixedWingPitch1], - settings->ChannelNeutral[settings->FixedWingPitch1]); - if (settings->FixedWingPitch2 != ACTUATORSETTINGS_FIXEDWINGPITCH2_NONE) { - cmd->Channel[settings->FixedWingPitch2] = scaleChannel(desired->Pitch, settings->ChannelMax[settings->FixedWingPitch2], - settings->ChannelMin[settings->FixedWingPitch2], - settings->ChannelNeutral[settings->FixedWingPitch2]); - } - // Set roll servo command - cmd->Channel[settings->FixedWingRoll1] = scaleChannel(desired->Roll, settings->ChannelMax[settings->FixedWingRoll1], - settings->ChannelMin[settings->FixedWingRoll1], - settings->ChannelNeutral[settings->FixedWingRoll1]); - if (settings->FixedWingRoll2 != ACTUATORSETTINGS_FIXEDWINGROLL2_NONE) { - cmd->Channel[settings->FixedWingRoll2] = scaleChannel(desired->Roll, settings->ChannelMax[settings->FixedWingRoll2], - settings->ChannelMin[settings->FixedWingRoll2], - settings->ChannelNeutral[settings->FixedWingRoll2]); - } - // Set yaw servo command - if (settings->FixedWingYaw != ACTUATORSETTINGS_FIXEDWINGYAW_NONE) { - cmd->Channel[settings->FixedWingYaw] = scaleChannel(desired->Yaw, settings->ChannelMax[settings->FixedWingYaw], - settings->ChannelMin[settings->FixedWingYaw], - settings->ChannelNeutral[settings->FixedWingYaw]); - } - // Set throttle servo command - if (manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) - cmd->Channel[settings->FixedWingThrottle] = scaleChannel(desired->Throttle, settings->ChannelMax[settings->FixedWingThrottle], - settings->ChannelMin[settings->FixedWingThrottle], - settings->ChannelNeutral[settings->FixedWingThrottle]); - else - cmd->Channel[settings->FixedWingThrottle] = -1; - - // Done - return 0; -} - -/** - * Mixer for Fixed Wing airframes with elevons. Converts desired roll,pitch,yaw and throttle to servo outputs. - * @return -1 if error, 0 if success - */ -static int32_t mixerFixedWingElevon(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd) -{ - // Check settings - if (settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE || - settings->FixedWingRoll2 == ACTUATORSETTINGS_FIXEDWINGROLL2_NONE || - settings->FixedWingThrottle == ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE) { - return -1; - } - - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - - // Set first elevon servo command - cmd->Channel[settings->FixedWingRoll1] = scaleChannel(desired->Pitch + desired->Roll, settings->ChannelMax[settings->FixedWingRoll1], - settings->ChannelMin[settings->FixedWingRoll1], - settings->ChannelNeutral[settings->FixedWingRoll1]); - - // Set second elevon servo command - cmd->Channel[settings->FixedWingRoll2] = scaleChannel(desired->Pitch - desired->Roll, settings->ChannelMax[settings->FixedWingRoll2], - settings->ChannelMin[settings->FixedWingRoll2], - settings->ChannelNeutral[settings->FixedWingRoll2]); - - // Set throttle servo command - if (manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) - cmd->Channel[settings->FixedWingThrottle] = scaleChannel(desired->Throttle, settings->ChannelMax[settings->FixedWingThrottle], - settings->ChannelMin[settings->FixedWingThrottle], - settings->ChannelNeutral[settings->FixedWingThrottle]); - else - cmd->Channel[settings->FixedWingThrottle] = -1; - - // Done - return 0; -} - -/** - * Mixer for VTOL (quads and octo copters). Converts desired roll,pitch,yaw and throttle to servo outputs. - * - * I will probably add settings to change the weighting for each control to each motor. This will allow more - * flexible support for various motor topologies. For now hard coding in simple versions and lettings the - * scaling capabilities fix the subtlties. - * - * Also, because of how the Throttle ranges from 0 to 1, the motors should too! - * - * @return -1 if error, 0 if success - */ -static int32_t mixerVTOL(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd) -{ - VTOLSettingsData vtolSettings; - VTOLStatusData vtolStatus; - VTOLSettingsGet(&vtolSettings); - VTOLStatusGet(&vtolStatus); - - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - - const int vtolMin = -1; - int vtolMax = -1; - - if ((manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) && (desired->Throttle >= 0)) - vtolMax = 1; - else - vtolMax = -1; - - if (((settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) + - (settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORS_NONE) + - (settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) + - (settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) + - (settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) + - (settings->VTOLMotorSW != ACTUATORSETTINGS_VTOLMOTORSW_NONE) + - (settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) + (settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE)) <= 2) { - return -1; // can't fly with 2 or less engines (i believe) - } - - if (settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) { - vtolStatus.MotorN = desired->Throttle * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_THROTTLE] + - desired->Pitch * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_PITCH] + - desired->Roll * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_ROLL] + desired->Yaw * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_YAW]; - cmd->Channel[settings->VTOLMotorN] = scaleChannel(bound(vtolStatus.MotorN, vtolMin, vtolMax), - settings->ChannelMax[settings->VTOLMotorN], - settings->ChannelMin[settings->VTOLMotorN], - settings->ChannelNeutral[settings->VTOLMotorN]); - } - if (settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORNE_NONE) { - vtolStatus.MotorNE = desired->Throttle * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_THROTTLE] + - desired->Pitch * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_PITCH] + - desired->Roll * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_ROLL] + desired->Yaw * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_YAW]; - cmd->Channel[settings->VTOLMotorNE] = scaleChannel(bound(vtolStatus.MotorNE, vtolMin, vtolMax), - settings->ChannelMax[settings->VTOLMotorNE], - settings->ChannelMin[settings->VTOLMotorNE], - settings->ChannelNeutral[settings->VTOLMotorNE]); - } - if (settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) { - vtolStatus.MotorE = desired->Throttle * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_THROTTLE] + - desired->Pitch * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_PITCH] + - desired->Roll * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_ROLL] + desired->Yaw * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_YAW]; - cmd->Channel[settings->VTOLMotorE] = scaleChannel(bound(vtolStatus.MotorE, vtolMin, vtolMax), - settings->ChannelMax[settings->VTOLMotorE], - settings->ChannelMin[settings->VTOLMotorE], - settings->ChannelNeutral[settings->VTOLMotorE]); - } - if (settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) { - vtolStatus.MotorSE = desired->Throttle * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_THROTTLE] + - desired->Pitch * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_PITCH] + - desired->Roll * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_ROLL] + desired->Yaw * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_YAW]; - cmd->Channel[settings->VTOLMotorSE] = scaleChannel(bound(vtolStatus.MotorSE, vtolMin, vtolMax), - settings->ChannelMax[settings->VTOLMotorSE], - settings->ChannelMin[settings->VTOLMotorSE], - settings->ChannelNeutral[settings->VTOLMotorSE]); - } - if (settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) { - vtolStatus.MotorS = desired->Throttle * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_THROTTLE] + - desired->Pitch * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_PITCH] + - desired->Roll * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_ROLL] + desired->Yaw * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_YAW]; - cmd->Channel[settings->VTOLMotorS] = scaleChannel(bound(vtolStatus.MotorS, vtolMin, vtolMax), - settings->ChannelMax[settings->VTOLMotorS], - settings->ChannelMin[settings->VTOLMotorS], - settings->ChannelNeutral[settings->VTOLMotorS]); - } - if (settings->VTOLMotorSW != ACTUATORSETTINGS_VTOLMOTORSW_NONE) { - vtolStatus.MotorSW = bound(desired->Throttle * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_THROTTLE] + - desired->Pitch * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_PITCH] + - desired->Roll * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_ROLL] + - desired->Yaw * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_YAW], vtolMin, vtolMax); - cmd->Channel[settings->VTOLMotorSW] = scaleChannel(vtolStatus.MotorSW, - settings->ChannelMax[settings->VTOLMotorSW], - settings->ChannelMin[settings->VTOLMotorSW], - settings->ChannelNeutral[settings->VTOLMotorSW]); - } - if (settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) { - vtolStatus.MotorW = desired->Throttle * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_THROTTLE] + - desired->Pitch * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_PITCH] + - desired->Roll * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_ROLL] + desired->Yaw * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_YAW]; - cmd->Channel[settings->VTOLMotorW] = scaleChannel(bound(vtolStatus.MotorW, vtolMin, vtolMax), - settings->ChannelMax[settings->VTOLMotorW], - settings->ChannelMin[settings->VTOLMotorW], - settings->ChannelNeutral[settings->VTOLMotorW]); - } - if (settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE) { - vtolStatus.MotorNW = desired->Throttle * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_THROTTLE] + - desired->Pitch * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_PITCH] + - desired->Roll * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_ROLL] + desired->Yaw * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_YAW]; - cmd->Channel[settings->VTOLMotorNW] = scaleChannel(bound(vtolStatus.MotorNW, vtolMin, vtolMax), - settings->ChannelMax[settings->VTOLMotorNW], - settings->ChannelMin[settings->VTOLMotorNW], - settings->ChannelNeutral[settings->VTOLMotorNW]); - } - VTOLStatusSet(&vtolStatus); - - return 0; -} - -static int32_t InterpolateCCPMCurve(float *output, float input, const float *curve) -{ - int8_t curvIndex; - float slope; - float offset; - float value; - - //determine which section of the 5 point curve we are on - if (input <= .25) { - curvIndex = 0; - offset = 0; - } else if (input <= .50) { - curvIndex = 1; - offset = 0.25; - } else if (input <= .75) { - curvIndex = 2; - offset = 0.50; - } else { - curvIndex = 3; - offset = 0.75; - } - - //calculate the linear interpolation for the selected segment - slope = (curve[curvIndex + 1] - curve[curvIndex]) / 0.25; - value = curve[curvIndex] + ((input - offset) * slope); - - //bound the output to valid percentage values - if (value > 100.0) - value = 100.0; - if (value < 0.0) - value = 0.0; - - *output = value; - - return 0; - -} - -/** - * Mixer for CCPM (single rotor helicopters). Converts desired roll,pitch,yaw and throttle to servo outputs. - * @return -1 if error, 0 if success - */ -static int32_t mixerCCPM(ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd) -{ -#define degtorad 0.0174532925 //pi/180 - // TODO: Implement CCPM mixers - int configOK; - float curveValue; - float throttle; - float bladePitch; - float CCPMCyclicConstant; - float servoW; - float servoX; - float servoY; - float servoZ; - - configOK = 0; - - //calculate the throttle value based on the curve - scale to 0.0 -> +1.0 - InterpolateCCPMCurve(&curveValue, desired->Throttle, settings->CCPMThrottleCurve); - throttle = curveValue / 100.0; - - //calculate the Blade Pitch value based on the curve - scale to -1.0 -> +1.0 - InterpolateCCPMCurve(&curveValue, desired->Throttle, settings->CCPMPitchCurve); - bladePitch = (curveValue / 50.0) - 1.0; - - //calculate how much Cyclic to apply to the mixing - CCPMCyclicConstant = 1 - settings->CCPMCollectiveConstant; - - // Set ServoW servo command - if (settings->CCPMServoW != ACTUATORSETTINGS_CCPMSERVOW_NONE) { - servoW = (CCPMCyclicConstant * ((sin((settings->CCPMAngleW + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll) - + (cos((settings->CCPMAngleW + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch))) - + (settings->CCPMCollectiveConstant * bladePitch); - - cmd->Channel[settings->CCPMServoW] = scaleChannel(servoW, settings->ChannelMax[settings->CCPMServoW], - settings->ChannelMin[settings->CCPMServoW], - settings->ChannelNeutral[settings->CCPMServoW]); - configOK++; - } - // Set ServoX servo command - if (settings->CCPMServoX != ACTUATORSETTINGS_CCPMSERVOX_NONE) { - servoX = (CCPMCyclicConstant * ((sin((settings->CCPMAngleX + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll) - + (cos((settings->CCPMAngleX + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch))) - + (settings->CCPMCollectiveConstant * bladePitch); - - cmd->Channel[settings->CCPMServoX] = scaleChannel(servoX, settings->ChannelMax[settings->CCPMServoX], - settings->ChannelMin[settings->CCPMServoX], - settings->ChannelNeutral[settings->CCPMServoX]); - configOK++; - } - // Set ServoY servo command - if (settings->CCPMServoY != ACTUATORSETTINGS_CCPMSERVOY_NONE) { - servoY = (CCPMCyclicConstant * ((sin((settings->CCPMAngleY + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll) - + (cos((settings->CCPMAngleY + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch))) - + (settings->CCPMCollectiveConstant * bladePitch); - - cmd->Channel[settings->CCPMServoY] = scaleChannel(servoY, settings->ChannelMax[settings->CCPMServoY], - settings->ChannelMin[settings->CCPMServoY], - settings->ChannelNeutral[settings->CCPMServoY]); - configOK++; - } - // Set ServoZ servo command - if (settings->CCPMServoZ != ACTUATORSETTINGS_CCPMSERVOZ_NONE) { - servoZ = (CCPMCyclicConstant * ((sin((settings->CCPMAngleZ + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll) - + (cos((settings->CCPMAngleZ + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch))) - + (settings->CCPMCollectiveConstant * bladePitch); - - cmd->Channel[settings->CCPMServoZ] = scaleChannel(servoZ, settings->ChannelMax[settings->CCPMServoZ], - settings->ChannelMin[settings->CCPMServoZ], - settings->ChannelNeutral[settings->CCPMServoZ]); - configOK++; - } - - // Set throttle servo command - if (settings->CCPMThrottle != ACTUATORSETTINGS_CCPMTHROTTLE_NONE) { - cmd->Channel[settings->CCPMThrottle] = scaleChannel(throttle, settings->ChannelMax[settings->CCPMThrottle], - settings->ChannelMin[settings->CCPMThrottle], - settings->ChannelNeutral[settings->CCPMThrottle]); - } else { - configOK = 0; - } - - // Set TailRotor servo command - if (settings->CCPMYawStabilizationInManualMode == ACTUATORSETTINGS_CCPMYAWSTABILIZATIONINMANUALMODE_FALSE) { //update the servo as per the desired control mode (manual or stabalized) - if (settings->CCPMTailRotor != ACTUATORSETTINGS_CCPMTAILROTOR_NONE) { - cmd->Channel[settings->CCPMTailRotor] = scaleChannel(desired->Yaw, settings->ChannelMax[settings->CCPMTailRotor], - settings->ChannelMin[settings->CCPMTailRotor], - settings->ChannelNeutral[settings->CCPMTailRotor]); - } else { - configOK = 0; - } - ccpmYawErrorLast = 0.0; - - } else { //TODO need to implement the stabilization PID loop to provide heading hold gyro functionality in manual control mode - //currently just coppied from simple stabilisation - - AttitudeActualGet(&attitudeActual); - StabilizationSettingsGet(&stabSettings); - - if (desired->Yaw < 0) { - desiredYaw = 360 + (desired->Yaw * 180.0); - } else { - desiredYaw = (desired->Yaw * 180.0); - } - - // Yaw stabilization control loop - ccpmYawError = desiredYaw - attitudeActual.Yaw; - //this should make it take the quickest path to reach the desired yaw - if (ccpmYawError > 180.0) - ccpmYawError -= 360; - if (ccpmYawError < -180.0) - ccpmYawError += 360; - ccpmYawDerivative = ccpmYawError - ccpmYawErrorLast; - ccpmYawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi; - ccpmYawIntegral = bound(ccpmYawIntegral + ccpmYawError * stabSettings.UpdatePeriod, -ccpmYawIntegralLimit, ccpmYawIntegralLimit); - desiredYaw = stabSettings.YawKp * ccpmYawError + stabSettings.YawKi * ccpmYawIntegral + stabSettings.YawKd * ccpmYawDerivative;; - desiredYaw = bound(desiredYaw, -1.0, 1.0); - ccpmYawErrorLast = ccpmYawError; - if (settings->CCPMTailRotor != ACTUATORSETTINGS_CCPMTAILROTOR_NONE) { - cmd->Channel[settings->CCPMTailRotor] = scaleChannel(desiredYaw, settings->ChannelMax[settings->CCPMTailRotor], - settings->ChannelMin[settings->CCPMTailRotor], - settings->ChannelNeutral[settings->CCPMTailRotor]); - } else { - configOK = 0; - } - } - if (configOK < 2) - return -1; - return 0; -} - -/** - * Convert channel from -1/+1 to servo pulse duration in microseconds - */ -static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral) -{ - int16_t valueScaled; - // Scale - if (value >= 0.0) { - valueScaled = (int16_t) (value * ((float)(max - neutral))) + neutral; - } else { - valueScaled = (int16_t) (value * ((float)(neutral - min))) + neutral; - } - - if (max > min) { - if (valueScaled > max) - valueScaled = max; - if (valueScaled < min) - valueScaled = min; - } else { - if (valueScaled < max) - valueScaled = max; - if (valueScaled > min) - valueScaled = min; - } - - return valueScaled; -} - -/** - * Set actuator output to the neutral values (failsafe) - */ -static void setFailsafe() -{ - ActuatorSettingsData settings; - ActuatorCommandData cmd; - - // Read settings - ActuatorSettingsGet(&settings); - - // Reset ActuatorCommand to neutral values - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - cmd.Channel[n] = settings.ChannelNeutral[n]; - } - - // Set alarm - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - - // Update servo outputs - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - PIOS_Servo_Set(n, cmd.Channel[n]); - } - - // Update output object - ActuatorCommandSet(&cmd); -} - -/** - * Bound input value between limits - */ -static float bound(float val, float min, float max) -{ - if (val < min) { - val = min; - } else if (val > max) { - val = max; - } - return val; -} - -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/Modules/Actuator/inc/actuator.h b/flight/OpenPilot/Modules/Actuator/inc/actuator.h deleted file mode 100644 index 6d640e1e4..000000000 --- a/flight/OpenPilot/Modules/Actuator/inc/actuator.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup ActuatorModule Actuator Module - * @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type. - * This is where all the mixing of channels is computed. - * @{ - * - * @file actuator.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Actuator module. Drives the actuators (servos, motors etc). - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ -#ifndef ACTUATOR_H -#define ACTUATOR_H - -int32_t ActuatorInitialize(); - -#endif // ACTUATOR_H - -/** - * @} - * @} - */