diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 4e795027b..8529811b7 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -40,6 +40,7 @@ #include "actuatordesired.h" #include "actuatorcommand.h" #include "flightstatus.h" +#include #include "mixersettings.h" #include "mixerstatus.h" #include "cameradesired.h" @@ -98,9 +99,10 @@ 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); -static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements); +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); @@ -108,7 +110,7 @@ static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev); float ProcessMixer(const int index, const float curve1, const float curve2, ActuatorDesiredData *desired, - const float period); + const float period, bool multirotor); // this structure is equivalent to the UAVObjects for one mixer. typedef struct { @@ -199,6 +201,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) ActuatorCommandData command; ActuatorDesiredData desired; MixerStatusData mixerStatus; + FlightModeSettingsData settings; FlightStatusData flightStatus; float throttleDesired; float collectiveDesired; @@ -245,6 +248,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) dTSeconds = dTMilliseconds * 0.001f; FlightStatusGet(&flightStatus); + FlightModeSettingsGet(&settings); ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); @@ -266,22 +270,32 @@ static void actuatorTask(__attribute__((unused)) void *parameters) 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 alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED; + bool AlwaysStabilizeWhenArmed = settings.AlwaysStabilizeWhenArmed == FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMED_TRUE; + if (alwaysArmed) { + AlwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous. + } // safety settings if (!armed) { - throttleDesired = 0; + 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) - if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { - desired.Roll = 0; - } - if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { - desired.Pitch = 0; - } - if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { - desired.Yaw = 0; + // 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; + } } } @@ -296,13 +310,13 @@ static void actuatorTask(__attribute__((unused)) void *parameters) AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - float curve1 = 0.0f; + 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); + curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM, multirotor); // The source for the secondary curve is selectable AccessoryDesiredData accessory; @@ -310,25 +324,38 @@ static void actuatorTask(__attribute__((unused)) void *parameters) switch (curve2Source) { case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: // assume reversible motor/mixer initially - curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); break; case MIXERSETTINGS_CURVE2SOURCE_ROLL: // Throttle curve contribution the same for +ve vs -ve roll - curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + 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 - curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2, - MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + 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 - curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + 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); + MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); break; case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: @@ -338,7 +365,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) 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); + curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor); } else { curve2 = 0.0f; } @@ -350,6 +377,8 @@ static void actuatorTask(__attribute__((unused)) void *parameters) 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). @@ -367,13 +396,15 @@ static void actuatorTask(__attribute__((unused)) void *parameters) } if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) { - if (curve1 < 0.0f) { - curve1 = 0.0f; + float nonreversible_curve1 = curve1; + float nonreversible_curve2 = curve2; + if (nonreversible_curve1 < 0.0f) { + nonreversible_curve1 = 0.0f; } - if (curve2 < 0.0f) { - curve2 = 0.0f; + if (nonreversible_curve2 < 0.0f) { + nonreversible_curve2 = 0.0f; } - status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds); + status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds, multirotor); // If not armed or motors aren't meant to spin all the time if (!armed || (!spinWhileArmed && !positiveThrottle)) { @@ -384,10 +415,14 @@ static void actuatorTask(__attribute__((unused)) void *parameters) // If armed meant to keep spinning, else if ((spinWhileArmed && !positiveThrottle) || (status[ct] < 0)) { - 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, dTSeconds); + status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor); // 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) { @@ -396,7 +431,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) status[ct] = 0; // force neutral throttle } } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) { - status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds); + status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor); } else { status[ct] = -1; @@ -442,16 +477,39 @@ static void actuatorTask(__attribute__((unused)) void *parameters) } } } + + // 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]) { - command.Channel[i] = scaleChannel(status[i], - actuatorSettings.ChannelMax[i], - actuatorSettings.ChannelMin[i], - actuatorSettings.ChannelNeutral[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]); + } } } @@ -495,7 +553,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) * Process mixing for one actuator */ float ProcessMixer(const int index, const float curve1, const float curve2, - ActuatorDesiredData *desired, const float period) + ActuatorDesiredData *desired, const float period, bool multirotor) { static float lastFilteredResult[MAX_MIX_ACTUATORS]; const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects @@ -509,8 +567,10 @@ float ProcessMixer(const int index, const float curve1, const float curve2, // note: no feedforward for reversable motors yet for safety reasons if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { - if (result < 0.0f) { // idle throttle - result = 0.0f; + if (!multirotor) { // we allow negative throttle with a multirotor + if (result < 0.0f) { // zero throttle + result = 0.0f; + } } // feed forward @@ -558,9 +618,9 @@ float ProcessMixer(const int index, const float curve1, const float curve2, * Input of 0 -> lookup(0) * Input of 1 -> lookup(1) */ -static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements) +static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor) { - float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements); + float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements, multirotor); if (input < 0.0f) { return -unsigned_value; @@ -578,7 +638,7 @@ static float MixerCurveFullRangeProportional(const float input, const float *cur * Input of 0 -> lookup(0) * Input of 1 -> lookup(1) */ -static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements) +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); @@ -586,16 +646,22 @@ static float MixerCurveFullRangeAbsolute(const float input, const float *curve, scale -= (float)idx1; // remainder if (curve[0] < -1) { - return input; - } - if (idx1 < 0) { - idx1 = 0; // clamp to lowest entry in table - scale = 0; + 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; } } @@ -638,6 +704,73 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr 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; + int16_t maxMotorScaled; + int16_t minMotorScaled; + int16_t diff; + + // Scale + valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral; + maxMotorScaled = (int16_t)(maxMotor * ((float)(max - neutral))) + neutral; + minMotorScaled = (int16_t)(minMotor * ((float)(max - neutral))) + neutral; + + + if (max > min) { + diff = max - maxMotorScaled; // difference between max allowed and actual max motor + if (diff < 0) { // if the difference is smaller than 0 we add it to the scaled value + valueScaled += diff; + } + diff = neutral - minMotorScaled; // difference between min allowed and actual min motor + if (diff > 0) { // if the difference is larger than 0 we add it to the scaled value + valueScaled += diff; + } + // todo: make this flow easier to understand + if (valueScaled > max) { + valueScaled = max; // clamp to max value only after scaling is done. + } + + if ((valueScaled < neutral) && (spinWhileArmed) && AlwaysStabilizeWhenArmed) { + valueScaled = neutral; // clamp to neutral value only after scaling is done. + } else if ((valueScaled < neutral) && (!spinWhileArmed) && AlwaysStabilizeWhenArmed) { + valueScaled = neutral; // clamp to neutral value only after scaling is done. //throttle goes to min if throttledesired is equal to or less than 0 below + } else if (valueScaled < neutral) { + valueScaled = min; // clamp to min value only after scaling is done. + } + } else { + // not sure what to do about reversed polarity right now. Why would anyone do this? + if (valueScaled < max) { + valueScaled = max; // clamp to max value only after scaling is done. + } + if (valueScaled > min) { + valueScaled = min; // clamp to min value only after scaling is done. + } + } + + // 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) */ diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 2ed1ae57b..39f628cb8 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -49,7 +49,7 @@ #include #include #include - +#include // Private constants #define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL @@ -239,6 +239,7 @@ static void stabilizationInnerloopTask() float *actuatorDesiredAxis = &actuator.Roll; int t; float dT; + bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); for (t = 0; t < AXES; t++) { @@ -308,7 +309,12 @@ static void stabilizationInnerloopTask() } } - actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f); + if (!multirotor) { + // we only need to clamp the desired axis to a sane range if the frame is not a multirotor type + // we don't want to do any clamping until after the motors are calculated and scaled. + // need to figure out what to do with a tricopter tail servo. + actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f); + } } actuator.UpdateTime = dT * 1000; diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index f12df3c88..2ffae11d6 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -105,6 +105,7 @@ %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ %0903NE:POI:PathPlanner:AutoCruise:Land;"/> +