/** ****************************************************************************** * @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 LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * 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 #include "accessorydesired.h" #include "actuator.h" #include "actuatorsettings.h" #include "systemsettings.h" #include "actuatordesired.h" #include "actuatorcommand.h" #include "flightstatus.h" #include #include "mixersettings.h" #include "mixerstatus.h" #include "cameradesired.h" #include "hwsettings.h" #include "manualcontrolcommand.h" #include "taskinfo.h" #include #include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include #endif #undef PIOS_INCLUDE_INSTRUMENTATION #ifdef PIOS_INCLUDE_INSTRUMENTATION #include static int8_t counter; // Counter 0xAC700001 total Actuator body execution time(excluding queue waits etc). #endif // Private constants #define MAX_QUEUE_SIZE 2 #if defined(PIOS_ACTUATOR_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE #else #define STACK_SIZE_BYTES 1312 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver #define FAILSAFE_TIMEOUT_MS 100 #define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM #define CAMERA_BOOT_DELAY_MS 7000 #define ACTUATOR_ONESHOT_CLOCK 12000000 #define ACTUATOR_ONESHOT125_PULSE_FACTOR 1.5f #define ACTUATOR_ONESHOT42_PULSE_FACTOR 0.5f #define ACTUATOR_MULTISHOT_PULSE_FACTOR 0.24f #define ACTUATOR_PWM_CLOCK 1000000 // Private types // Private variables static xQueueHandle queue; static xTaskHandle taskHandle; static FrameType_t frameType = FRAME_TYPE_MULTIROTOR; static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE; static bool camStabEnabled; static bool camControlEnabled; static uint8_t pinsMode[MAX_MIX_ACTUATORS]; // used to inform the actuator thread that actuator update rate is changed static ActuatorSettingsData actuatorSettings; static bool spinWhileArmed; // used to inform the actuator thread that mixer settings are changed static MixerSettingsData mixerSettings; static int mixer_settings_count = 2; // Private functions static void actuatorTask(void *parameters); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool alwaysStabilizeWhenArmed, float throttleDesired); static void setFailsafe(); static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor); static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor); static bool set_channel(uint8_t mixer_channel, uint16_t value); static void actuator_update_rate_if_changed(bool force_update); static void MixerSettingsUpdatedCb(UAVObjEvent *ev); static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev); float ProcessMixer(const int index, const float curve1, const float curve2, ActuatorDesiredData *desired, bool multirotor, bool fixedwing); // this structure is equivalent to the UAVObjects for one mixer. typedef struct { uint8_t type; int8_t matrix[5]; } __attribute__((packed)) Mixer_t; /** * @brief Module initialization * @return 0 */ int32_t ActuatorStart() { // Start main task xTaskCreate(actuatorTask, "Actuator", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); #endif SettingsUpdatedCb(NULL); MixerSettingsUpdatedCb(NULL); ActuatorSettingsUpdatedCb(NULL); return 0; } /** * @brief Module initialization * @return 0 */ int32_t ActuatorInitialize() { // Register for notification of changes to ActuatorSettings ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); // Register for notification of changes to MixerSettings MixerSettingsConnectCallback(MixerSettingsUpdatedCb); // Listen for ActuatorDesired updates (Primary input to this module) ActuatorDesiredInitialize(); queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); ActuatorDesiredConnectQueue(queue); // Register AccessoryDesired (Secondary input to this module) AccessoryDesiredInitialize(); // Check if CameraStab module is enabled HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); camStabEnabled = (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED); camControlEnabled = (optionalModules.CameraControl == HWSETTINGS_OPTIONALMODULES_ENABLED); // Primary output of this module ActuatorCommandInitialize(); #ifdef DIAG_MIXERSTATUS // UAVO only used for inspecting the internal status of the mixer during debug MixerStatusInitialize(); #endif #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); #endif SystemSettingsConnectCallback(&SettingsUpdatedCb); return 0; } MODULE_INITCALL(ActuatorInitialize, ActuatorStart); /** * @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. * * Because of how the Throttle ranges from 0 to 1, the motors should too! * * Note this code depends on the UAVObjects for the mixers being all being the same * and in sequence. If you change the object definition, make sure you check the code! * * @return -1 if error, 0 if success */ static void actuatorTask(__attribute__((unused)) void *parameters) { UAVObjEvent ev; portTickType lastSysTime; portTickType thisSysTime; uint32_t dTMilliseconds; ActuatorCommandData command; ActuatorDesiredData desired; MixerStatusData mixerStatus; FlightModeSettingsData settings; FlightStatusData flightStatus; float throttleDesired; float collectiveDesired; #ifdef PIOS_INCLUDE_INSTRUMENTATION counter = PIOS_Instrumentation_CreateCounter(0xAC700001); #endif /* Read initial values of ActuatorSettings */ ActuatorSettingsGet(&actuatorSettings); /* Read initial values of MixerSettings */ MixerSettingsGet(&mixerSettings); /* Force an initial configuration of the actuator update rates */ actuator_update_rate_if_changed(true); // Go to the neutral (failsafe) values until an ActuatorDesired update is received setFailsafe(); // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); #endif // Wait until the ActuatorDesired object is updated uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS); #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeStart(counter); #endif if (rc != pdTRUE) { /* Update of ActuatorDesired timed out. Go to failsafe */ setFailsafe(); continue; } // Check how long since last update thisSysTime = xTaskGetTickCount(); dTMilliseconds = (thisSysTime == lastSysTime) ? 1 : (thisSysTime - lastSysTime) * portTICK_RATE_MS; lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); FlightModeSettingsGet(&settings); ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); // read in throttle and collective -demultiplex thrust switch (thrustType) { case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: throttleDesired = desired.Thrust; ManualControlCommandCollectiveGet(&collectiveDesired); break; case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: ManualControlCommandThrottleGet(&throttleDesired); collectiveDesired = desired.Thrust; break; default: ManualControlCommandThrottleGet(&throttleDesired); ManualControlCommandCollectiveGet(&collectiveDesired); } bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool activeThrottle = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors bool positiveThrottle = (throttleDesired > 0.00f); bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor. bool fixedwing = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING); // check if frame is a fixedwing. bool alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED; bool alwaysStabilizeWhenArmed = flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE; if (alwaysArmed) { alwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous. } // safety settings if (!armed) { throttleDesired = 0.00f; // this also happens in scaleMotors as a per axis check } if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) { // throttleDesired should never be 0 or go below 0. // force set all other controls to zero if throttle is cut (previously set in Stabilization) // todo: can probably remove this if (!(multirotor && alwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { desired.Roll = 0.00f; } if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { desired.Pitch = 0.00f; } if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { desired.Yaw = 0.00f; } } } #ifdef DIAG_MIXERSTATUS MixerStatusGet(&mixerStatus); #endif if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers. setFailsafe(); continue; } AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); float curve1 = 0.0f; // curve 1 is the throttle curve applied to all motors. float curve2 = 0.0f; // Interpolate curve 1 from throttleDesired as input. // assume reversible motor/mixer initially. We can later reverse this. The difference is simply that -ve throttleDesired values // map differently curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM, multirotor); // The source for the secondary curve is selectable AccessoryDesiredData accessory; uint8_t curve2Source = mixerSettings.Curve2Source; switch (curve2Source) { case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: // assume reversible motor/mixer initially curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); break; case MIXERSETTINGS_CURVE2SOURCE_ROLL: // Throttle curve contribution the same for +ve vs -ve roll if (multirotor) { curve2 = MixerCurveFullRangeProportional(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } else { curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } break; case MIXERSETTINGS_CURVE2SOURCE_PITCH: // Throttle curve contribution the same for +ve vs -ve pitch if (multirotor) { curve2 = MixerCurveFullRangeProportional(desired.Pitch, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } else { curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } break; case MIXERSETTINGS_CURVE2SOURCE_YAW: // Throttle curve contribution the same for +ve vs -ve yaw if (multirotor) { curve2 = MixerCurveFullRangeProportional(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } else { curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } break; case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: // assume reversible motor/mixer initially curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); break; case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY3: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) { // Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want. curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } else { curve2 = 0.0f; } break; default: curve2 = 0.0f; break; } float *status = (float *)&mixerStatus; // access status objects as an array of floats Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; float maxMotor = -1.0f; // highest motor value. Addition method needs this to be -1.0f, division method needs this to be 1.0f float minMotor = 1.0f; // lowest motor value Addition method needs this to be 1.0f, division method needs this to be -1.0f for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { // During boot all camera actuators should be completely disabled (PWM pulse = 0). // command.Channel[i] is reused below as a channel PWM activity flag: // 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later. // Setting it to 1 by default means "Rescale this channel and enable PWM on its output". command.Channel[ct] = 1; uint8_t mixer_type = mixers[ct].type; if (mixer_type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us status[ct] = -1; continue; } if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) { float nonreversible_curve1 = curve1; float nonreversible_curve2 = curve2; if (nonreversible_curve1 < 0.0f) { nonreversible_curve1 = 0.0f; } if (nonreversible_curve2 < 0.0f) { if (!multirotor) { // allow negative throttle if multirotor. function scaleMotors handles the sanity checks. nonreversible_curve2 = 0.0f; } } status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, multirotor, fixedwing); // If not armed or motors aren't meant to spin all the time if (!armed || (!spinWhileArmed && !positiveThrottle)) { status[ct] = -1; // force min throttle } // If armed meant to keep spinning, else if ((spinWhileArmed && !positiveThrottle) || (status[ct] < 0)) { if (!multirotor) { status[ct] = 0; // allow throttle values lower than 0 if multirotor. // Values will be scaled to 0 if they need to be in the scaleMotor function } } } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { status[ct] = ProcessMixer(ct, curve1, curve2, &desired, multirotor, fixedwing); // Reversable Motors are like Motors but go to neutral instead of minimum // If not armed or motor is inactive - no "spinwhilearmed" for this engine type if (!armed || !activeThrottle) { status[ct] = 0; // force neutral throttle } } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) { status[ct] = ProcessMixer(ct, curve1, curve2, &desired, multirotor, fixedwing); } else { status[ct] = -1; // 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 // the correct behavior is since it seems domain specific. I don't love // this code if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && (mixer_type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) { if (AccessoryDesiredInstGet(mixer_type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) { status[ct] = accessory.AccessoryVal; } else { status[ct] = -1; } } if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && (mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) { if (camStabEnabled) { CameraDesiredData cameraDesired; CameraDesiredGet(&cameraDesired); switch (mixer_type) { case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: status[ct] = cameraDesired.RollOrServo1; break; case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: status[ct] = cameraDesired.PitchOrServo2; break; case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: status[ct] = cameraDesired.Yaw; break; default: break; } } else { status[ct] = -1; } // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) { command.Channel[ct] = 0; } } if (mixer_type == MIXERSETTINGS_MIXER1TYPE_CAMERATRIGGER) { if (camControlEnabled) { CameraDesiredTriggerGet(&status[ct]); } else { status[ct] = 0; } } } // If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value. // For use in function scaleMotor if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { if (maxMotor < status[ct]) { maxMotor = status[ct]; } if (minMotor > status[ct]) { minMotor = status[ct]; } } } // Set real actuator output values scaling them from mixers. All channels // will be set except explicitly disabled (which will have PWM pulse = 0). for (int i = 0; i < MAX_MIX_ACTUATORS; i++) { if (command.Channel[i]) { if (mixers[i].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If mixer is for a motor we need to find the highest value of all motors command.Channel[i] = scaleMotor(status[i], actuatorSettings.ChannelMax[i], actuatorSettings.ChannelMin[i], actuatorSettings.ChannelNeutral[i], maxMotor, minMotor, armed, alwaysStabilizeWhenArmed, throttleDesired); } else { // else we scale the channel command.Channel[i] = scaleChannel(status[i], actuatorSettings.ChannelMax[i], actuatorSettings.ChannelMin[i], actuatorSettings.ChannelNeutral[i]); } } } // Store update time command.UpdateTime = dTMilliseconds; if (command.UpdateTime > command.MaxUpdateTime) { command.MaxUpdateTime = command.UpdateTime; } // Update output object ActuatorCommandSet(&command); // Update in case read only (eg. during servo configuration) ActuatorCommandGet(&command); #ifdef DIAG_MIXERSTATUS MixerStatusSet(&mixerStatus); #endif // Update servo outputs bool success = true; for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { success &= set_channel(n, command.Channel[n]); } PIOS_Servo_Update(); if (!success) { command.NumFailedUpdates++; ActuatorCommandSet(&command); AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); } #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeEnd(counter); #endif } } /** * Process mixing for one actuator */ float ProcessMixer(const int index, const float curve1, const float curve2, ActuatorDesiredData *desired, bool multirotor, bool fixedwing) { const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects const Mixer_t *mixer = &mixers[index]; float differential = 1.0f; // Apply differential only for fixedwing and Roll servos if (fixedwing && (mixerSettings.FirstRollServo > 0) && (mixer->type == MIXERSETTINGS_MIXER1TYPE_SERVO) && (mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] != 0)) { // Positive differential if (mixerSettings.RollDifferential > 0) { // Check for first Roll servo (should be left aileron or elevon) and Roll desired (positive/negative) if (((index == mixerSettings.FirstRollServo - 1) && (desired->Roll > 0.0f)) || ((index != mixerSettings.FirstRollServo - 1) && (desired->Roll < 0.0f))) { differential -= (mixerSettings.RollDifferential * 0.01f); } } else if (mixerSettings.RollDifferential < 0) { if (((index == mixerSettings.FirstRollServo - 1) && (desired->Roll < 0.0f)) || ((index != mixerSettings.FirstRollServo - 1) && (desired->Roll > 0.0f))) { differential -= (-mixerSettings.RollDifferential * 0.01f); } } } float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2]) * curve2) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL]) * desired->Roll * differential) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH]) * desired->Pitch) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW]) * desired->Yaw)) / 128.0f; if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { if (!multirotor) { // we allow negative throttle with a multirotor if (result < 0.0f) { // zero throttle result = 0.0f; } } } return result; } /** * Interpolate a throttle curve * Full range input (-1 to 1) for yaw, roll, pitch * Output range (-1 to 1) reversible motor/throttle curve * * Input of -1 -> -lookup(1) * Input of 0 -> lookup(0) * Input of 1 -> lookup(1) */ static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor) { float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements, multirotor); if (input < 0.0f) { return -unsigned_value; } else { return unsigned_value; } } /** * Interpolate a throttle curve * Full range input (-1 to 1) for yaw, roll, pitch * Output range (0 to 1) non-reversible motor/throttle curve * * Input of -1 -> lookup(1) * Input of 0 -> lookup(0) * Input of 1 -> lookup(1) */ static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor) { float abs_input = fabsf(input); float scale = abs_input * (float)(elements - 1); int idx1 = scale; scale -= (float)idx1; // remainder if (curve[0] < -1) { return abs_input; } int idx2 = idx1 + 1; if (idx2 >= elements) { idx2 = elements - 1; // clamp to highest entry in table if (idx1 >= elements) { if (multirotor) { // if multirotor frame we can return throttle values higher than 100%. // Since the we don't have elements in the curve higher than 100% we return // the last element multiplied by the throttle float if (input < 2.0f) { // this limits positive throttle to 200% of max value in table (Maybe this is too much allowance) return curve[idx2] * input; } else { return curve[idx2] * 2.0f; // return 200% of max value in table } } idx1 = elements - 1; } } float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale; return unsigned_value; } /** * 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.0f) { 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; } /** * Move and compress all motor outputs so that none goes below neutral, * and all motors are below or equal to max. */ static inline int16_t scaleMotorMoveAndCompress(float valueMotor, int16_t max, int16_t neutral, float maxMotor, float minMotor) { // The valueMotor parameter is the desired motor value somewhere in the // [minMotor, maxMotor] range, which is [< -1.00, > 1.00]. // // Before converting valueMotor to the [neutral, max] range, we scale // valueMotor to a value in the [0.0f, 1.0f] range. // // This is done by, first, conceptually moving all three values valueMotor, // minMotor, and maxMotor, equally so that the [minMotor, maxMotor] range, // are contained or overlaps with the [0.0f, 1.0f] range. // // Then if the [minMotor, maxMotor] range is larger than 1.0f, the values // are compressed enough to shrink the [minMotor + move, maxMotor + move] // range to fit within the [0.0f, 1.0f] range. // First move the values so that the source range [minMotor, maxMotor] // covers the target range [0.0f, 1.0f] as much as possible. float moveValue = 0.0f; if (minMotor <= 0.0f) { // Negative minMotor always adjust to 0. moveValue = -minMotor; } else if (maxMotor > 1.0f) { // A too large maxMotor value adjust the range down towards, but not past, the minMotor value. float beyondMax = maxMotor - 1.0f; moveValue = -(beyondMax < minMotor ? beyondMax : minMotor); } // Then calculate the compress value, if the source range is greater than 1.0f. float compressValue = 1.0f; float rangeMotor = maxMotor - minMotor; if (rangeMotor > 1.0f) { compressValue = rangeMotor; } // Combine the movement and compression, to get the value within [0.0f, 1.0f] float movedAndCompressedValue = (valueMotor + moveValue) / compressValue; // And last, convert the value into the [neutral, max] range. int16_t valueScaled = movedAndCompressedValue * ((float)(max - neutral)) + neutral; if (valueScaled > max) { valueScaled = max; // clamp to max value only after scaling is done. } PIOS_Assert(valueScaled >= neutral); return valueScaled; } /** * Constrain motor values to keep any one motor value from going too far out of range of another motor */ static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool alwaysStabilizeWhenArmed, float throttleDesired) { int16_t valueScaled; if (max > min) { valueScaled = scaleMotorMoveAndCompress(value, max, neutral, maxMotor, minMotor); } else { // not sure what to do about reversed polarity right now. Why would anyone do this? valueScaled = scaleChannel(value, max, min, neutral); } // I've added the bool alwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max. // NEVER should a motor be command at between min and neutral. I don't like the idea of stabilization ever commanding a motor to min, but we give people the option // This prevents motors startup sync issues causing possible ESC failures. // safety checks if (!armed) { // if not armed return min EVERYTIME! valueScaled = min; } else if (!alwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) { // all motors idle is alwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed // stabilize when armed? valueScaled = neutral; } else if (!spinWhileArmed && (throttleDesired <= 0.0f)) { // soft disarm valueScaled = min; } return valueScaled; } /** * Set actuator output to the neutral values (failsafe) */ static void setFailsafe() { /* grab only the parts that we are going to use */ int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; ActuatorCommandChannelGet(Channel); const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects // Reset ActuatorCommand to safe values for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { Channel[n] = actuatorSettings.ChannelMin[n]; } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { // reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point Channel[n] = actuatorSettings.ChannelNeutral[n]; } else { Channel[n] = 0; } } // Set alarm AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); // Update servo outputs for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { set_channel(n, Channel[n]); } // Send the updated command PIOS_Servo_Update(); // Update output object's parts that we changed ActuatorCommandChannelSet(Channel); } /** * determine buzzer or blink sequence **/ typedef enum { BUZZ_BUZZER = 0, BUZZ_ARMING = 1, BUZZ_INFO = 2, BUZZ_MAX = 3 } buzzertype; static inline bool buzzerState(buzzertype type) { // This is for buzzers that take a PWM input static uint32_t tune[BUZZ_MAX] = { 0 }; static uint32_t tunestate[BUZZ_MAX] = { 0 }; uint32_t newTune = 0; if (type == BUZZ_BUZZER) { // Decide what tune to play if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { newTune = 0b11110110110000; // pause, short, short, short, long } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { newTune = 0x80000000; // pause, short } else { newTune = 0; } } else { // BUZZ_ARMING || BUZZ_INFO uint8_t arming; FlightStatusArmedGet(&arming); // base idle tune newTune = 0x80000000; // 0b1000... // Merge the error pattern for InfoLed if (type == BUZZ_INFO) { if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { newTune |= 0b00000000001111111011111110000000; } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { newTune |= 0b00000000000000110110110000000000; } } // fast double blink pattern if armed if (arming == FLIGHTSTATUS_ARMED_ARMED) { newTune |= 0xA0000000; // 0b101000... } } // Do we need to change tune? if (newTune != tune[type]) { tune[type] = newTune; // resynchronize all tunes on change, so they stay in sync for (int i = 0; i < BUZZ_MAX; i++) { tunestate[i] = tune[i]; } } // Play tune bool buzzOn = false; static portTickType lastSysTime = 0; portTickType thisSysTime = xTaskGetTickCount(); portTickType dT = 0; // For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed if (tune[type]) { if (thisSysTime > lastSysTime) { dT = thisSysTime - lastSysTime; } else { lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80 } buzzOn = (tunestate[type] & 1); if (dT > 80) { // Go to next bit in alarm_seq_state for (int i = 0; i < BUZZ_MAX; i++) { tunestate[i] >>= 1; if (tunestate[i] == 0) { // All done, re-start the tune tunestate[i] = tune[i]; } } lastSysTime = thisSysTime; } } return buzzOn; } #if defined(ARCH_POSIX) || defined(ARCH_WIN32) static bool set_channel(uint8_t mixer_channel, uint16_t value) { return true; } #else static bool set_channel(uint8_t mixer_channel, uint16_t value) { switch (actuatorSettings.ChannelType[mixer_channel]) { case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], buzzerState(BUZZ_BUZZER) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]); return true; case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED: PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], buzzerState(BUZZ_ARMING) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]); return true; case ACTUATORSETTINGS_CHANNELTYPE_INFOLED: PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], buzzerState(BUZZ_INFO) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]); return true; case ACTUATORSETTINGS_CHANNELTYPE_PWM: { uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]]; switch (mode) { case ACTUATORSETTINGS_BANKMODE_ONESHOT125: // Remap 1000-2000 range to 125-250µs PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value * ACTUATOR_ONESHOT125_PULSE_FACTOR); break; case ACTUATORSETTINGS_BANKMODE_ONESHOT42: // Remap 1000-2000 range to 41,666-83,333µs PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value * ACTUATOR_ONESHOT42_PULSE_FACTOR); break; case ACTUATORSETTINGS_BANKMODE_MULTISHOT: // Remap 1000-2000 range to 5-25µs PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], (value * ACTUATOR_MULTISHOT_PULSE_FACTOR) - 180); break; case ACTUATORSETTINGS_BANKMODE_DSHOT: // Remap 0-2000 range to: 0 = disarmed, 1 to 47 = Reserved for special commands, 48 to 2047 = Active throttle control. if (value > 0) { value += 47; /* skip over reserved values */ } PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value); break; default: PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value); break; } return true; } #if defined(PIOS_INCLUDE_I2C_ESC) case ACTUATORSETTINGS_CHANNELTYPE_MK: return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel], value); case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel], value); #endif default: return false; } return false; } #endif /* if defined(ARCH_POSIX) || defined(ARCH_WIN32) */ /** * @brief Update the servo update rate */ static void actuator_update_rate_if_changed(bool force_update) { static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM]; static uint16_t prevDShotMode; bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0); bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0); if (force_update || (prevDShotMode != actuatorSettings.DShotMode)) { PIOS_Servo_DSHot_Rate(actuatorSettings.DShotMode); prevDShotMode = actuatorSettings.DShotMode; } // check if any setting is changed if (updateMode || updateFreq) { /* Something has changed, apply the settings to HW */ uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 }; for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { enum pios_servo_bank_mode servo_bank_mode = PIOS_SERVO_BANK_MODE_PWM; switch (actuatorSettings.BankMode[i]) { case ACTUATORSETTINGS_BANKMODE_ONESHOT125: case ACTUATORSETTINGS_BANKMODE_ONESHOT42: case ACTUATORSETTINGS_BANKMODE_MULTISHOT: freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered clock[i] = ACTUATOR_ONESHOT_CLOCK; // Setup an 12MHz timer clock servo_bank_mode = PIOS_SERVO_BANK_MODE_SINGLE_PULSE; break; case ACTUATORSETTINGS_BANKMODE_PWMSYNC: freq[i] = 100; clock[i] = ACTUATOR_PWM_CLOCK; servo_bank_mode = PIOS_SERVO_BANK_MODE_SINGLE_PULSE; break; case ACTUATORSETTINGS_BANKMODE_DSHOT: freq[i] = 100; clock[i] = ACTUATOR_PWM_CLOCK; servo_bank_mode = PIOS_SERVO_BANK_MODE_DSHOT; break; default: // PWM freq[i] = actuatorSettings.BankUpdateFreq[i]; clock[i] = ACTUATOR_PWM_CLOCK; servo_bank_mode = PIOS_SERVO_BANK_MODE_PWM; break; } if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) { PIOS_Servo_SetBankMode(i, servo_bank_mode); } } memcpy(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)); PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM); memcpy(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)); // retrieve mode from related bank for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) { uint8_t bank = PIOS_Servo_GetPinBank(i); pinsMode[i] = actuatorSettings.BankMode[bank]; } } } static void update_servo_active() { /* For each mixer output that is not disabled, * figure out servo address and send allocation map to pios_servo driver. * We need to execute this when either ActuatorSettings or MixerSettings change. */ uint32_t servo_active = 0; Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) { servo_active |= 1 << actuatorSettings.ChannelAddr[ct]; } } PIOS_Servo_SetActive(servo_active); } static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { ActuatorSettingsGet(&actuatorSettings); spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; if (frameType == FRAME_TYPE_GROUND) { spinWhileArmed = false; } actuator_update_rate_if_changed(false); update_servo_active(); } static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { MixerSettingsGet(&mixerSettings); mixer_settings_count = 0; Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) { mixer_settings_count++; } } update_servo_active(); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { frameType = GetCurrentFrameType(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES uint8_t TreatCustomCraftAs; VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs); if (frameType == FRAME_TYPE_CUSTOM) { switch (TreatCustomCraftAs) { case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: frameType = FRAME_TYPE_FIXED_WING; break; case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: frameType = FRAME_TYPE_MULTIROTOR; break; case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND: frameType = FRAME_TYPE_GROUND; break; } } #endif SystemSettingsThrustControlGet(&thrustType); } /** * @} * @} */