1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-19 09:54:15 +01:00

OP-1259 PowerDelayComp addtl inverted modes

This commit is contained in:
Cliff Geerdes 2014-03-22 01:18:06 -04:00
parent 820e8dfa3e
commit 6d7d10caba
4 changed files with 190 additions and 47 deletions

View File

@ -111,7 +111,8 @@ static uint8_t cruise_control_max_angle;
static float cruise_control_max_power_factor;
static float cruise_control_power_trim;
static int8_t cruise_control_inverted_power_switch;
static float cruise_control_neutral_thrust;
static float cruise_control_max_power_factor_angle;
static float cruise_control_half_power_delay;
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
// Private functions
@ -558,55 +559,188 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Thrust = stabDesired.Thrust;
// Cruise Control
// modify thrust according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// to maintain same altitude with changing bank angle
// but without manually adjusting thrust
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
uint8_t previous_flight_mode_switch_position = 254; // something invalid
if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
&& cruise_control_max_power_factor > 0.0001f) {
static uint8_t toggle;
static float factor;
static uint32_t previous_time;
static uint8_t calc_count;
float angle;
uint32_t time;
// flight mode has changed. there could be a time gap
if (flight_mode_switch_position != previous_flight_mode_switch_position) {
// flag to skip this loop because time diff may be invalid
previous_time = 0;
}
time = PIOS_DELAY_GetuS(); // good for 72 minutes, then it wraps (handled OK)
// get attitude state and calculate angle
// do it every 8th iteration to save CPU
if ((toggle++ & 7) == 0) {
if (time != previous_time && calc_count++ >= 8) {
static float previous_angle;
float rate;
// float thrust;
calc_count = 0;
// spherical right triangle
// 0 <= acosf() <= Pi
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
// if past the cutoff angle (60 to 180 (180 means never))
if (angle > cruise_control_max_angle) {
// -1 reversed collective, 0 zero power, or 1 normal power
// these are all unboosted
factor = cruise_control_inverted_power_switch;
// combined bank angle change rate in degrees per second
// rate is currently calculated over the most recent CALCCOUNT loops
// keeping the sign for rate is important, it can be negative
if (previous_time == 0UL) {
rate = 0.0f;
} else {
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = cruise_control_max_power_factor;
} else {
factor = 1.0f / fabsf(cos_lookup_deg(angle));
if (factor > cruise_control_max_power_factor) {
factor = cruise_control_max_power_factor;
// handle wrap around
// the assumption here is that it's been less than 0x7fffffff since prev_time
// and thus likewise since time
// i.e. that casting prev_time to a signed type produces a negative
if (time >= previous_time) { // the usual case
rate = (angle - previous_angle) / ((float) (time - previous_time) / 1000000.0f);
} else { // the wrap around case
rate = (angle - previous_angle) / ((float) ((uint32_t) ((int32_t) time - (int32_t) previous_time)) / 1000000.0f);
}
}
previous_time = time;
previous_angle = angle;
// define "within range" to be those transitions that should be executing now
// - recall that each impulse transition is spread out over a range of time / angle
// there is only one transition and the high power level for it is either
// = full thrust
// = max power factor
// = 1/fabsf(cos(angle))
// OK, you could say there are two transitions in 360 degrees (90 and 270)
{
float thrust;
{
float full_thrust_angle;
// calculate angle where thrust is full (as limited by max_thrust)
full_thrust_angle = RAD2DEG(acosf(cruise_control_max_thrust / actuatorDesired.Thrust));
// if full thrust comes before the artificially limited max_power_factor
if (full_thrust_angle < cruise_control_max_power_factor_angle) {
thrust = cruise_control_max_thrust;
} else {
thrust = cruise_control_max_power_factor * actuatorDesired.Thrust;
// 'full_thrust_angle' is now the angle that goes with 'thrust'
full_thrust_angle = cruise_control_max_power_factor_angle;
}
// if the transition is outside the max thrust regions
if (full_thrust_angle < cruise_control_max_angle
|| 180.0f - cruise_control_max_angle < full_thrust_angle ) {
// max thrust is 1/cos(transition angle)
thrust = 1.0f / fabsf(cos_lookup_deg(cruise_control_max_angle));
}
}
// determine if we are in range of the transition
// calculate the actual proportion of change in thrust
switch (cruise_control_inverted_power_switch) {
case -3:
case -2:
// CP heli case, stroke is max to -max
// thrust = (thrust + thrust) / (cruise_control_max_thrust + cruise_control_max_thrust);
thrust /= cruise_control_max_thrust;
break;
case -1:
// CP heli case, stroke is max to -stick
thrust = (thrust + actuatorDesired.Thrust) / (cruise_control_max_thrust + cruise_control_max_thrust);
break;
case 0:
// normal multi-copter case, stroke is max to min
thrust = (thrust - cruise_control_min_thrust) / (cruise_control_max_thrust - cruise_control_min_thrust);
break;
case 1:
// simply turn off boost, stroke is max to stick
thrust = (thrust - actuatorDesired.Thrust) / (cruise_control_max_thrust - cruise_control_min_thrust);
break;
case 2:
// CP heli case, no transition, stroke is zero
thrust = 0.0f;
break;
}
// multiply this proportion of max stroke, times the max stroke time, to get this stroke time
// we only want half of this time before the transition (and half after the transition)
thrust *= cruise_control_half_power_delay;
// times angular rate gives angle that this stroke will take to execute
thrust *= fabsf(rate);
// if the transition is within range we use it, else we just use the current calculated thrust
if (cruise_control_max_angle - thrust < angle
&& angle < cruise_control_max_angle + thrust) {
// default to a little above max angle
angle = cruise_control_max_angle + 0.01f;
// if roll direction is downward then thrust value is taken from below max angle
if (rate < 0.0f) {
angle -= 0.02f;
}
}
}
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = cruise_control_max_power_factor;
} else {
// the simple bank angle boost calculation that Cruise Control revolves around
factor = 1.0f / fabsf(cos_lookup_deg(angle));
// factor in the power trim, no effect at 1.0, linear effect increases with factor
factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
// if inverted and they want negative boost
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
factor = -factor;
// as long as thrust is getting reversed
// we may as well do pitch and yaw for a complete "invert switch"
// limit to user specified max power multiplier
if (factor > cruise_control_max_power_factor) {
factor = cruise_control_max_power_factor;
}
}
/*
convert these to enums? something like
upright power: zero, normal, boosted
inverted thrust direction: unchanged, reversed
inverted power: zero, normal, boosted
inverted yaw/pitch reverse: off, on
*/
// if past max angle and so needing to go into an inverted mode
if (angle >= cruise_control_max_angle) {
// -3 inverted mode, -2 boosted reverse, -1 normal reverse, 0 zero power, 1 normal power, 2 boosted power
switch (cruise_control_inverted_power_switch) {
case -3: // reversed boosted thrust with pitch/yaw reverse
actuatorDesired.Pitch = -actuatorDesired.Pitch;
actuatorDesired.Yaw = -actuatorDesired.Yaw;
factor = -factor;
break;
case -2: // reversed boosted thrust
factor = -factor;
break;
case -1: // reversed normal thrust
factor = -1.0f;
break;
case 0: // no thrust
factor = -0.0f;
break;
case 1: // normal thrust
factor = 1.0f;
break;
case 2: // normal boosted thrust
// no change to factor
break;
}
}
}
// also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
if (actuatorDesired.Thrust > cruise_control_min_thrust) {
// quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
// CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
actuatorDesired.Thrust = (actuatorDesired.Thrust - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
actuatorDesired.Thrust *= factor;
// limit to user specified absolute max thrust
if (actuatorDesired.Thrust > cruise_control_max_thrust) {
actuatorDesired.Thrust = cruise_control_max_thrust;
} else if (actuatorDesired.Thrust < cruise_control_min_thrust) {
@ -615,6 +749,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
}
previous_flight_mode_switch_position = flight_mode_switch_position;
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuatorDesired);
} else {
@ -827,13 +963,14 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f;
}
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f;
cruise_control_max_angle = settings.CruiseControlMaxAngle;
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
cruise_control_neutral_thrust = (float)settings.CruiseControlNeutralThrust / 100.0f;
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f;
cruise_control_max_angle = settings.CruiseControlMaxAngle;
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
cruise_control_half_power_delay = settings.CruiseControlPowerDelayComp / 2.0f;
cruise_control_max_power_factor_angle = RAD2DEG(acosf(1.0f / settings.CruiseControlMaxPowerFactor));
memcpy(
cruise_control_flight_mode_switch_pos_enable,

View File

@ -40,7 +40,9 @@
/* EP1 */
/* rx/tx buffer base address */
#define ENDP1_TXADDR (0x60)
#define ENDP1_RXADDR (0x80)
// fix the F1 USB issue
//#define ENDP1_RXADDR (0x80)
#define ENDP1_RXADDR (0xa0)
/* EP2 */
/* rx/tx buffer base address */

View File

@ -13377,7 +13377,6 @@ border-radius: 5;</string>
</property>
<property name="decimals">
<number>5</number>
</property>
<property name="singleStep">
<double>0.000100000000000</double>
@ -19418,7 +19417,6 @@ border-radius: 5;</string>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
@ -19943,7 +19941,6 @@ border-radius: 5;</string>
<red>39</red>
<green>39</green>
<blue>39</blue>
</color>
</brush>
</colorrole>
@ -21978,7 +21975,6 @@ border-radius: 5;</string>
</size>
</property>
</spacer>
</item>
<item row="1" column="3">
<widget class="QDoubleSpinBox" name="AccelKp">
@ -24199,7 +24195,7 @@ border-radius: 5;</string>
<item row="1" column="4">
<widget class="QDoubleSpinBox" name="doubleSpinBox_6">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;-1, 0, or 1. Cruise Control multiplies the throttle/collective stick by this value if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrust) when inverted. 1 says to use the unboosted stick value. -1 (DON'T USE, INCOMPLETE, UNTESTED, for use by CP helis using idle up) says to reverse the collective stick when inverted.
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;-3, -2, -1, 0, 1, or 2. Cruise Control uses this to determine the inverted power mode which is used if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrust) when inverted. 1 says to use the unboosted stick value. 2 says use the boosted stick value. -1 (UNTESTED, for use by CP helis using idle up) says to reverse the collective channel when inverted. -2 is -1 but boosted. -3 is -2 but with pitch and yaw also inverted.
&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
@ -24212,10 +24208,10 @@ border-radius: 5;</string>
<number>0</number>
</property>
<property name="minimum">
<double>-1.000000000000000</double>
<double>-3.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
<double>2.000000000000000</double>
</property>
<property name="objrelation" stdset="0">
<stringlist>
@ -24345,7 +24341,7 @@ border-radius: 5;</string>
<item row="3" column="4">
<widget class="QDoubleSpinBox" name="doubleSpinBox_7">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This needs to be 0 for all copters except CP helis that are using idle up.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The fraction of a second it takes for your vehicle to go from zero thrust (10%) for multis (full negative thrust (-90%) for CP helis) to full positive thrust (+90%). Increase this if continuous left flips walk off to the left.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -24354,12 +24350,21 @@ border-radius: 5;</string>
<bool>true</bool>
</property>
<property name="decimals">
<number>0</number>
<number>5</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.250000000000000</double>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlNeutralThrust</string>
<string>fieldname:CruiseControlPowerDelayComp</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
@ -24557,7 +24562,7 @@ color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>NeutralThrust</string>
<string>PowerDelayComp</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@ -26162,7 +26167,6 @@ border-radius: 5;</string>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="styleSheet">
<string notr="true"/>

View File

@ -30,13 +30,13 @@
<field name="RattitudeModeTransistion" units="%" type="uint8" elements="1" defaultvalue="80"/>
<field name="CruiseControlMinThrust" units="%" type="uint8" elements="1" defaultvalue="5"/>
<field name="CruiseControlMaxThrust" units="%" type="uint8" elements="1" defaultvalue="90"/>
<field name="CruiseControlMinThrust" units="%" type="uint8" elements="1" defaultvalue="5"/>
<field name="CruiseControlMaxThrust" units="%" type="uint8" elements="1" defaultvalue="90"/>
<field name="CruiseControlMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="105"/>
<field name="CruiseControlMaxPowerFactor" units="x" type="float" elements="1" defaultvalue="3.0"/>
<field name="CruiseControlPowerTrim" units="%" type="float" elements="1" defaultvalue="100.0"/>
<field name="CruiseControlInvertedPowerSwitch" units="" type="int8" elements="1" defaultvalue="0"/>
<field name="CruiseControlNeutralThrust" units="%" type="uint8" elements="1" defaultvalue="0"/>
<field name="CruiseControlPowerDelayComp" units="sec" type="float" elements="1" defaultvalue="0.25"/>
<field name="CruiseControlFlightModeSwitchPosEnable" units="" type="enum" elements="6" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>