From 6d7d10caba5af4697c4e0a3a90b8bbb2f22deb91 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 22 Mar 2014 01:18:06 -0400 Subject: [PATCH] OP-1259 PowerDelayComp addtl inverted modes --- flight/modules/Stabilization/stabilization.c | 199 +++++++++++++++--- flight/pios/inc/usb_conf.h | 4 +- .../src/plugins/config/stabilization.ui | 28 +-- .../stabilizationsettings.xml | 6 +- 4 files changed, 190 insertions(+), 47 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index e5607272e..d148a8867 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -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, diff --git a/flight/pios/inc/usb_conf.h b/flight/pios/inc/usb_conf.h index 6c9ec9008..2540734df 100644 --- a/flight/pios/inc/usb_conf.h +++ b/flight/pios/inc/usb_conf.h @@ -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 */ diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 6414995eb..2a663a33d 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -13377,7 +13377,6 @@ border-radius: 5; 5 - 0.000100000000000 @@ -19418,7 +19417,6 @@ border-radius: 5; false - 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; 39 39 39 - @@ -21978,7 +21975,6 @@ border-radius: 5; - @@ -24199,7 +24195,7 @@ border-radius: 5; - <html><head/><body><p>-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. + <html><head/><body><p>-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. </p></body></html> @@ -24212,10 +24208,10 @@ border-radius: 5; 0 - -1.000000000000000 + -3.000000000000000 - 1.000000000000000 + 2.000000000000000 @@ -24345,7 +24341,7 @@ border-radius: 5; - <html><head/><body><p>This needs to be 0 for all copters except CP helis that are using idle up.</p></body></html> + <html><head/><body><p>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.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -24354,12 +24350,21 @@ border-radius: 5; true - 0 + 5 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.250000000000000 objname:StabilizationSettings - fieldname:CruiseControlNeutralThrust + fieldname:CruiseControlPowerDelayComp haslimits:no scale:1 buttongroup:16 @@ -24557,7 +24562,7 @@ color: rgb(255, 255, 255); border-radius: 5; - NeutralThrust + PowerDelayComp Qt::AlignCenter @@ -26162,7 +26167,6 @@ border-radius: 5; Qt::StrongFocus - diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 76153fd9d..7da6f92b4 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -30,13 +30,13 @@ - - + + - +