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-1835 - Motor Constraints for 15.02.02

Browse Source 
OP-1835 - Motor Constraints for 15.02.02
This commit is contained in:
RS2K 2015-05-02 12:55:43 -05:00
parent 5475b395e8
commit 57d062ff67
3 changed files with 186 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

221
flight/modules/Actuator/actuator.c
View File

@ -40,6 +40,7 @@
#include "actuatordesired.h"
#include "actuatorcommand.h"
#include "flightstatus.h"
#include <flightmodesettings.h>
#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)
*/

10
flight/modules/Stabilization/innerloop.c
View File

@ -49,7 +49,7 @@
#include <stabilization.h>
#include <virtualflybar.h>
#include <cruisecontrol.h>
#include <sanitycheck.h>
// 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;

1
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -105,6 +105,7 @@
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0903NE:POI:PathPlanner:AutoCruise:Land;"/>
<field name="AlwaysStabilizeWhenArmed" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE" description="For Multirotors. Always stabilize no matter the throttle setting when vehicle is armed. Does not work when vehicle is set to Always Armed."/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>