From 6d7d10caba5af4697c4e0a3a90b8bbb2f22deb91 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 22 Mar 2014 01:18:06 -0400 Subject: [PATCH 01/67] 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 @@ - - + + - + From 81f31e64a93ba5dfc78963f547eec7c1b8849fd3 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 26 Mar 2014 13:48:24 -0400 Subject: [PATCH 02/67] OP-1259 WIP to merge next in locally --- flight/modules/Stabilization/stabilization.c | 340 ++++++++++-------- .../src/plugins/config/stabilization.ui | 8 +- .../stabilizationsettings.xml | 2 +- 3 files changed, 205 insertions(+), 145 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index d148a8867..53a0a8d02 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -113,6 +113,7 @@ static float cruise_control_power_trim; static int8_t cruise_control_inverted_power_switch; static float cruise_control_max_power_factor_angle; static float cruise_control_half_power_delay; +static float cruise_control_thrust_difference; static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; // Private functions @@ -122,6 +123,9 @@ static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent *ev); static void BankUpdatedCb(UAVObjEvent *ev); static void SettingsBankUpdatedCb(UAVObjEvent *ev); +static float CruiseControlAngleToFactor(float angle); +static float CruiseControlFactorToThrust(float factor, float stick_thrust); +static float CruiseControlLimitThrust(float thrust); /** * Module initialization @@ -433,7 +437,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f // the following assumes the transition would otherwise be at 0.618033989f - // and that assumes that Att ramps up to max roll rate + // and THAT assumes that Att ramps up to max roll rate // when a small number of degrees off of where it should be // if below the transition angle (still in attitude mode) @@ -441,21 +445,24 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) if (magnitude <= rattitude_mode_transition_stick_position) { magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position; } else { - magnitude = (magnitude - rattitude_mode_transition_stick_position) * (1.0f-STICK_VALUE_AT_MODE_TRANSITION) / (1.0f - rattitude_mode_transition_stick_position) + STICK_VALUE_AT_MODE_TRANSITION; + magnitude = (magnitude - rattitude_mode_transition_stick_position) + * (1.0f-STICK_VALUE_AT_MODE_TRANSITION) + / (1.0f - rattitude_mode_transition_stick_position) + + STICK_VALUE_AT_MODE_TRANSITION; } rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude + magnitude * rateDesiredAxisRate; // Compute the inner loop for the averaged rate // actuatorDesiredAxis[i] is the weighted average - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, + rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: - // Store for debugging output rateDesiredAxis[i] = stabDesiredAxis[i]; @@ -564,42 +571,47 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // 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 float factor; + static float previous_angle; static uint32_t previous_time; + static bool previous_time_valid; // initially false + static bool inverted_flight_mode; static uint8_t calc_count; - float angle; + static uint8_t previous_flight_mode_switch_position; uint32_t time; - // flight mode has changed. there could be a time gap + // flight mode has changed. there could be a time gap. leave thrust alone. + // angle could be invalid because of the time spent with CC off + // if (cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != cruise_control_flight_mode_switch_pos_enable[previous_flight_mode_switch_position]) { if (flight_mode_switch_position != previous_flight_mode_switch_position) { - // flag to skip this loop because time diff may be invalid - previous_time = 0; + previous_flight_mode_switch_position = flight_mode_switch_position; + // flag to skip delay comp calculations and just use thrust as it is + // because time diff may be invalid + previous_time_valid = false; } 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 (time != previous_time && calc_count++ >= 8) { - static float previous_angle; - float rate; - // float thrust; + if ((time != previous_time && calc_count++ >= 8) || previous_time_valid == false) { + float angle; calc_count = 0; // spherical right triangle - // 0 <= acosf() <= Pi + // 0 <= acosf() <= Pi(180) angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch))); // combined bank angle change rate in degrees per second // rate is currently calculated over the most recent CALCCOUNT loops + // it may be that the angle doesn't change at all between two consecutive passes + // because the sensors aren't read that often // keeping the sign for rate is important, it can be negative - if (previous_time == 0UL) { - rate = 0.0f; - } else { + if (previous_time_valid) { + float rate; // handle wrap around // the assumption here is that it's been less than 0x7fffffff since prev_time // and thus likewise since time @@ -609,98 +621,94 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } 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 + // 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) + // 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 (actuatorDesired.Thrust > 0.0f) { + float max_thrust; + // calculate the max positive thrust at the transition + // <5% t<5 + // 5.01% 19.3 + // 10% 38.6 + // 50% max + // 100% max + thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust); + +/* +revisit all the GCS settings +51 <= maxt <= 100 +No (0 <= mint <= 49 plus -51 -> -100) +0 <= mint <= 49 or maybe -100 -> 49 +1 <= mpf <= 50 +50 <= trim <= 200 +?? <= maxa <= 255? +0 <= pdc <= 2? +*/ + + // determine if we are in range of the transition + +//CP helis go -max to +max and have min set to 0 and max to +1 + +//min_thrust is defined to be where the power goes when inverted + + // 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_thrust_difference; + break; + case -1: + // CP heli case, stroke is max to -stick + thrust = (thrust + CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference; + break; + case 0: + // normal multi-copter case, stroke is max to zero + // technically max to constant min_thrust + // can be used by CP + thrust = (thrust - cruise_control_min_thrust) / cruise_control_thrust_difference; +//shit, cruise_control_thrust_difference is a problem, maybe remove it +//thrust_difference is always max-min, and now for CP it needs to be max+max here +//maybe not a problem since we don't limit thrust on the negative side any more +//CP: max=1.0, min=-1.0 diff=2.0 +//MR: max=0.9, min=0.1 diff=0.8 + break; + case 1: + // simply turn off boost, stroke is max to +stick + thrust = (thrust - CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference; + break; + case 2: + // CP heli case, no transition, stroke is zero + thrust = 0.0f; + break; } - // 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)); + // 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; + } } - } - // 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; + factor = CruiseControlAngleToFactor(angle); + } else { // if thrust > 0 set factor from angle; else + factor = 1.0f; } - // 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; - // 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 @@ -709,47 +717,46 @@ 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; + inverted_flight_mode = false; + // 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 + inverted_flight_mode = true; + 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; + } } - } - } + } // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone + previous_time = time; + previous_time_valid = true; + previous_angle = angle; + } // every 8th time - // also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors - if (actuatorDesired.Thrust > cruise_control_min_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) { - actuatorDesired.Thrust = cruise_control_min_thrust; - } + // don't touch thrust if it's less than min_thrust + // without this test, props will spin up to min thrust even at zero throttle stick + actuatorDesired.Thrust = CruiseControlFactorToThrust(factor, actuatorDesired.Thrust); + if (inverted_flight_mode) { + actuatorDesired.Pitch = -actuatorDesired.Pitch; + actuatorDesired.Yaw = -actuatorDesired.Yaw; } - } - - previous_flight_mode_switch_position = flight_mode_switch_position; + } // if Cruise Control is enabled on this flight switch position if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { ActuatorDesiredSet(&actuatorDesired); @@ -920,6 +927,54 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) } +static float CruiseControlAngleToFactor(float angle) +{ + float factor; + // 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; + // limit to user specified max power multiplier + if (factor > cruise_control_max_power_factor) { + factor = cruise_control_max_power_factor; + } + } + return (factor); +} + + +// assumes 1.0 <= factor <= 100.0 +// a factor of less than 1.0 could make it return a value less than cruise_control_min_thrust +static float CruiseControlFactorToThrust(float factor, float thrust) +{ + // don't touch if below min_thrust or we never get to full down stick + // e.g. multicopter motors always spin + if (thrust > cruise_control_min_thrust) { + thrust = CruiseControlLimitThrust(thrust * factor); + } + return (thrust); +} + + +// we don't want to limit it to cruise_control_min_thrust on the low side +// because that is always close to zero, and CP helis need to run down to -1 +// but CP needs to have min=-1 +static float CruiseControlLimitThrust(float thrust) +{ + // limit to user specified absolute max thrust + if (thrust > cruise_control_max_thrust) { + thrust = cruise_control_max_thrust; +// } else if (thrust < cruise_control_min_thrust) { +// thrust = cruise_control_min_thrust; + } + return (thrust); +} + + static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { StabilizationSettingsGet(&settings); @@ -957,10 +1012,10 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) cur_flight_mode = -1; // Rattitude stick angle where the attitude to rate transition happens - if (settings.RattitudeModeTransistion < (uint8_t) 10) { + if (settings.RattitudeModeTransition < (uint8_t) 10) { rattitude_mode_transition_stick_position = 10.0f / 100.0f; } else { - rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f; + rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f; } cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f; @@ -971,6 +1026,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) 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)); + cruise_control_thrust_difference = cruise_control_max_thrust - cruise_control_min_thrust; memcpy( cruise_control_flight_mode_switch_pos_enable, diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 2a663a33d..593fba6e1 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -13377,6 +13377,7 @@ border-radius: 5; 5 + 0.000100000000000 @@ -19417,6 +19418,7 @@ 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)); @@ -19941,6 +19943,7 @@ border-radius: 5; 39 39 39 + @@ -21975,6 +21978,7 @@ border-radius: 5; + @@ -24004,7 +24008,7 @@ border-radius: 5; - + 0 @@ -24050,7 +24054,7 @@ border-radius: 5; objname:StabilizationSettings - fieldname:RattitudeModeTransistion + fieldname:RattitudeModeTransition haslimits:no scale:1 buttongroup:15 diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 7da6f92b4..09edbfd18 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -28,7 +28,7 @@ - + From 48345f019456f58d36092b05a7c4854bff516a25 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Thu, 27 Mar 2014 01:37:56 -0400 Subject: [PATCH 03/67] OP-1259 First tested version --- flight/modules/Stabilization/stabilization.c | 163 +++++++++++++------ 1 file changed, 111 insertions(+), 52 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 53a0a8d02..e1d8105eb 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -580,65 +580,87 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) static bool previous_time_valid; // initially false static bool inverted_flight_mode; static uint8_t calc_count; - static uint8_t previous_flight_mode_switch_position; + static uint8_t previous_flight_mode_switch_position = 250; uint32_t time; - // flight mode has changed. there could be a time gap. leave thrust alone. - // angle could be invalid because of the time spent with CC off - // if (cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != cruise_control_flight_mode_switch_pos_enable[previous_flight_mode_switch_position]) { + // It takes significant time for the motors of a multi-copter to + // spin up. It takes significant time for the collective servo of + // a CP heli to move from one end to the other. Both of those are + // modeled here as linear, i.e. twice as much change takes twice + // as long. Given a correctly configured maximum delay time this + // code calculates how far in advance to start the control + // transition so that half way through the physical transition it + // is just crossing the transition angle. + // Example: Rotation rate = 360. Full stroke delay = 0.2 + // Transition angle 90 degrees. Start the transition 0.1 second + // before 90 degrees (36 degrees at 360 deg/sec) and it will be + // complete 0.1 seconds after 90 degrees. + + // Detect if the flight mode switch has changed. If it has, there + // could be a time gap. E.g. enabled, then disabled for 30 seconds + // then enabled again. Previous_angle will also be invalid because + // of the time spent with Cruise Control off. if (flight_mode_switch_position != previous_flight_mode_switch_position) { previous_flight_mode_switch_position = flight_mode_switch_position; - // flag to skip delay comp calculations and just use thrust as it is - // because time diff may be invalid + // Force calculations on this pass (usually every 8th pass), + // but ignore rate calculations (uses time, previous_time, angle, + // previous_angle) previous_time_valid = false; } - 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 + time = PIOS_DELAY_GetuS(); + + // Get roll and pitch angles, calculate combined angle, and begin + // the general algorithm. + // Example: 45 degrees roll plus 45 degrees pitch = 60 degrees + // Do it every 8th iteration to save CPU. if ((time != previous_time && calc_count++ >= 8) || previous_time_valid == false) { - float angle; + float angle, angle_unmodified; calc_count = 0; // spherical right triangle - // 0 <= acosf() <= Pi(180) - angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch))); + // 0.0 <= angle <= 180.0 + angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) + * cos_lookup_deg(attitudeState.Pitch))); - // combined bank angle change rate in degrees per second - // rate is currently calculated over the most recent CALCCOUNT loops - // it may be that the angle doesn't change at all between two consecutive passes - // because the sensors aren't read that often - // keeping the sign for rate is important, it can be negative + // Calculate rate as a combined (roll and pitch) bank angle + // change; in degrees per second. Rate is calculated over the + // most recent 8 loops through stabilization. We could have + // asked the gyros. This is probably cheaper. if (previous_time_valid) { float rate; - // 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); - } - // 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 + // Keeping the sign for rate is important. It can be negative. + rate = (angle - previous_angle) / ((float) (time - previous_time) / 1000000.0f); - // there is only one transition and the high power level for it is either + // 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: + // = 1/fabsf(cos(angle)) * current thrust + // = max power factor * current thrust // = full thrust - // = max power factor - // = 1/fabsf(cos(angle)) - // OK, you could say there are two transitions in 360 degrees (90 and 270) + // You can hit it with angle either increasing or decreasing + // (rate positive or negative). + // Thrust is never boosted for negative values of + // actuatorDesired.Thrust + // When the aircraft is upright, thrust is always boosted + // for positive values of actuatorDesired.Thrust + // When the aircraft is inverted, thrust is sometimes + // boosted or reversed (or combinations thereof) or zeroed + // for positive values of actuatorDesired.Thrust + // It depends on the inverted power switch mode. + // Of course, you can set MaxPowerFactor to 1.0 to + // effectively disable boost. + + // Only perform boost calculations for stick values > 0.0 if (actuatorDesired.Thrust > 0.0f) { - float max_thrust; - // calculate the max positive thrust at the transition - // <5% t<5 - // 5.01% 19.3 - // 10% 38.6 - // 50% max - // 100% max + float thrust; + thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust); /* @@ -654,36 +676,70 @@ No (0 <= mint <= 49 plus -51 -> -100) // determine if we are in range of the transition -//CP helis go -max to +max and have min set to 0 and max to +1 - -//min_thrust is defined to be where the power goes when inverted - - // calculate the actual proportion of change in thrust + // given the thrust at max_angle and actuatorDesired.Thrust + // (typically close to 1.0), change variable 'thrust' to + // be the proportion of the largest thrust change possible + // that occurs when going into inverted mode. + // Example: 'thrust' is 0.8 A quad has min_thrust set + // to 0.05 The difference is 0.75. The largest possible + // difference with this setup is 0.9 - 0.05 = 0.85, so + // the proportion is 0.75/0.85 + // That is nearly a full throttle stroke. switch (cruise_control_inverted_power_switch) { + // reversed and boosted with servo reversing on pitch and yaw too case -3: + // reversed and boosted case -2: - // CP heli case, stroke is max to -max - thrust = (thrust + thrust) / cruise_control_thrust_difference; + // CP heli case, stroke is max to min + thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust)) / cruise_control_thrust_difference; break; + // reversed, but not boosted case -1: // CP heli case, stroke is max to -stick thrust = (thrust + CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference; break; + // zeroed case 0: // normal multi-copter case, stroke is max to zero // technically max to constant min_thrust // can be used by CP - thrust = (thrust - cruise_control_min_thrust) / cruise_control_thrust_difference; + thrust = (thrust - CruiseControlLimitThrust(0.0f)) / cruise_control_thrust_difference; +//should this be changed to "max to 0.0f" +// that would mean min is only used for enable +// and that a large min, like 0.5 would make delay inaccurate +//that would allow CP to set min to -0.5 like for mode=1or2? + +//do we need two settings? min_inverted_thrust and min_enabled_thrust + //shit, cruise_control_thrust_difference is a problem, maybe remove it //thrust_difference is always max-min, and now for CP it needs to be max+max here //maybe not a problem since we don't limit thrust on the negative side any more //CP: max=1.0, min=-1.0 diff=2.0 -//MR: max=0.9, min=0.1 diff=0.8 +//CP: max=1.0, min=-0.1 diff=1.1 +//CP: max=1.0, min= 0.0 diff=1.0 +//CP: max=1.0, min= 0.1 diff=0.9 +//MR: max=0.9, min= 0.1 diff=0.8 +/* +as long as the calculated diff is <= diff it is OK +- that means that stroke must always be to min or smaller magnitude +- e.g. min=-0.5 stroke can't go to -1.0 +- so -factor*throttle must be limited to -0.5 +- but throttle alone can got to -1.0 +maybe we need a CP flag +some CP combinations are dangerous? +do not allow thrust to change direction with a small stick change +min=-0.5 + +Hmmm stroke is max to min, except that we haven't gotten to max yet +- it is really current to min (with current taken from the first time the transition was in range) +*/ break; + // unreversed, unboosted case 1: // simply turn off boost, stroke is max to +stick thrust = (thrust - CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference; break; + // unreversed, boosted case 2: // CP heli case, no transition, stroke is zero thrust = 0.0f; @@ -697,6 +753,10 @@ No (0 <= mint <= 49 plus -51 -> -100) // 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) { +// this may need rework in case max_angle is close to 0 or 180 + +// wrong, need stuff below for factor? above / below transition and AtoF doesn't do transition +// where is the transition angle taken into account // 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 @@ -704,7 +764,6 @@ No (0 <= mint <= 49 plus -51 -> -100) angle -= 0.02f; } } - factor = CruiseControlAngleToFactor(angle); } else { // if thrust > 0 set factor from angle; else factor = 1.0f; @@ -746,7 +805,7 @@ inverted yaw/pitch reverse: off, on } // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone previous_time = time; previous_time_valid = true; - previous_angle = angle; + previous_angle = angle_unmodified; } // every 8th time // don't touch thrust if it's less than min_thrust @@ -968,8 +1027,8 @@ static float CruiseControlLimitThrust(float thrust) // limit to user specified absolute max thrust if (thrust > cruise_control_max_thrust) { thrust = cruise_control_max_thrust; -// } else if (thrust < cruise_control_min_thrust) { -// thrust = cruise_control_min_thrust; + } else if (thrust < cruise_control_min_thrust) { + thrust = cruise_control_min_thrust; } return (thrust); } From 2fa8bc031a7c339cc6a6a2308595079a94f9f1b1 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Sun, 13 Apr 2014 22:24:57 +0200 Subject: [PATCH 04/67] First try --- .../diagrams/default/system-health.svg | 183 ++++++++++++++---- 1 file changed, 142 insertions(+), 41 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 914709810..91e8e9588 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -2,6 +2,7 @@ @@ -59,6 +60,14 @@ + + + + y="407.2244" + x="559.82343" + height="14.671674" + width="14.450732" + id="PathPlan" + style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.86811024;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" + inkscape:label="#rect6261" /> + + + + + - - - - - - Airspeed + + + + + + + + + Test + Path Date: Thu, 17 Apr 2014 12:38:53 +0200 Subject: [PATCH 05/67] OP-1303 Add PathPlan Alarm & SystemConfigAlarm, interface refurbishment --- .../diagrams/default/system-health.svg | 5099 +++++++---------- 1 file changed, 2195 insertions(+), 2904 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 91e8e9588..ee5ac3c2c 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -2,7 +2,6 @@ + inkscape:guide-bbox="true" + inkscape:snap-global="false"> + - - - - - - - + + + + + + + + @@ -652,2937 +689,2191 @@ inkscape:groupmode="layer" inkscape:label="Background"> - - - - - - - - - - - + style="fill:#483737;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.00617588;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" /> + style="fill:#241c1c;fill-opacity:1;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + id="rect4550-8-7-82" + width="10.470912" + height="8.5405388" + x="533.97754" + y="411.38937" + ry="1" /> + + width="11.450287" + height="44.10326" + x="500.16193" + y="377.17593" + ry="1" + inkscape:label="#rect4550" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-7" + width="37.799622" + height="13.255064" + x="539.24451" + y="395.2457" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-4" + width="63.468098" + height="12.839675" + x="513.57617" + y="381.38202" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-0" + width="91.066727" + height="13.183137" + x="499.96637" + y="346.05209" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-0-9" + width="91.019348" + height="13.183137" + x="499.96637" + y="360.26038" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-7-4" + width="24.646557" + height="13.221018" + x="513.58087" + y="395.29047" + ry="0" /> - - - - - - + width="10.470912" + height="8.5405388" + x="518.78839" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8" /> - + width="10.470912" + height="8.5405388" + x="530.5553" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-7" /> + + width="10.470912" + height="8.5405388" + x="554.08905" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-15" /> + style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline" + id="I2C" + width="10.470912" + height="8.5405388" + x="565.8559" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-2" /> + System HEALTH + Sensors + Auto + Misc. + Link + Power + Syst. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + style="fill:none;stroke:#000000;stroke-width:1.00617588;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + id="rect4795" + width="95.066467" + height="79.056633" + x="0.25920519" + y="0.25921404" + ry="1" /> + PathFollow + PathPlan + Attitude + Stabilization + GPS + Sensors + Airspeed + Baro + CPU + Event + Memory + Stack + I2C + Config. + Boot + Telemetry + Battery + FlightTime + INPUT (Receiver) + OUTPUT (Servos) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Boot Fault - Battery - Sensors - Airspeed - - - - - - - - - Test - Path + transform="translate(-497.66563,-344.28037)" + style="fill:none;stroke:#ffffff;stroke-width:0.53149605;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + id="Actuator-7" + width="11.450287" + height="44.10326" + x="578.91998" + y="377.17593" + ry="1" + inkscape:label="#rect4550-4" /> + + + + + + + + + + + + + + + + + + + + + + From 30fb96c39f2c7ff3eaefddf5c7cf97807c790479 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Wed, 23 Apr 2014 15:49:17 +0200 Subject: [PATCH 06/67] OP-1303 Add PathPlan and SystemConfig Alarm (with correct red cross and labels in capital) --- .../diagrams/default/system-health.svg | 555 +++++++++--------- 1 file changed, 277 insertions(+), 278 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index ee5ac3c2c..aea88b5a5 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -27,14 +27,14 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="5.2771065" - inkscape:cx="72.653" - inkscape:cy="42.107801" - inkscape:current-layer="layer1" + inkscape:zoom="7.4629555" + inkscape:cx="49.128572" + inkscape:cy="35.389159" + inkscape:current-layer="layer34" id="namedview3608" showgrid="true" - inkscape:window-width="1366" - inkscape:window-height="712" + inkscape:window-width="1600" + inkscape:window-height="831" inkscape:window-x="-4" inkscape:window-y="-4" inkscape:window-maximized="1" @@ -60,6 +60,10 @@ + @@ -750,8 +754,8 @@ id="rect4358-0-9" width="91.019348" height="13.183137" - x="499.96637" - y="360.26038" + x="500.06686" + y="360.27713" ry="0" /> System HEALTH + y="379.36826">SYSTEM HEALTH + ry="1" + inkscape:label="#rect4550-8-1-4-21" /> + inkscape:label="#g3406"> + d="M 7.9615204,18.056455 26.210275,26.874464" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19737208;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="M 7.9364397,26.848957 26.252147,18.055286" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19791019;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + + + + + + + inkscape:label="#g3422"> + d="M 20.794895,53.039633 39.08945,61.825066" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.1966573;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="M 20.83573,61.847333 39.066764,53.07372" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19377422;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3410"> + d="M 3.4567237,76.822428 12.990338,33.097762" + style="fill:none;stroke:#ff0000;stroke-width:0.9785496px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + d="M 3.4803745,33.071658 12.972435,76.831784" + style="fill:none;stroke:#ff0000;stroke-width:0.9703486px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + inkscape:label="#g3434"> + d="m 82.126214,76.801116 9.72548,-43.665292" + style="fill:none;stroke:#ff0000;stroke-width:0.98767602px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + d="m 82.214671,33.124278 9.60456,43.671634" + style="fill:none;stroke:#ff0000;stroke-width:0.97509456px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> + inkscape:label="#g3426"> + d="m 46.502131,52.910887 14.610607,9.14342" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.09097731;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="M 46.442675,61.953487 61.145921,52.934252" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.08697307;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3430"> + d="m 63.233236,52.875846 14.63044,9.079506" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.08789504;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="m 63.192544,61.969763 14.647987,-9.06903" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.08791935;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3414"> + d="m 20.93643,38.821072 12.493301,9.22163" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01262045;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="M 20.937064,48.026792 33.498002,38.872801" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01214695;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3418"> + d="m 35.705761,38.803039 12.45607,9.277316" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01467943;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="M 35.66424,48.062824 48.169858,38.865363" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.01231074;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3382"> + d="M 7.9267385,3.9562181 26.225281,12.757016" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19783366;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="M 7.9328239,12.666843 26.286235,3.9527152" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19370687;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3442"> + d="M 33.20487,75.243494 43.009818,67.67709" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80505109;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="m 33.214298,67.65316 9.826498,7.608655" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81616998;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3454"> + d="m 68.473832,75.266428 9.874503,-7.54121" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80655557;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="m 68.55267,67.640417 9.780922,7.610454" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81437129;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3450"> + d="m 56.728611,75.196387 9.852857,-7.519565" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80451399;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="m 56.761123,67.664132 9.82824,7.574867" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81442791;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3446"> + d="m 45.020244,75.290833 9.74566,-7.613709" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.80511868;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="m 45.015735,67.662945 9.78324,7.565398" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.81205326;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + inkscape:label="#g3438"> + d="m 21.425298,75.227504 9.844871,-7.557269" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.8062014;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + d="M 21.451681,67.664449 31.2556,75.243937" + style="fill:#ff0000;stroke:#ff0000;stroke-width:0.8136676;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> PathFollow + id="tspan4289" + x="67.606667" + y="8.3444462">PATH PathPlan + id="tspan4291" + x="93.860985" + y="8.3444462">PLAN Attitude + id="tspan3458" + x="10.759087" + y="8.3200321">ATTITUDE Stabilization + id="tspan4287" + x="41.377663" + y="8.3434696">STAB GPS + x="16.329399" + y="20.115072">GPS Sensors + id="tspan4319" + x="37.037819" + y="20.115072">SENSORS Airspeed + id="tspan4321" + x="62.913303" + y="20.115072">AIRSPEED Baro + id="tspan4295" + x="93.336571" + y="20.116049">BARO CPU + x="29.670021" + y="58.514309">CPU Event + id="tspan4311" + x="71.622093" + y="58.514309">EVENT Memory + id="tspan3462" + x="43.505878" + y="58.514309">MEM. Stack + id="tspan4309" + x="56.846539" + y="58.513577">STACK I2C Config. + id="tspan4317" + x="26.501106" + y="37.561607">CONF. Boot + id="tspan4313" + x="44.544518" + y="37.561607">BOOT Telemetry + id="tspan4315" + x="28.787336" + y="49.196098">TELEM. Battery + id="tspan4303" + x="58.840435" + y="49.196095">BATT. FlightTime + id="tspan4305" + x="80.110184" + y="49.196095">TIME INPUT (Receiver) + x="-82.369514" + y="7.8694248">INPUT (Receiver) OUTPUT (Servos) + x="-82.874397" + y="73.338943">OUTPUT (Servos) + style="display:none"> @@ -2831,7 +2830,7 @@ sodipodi:cy="35.07505" sodipodi:rx="0.5" sodipodi:ry="0.5" - d="m 14,35.07505 a 0.5,0.5 0 1 1 -2.75e-4,-0.01658" + d="m 14,35.07505 c 0,0.276143 -0.223858,0.5 -0.5,0.5 -0.276142,0 -0.5,-0.223857 -0.5,-0.5 0,-0.276142 0.223858,-0.5 0.5,-0.5 0.269688,0 0.490781,0.21388 0.499725,0.483419" sodipodi:start="0" sodipodi:end="6.2500167" sodipodi:open="true" @@ -2864,7 +2863,7 @@ sodipodi:cy="35.07505" sodipodi:rx="0.5" sodipodi:ry="0.5" - d="m 14,35.07505 a 0.5,0.5 0 1 1 -2.75e-4,-0.01658" + d="m 14,35.07505 c 0,0.276143 -0.223858,0.5 -0.5,0.5 -0.276142,0 -0.5,-0.223857 -0.5,-0.5 0,-0.276142 0.223858,-0.5 0.5,-0.5 0.269688,0 0.490781,0.21388 0.499725,0.483419" sodipodi:start="0" sodipodi:end="6.2500167" sodipodi:open="true" From aea3ea0f00a85f135bdc8137262474fc0e5b31f8 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Fri, 25 Apr 2014 10:06:05 +0200 Subject: [PATCH 07/67] OP-1303 correct Airspeed-Error alarm --- .../openpilotgcs/diagrams/default/system-health.svg | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index aea88b5a5..fdf08b021 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -29,12 +29,12 @@ inkscape:pageshadow="2" inkscape:zoom="7.4629555" inkscape:cx="49.128572" - inkscape:cy="35.389159" - inkscape:current-layer="layer34" + inkscape:cy="46.308281" + inkscape:current-layer="foreground" id="namedview3608" showgrid="true" - inkscape:window-width="1600" - inkscape:window-height="831" + inkscape:window-width="1366" + inkscape:window-height="712" inkscape:window-x="-4" inkscape:window-y="-4" inkscape:window-maximized="1" @@ -1992,10 +1992,10 @@ - - 12 - - - 12 - - - 12 - - + 12 @@ -161,16 +143,7 @@ - - 12 - - - 12 - - - 12 - - + 12 @@ -269,16 +242,7 @@ - - 12 - - - 12 - - - 12 - - + 12 @@ -457,16 +421,7 @@ Flight Mode Switch Settings - - 0 - - - 0 - - - 0 - - + 0 @@ -546,21 +501,12 @@ 0 0 - 768 - 742 + 774 + 753 - - 12 - - - 12 - - - 12 - - + 12 @@ -587,7 +533,7 @@ Stabilization Modes Configuration - + 9 @@ -626,6 +572,51 @@ + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + + 120 + 20 + + + + + 16777215 + 20 + + + + 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)); +color: rgb(255, 255, 255); +border-radius: 5; +font: bold 12px; +margin:1px; + + + Thrust + + + Qt::AlignCenter + + + @@ -684,7 +675,7 @@ margin:1px; - + Qt::Horizontal @@ -792,16 +783,7 @@ margin:1px; QFrame::Raised - - 1 - - - 1 - - - 1 - - + 1 @@ -843,16 +825,7 @@ margin:1px; QFrame::Raised - - 1 - - - 1 - - - 1 - - + 1 @@ -894,16 +867,7 @@ margin:1px; QFrame::Raised - - 1 - - - 1 - - - 1 - - + 1 @@ -936,6 +900,48 @@ margin:1px; + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 1 + + + + + + 0 + 0 + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + @@ -973,23 +979,14 @@ margin:1px; Flight Mode Switch Positions - - 9 - - - 9 - - - 9 - - + 9 6 - + 80 @@ -1078,7 +1075,7 @@ channel value for each flight mode. - + 0 0 @@ -1090,7 +1087,7 @@ channel value for each flight mode. - Avoid "Manual" for multirotors! + Avoid "Manual" for multirotors! Never select "Altitude", "VelocityControl" or "CruiseControl" on a fixed wing! Qt::AlignCenter @@ -1116,22 +1113,6 @@ channel value for each flight mode. - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 10 - 20 - - - - @@ -1164,35 +1145,6 @@ channel value for each flight mode. - - - - - 105 - 20 - - - - - 16777215 - 20 - - - - 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)); -color: rgb(255, 255, 255); -border-radius: 5; -font: bold 12px; -margin:1px; - - - Cruise Control - - - Qt::AlignCenter - - - @@ -1229,294 +1181,6 @@ margin:1px; - - - - - 30 - 0 - - - - - 16777215 - 16777215 - - - - - 6 - - - 1 - - - 1 - - - 1 - - - 1 - - - - - - 0 - 0 - - - - - 0 - 20 - - - - - 16777215 - 20 - - - - - 75 - true - - - - <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #1.</p></body></html> - - - Qt::RightToLeft - - - - - - - objname:StabilizationSettings - fieldname:CruiseControlFlightModeSwitchPosEnable - index:0 - haslimits:no - scale:1 - - - - - - - - - 0 - 20 - - - - - 16777215 - 20 - - - - - 75 - true - - - - <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #2.</p></body></html> - - - Qt::RightToLeft - - - - - - - objname:StabilizationSettings - fieldname:CruiseControlFlightModeSwitchPosEnable - index:1 - haslimits:no - scale:1 - - - - - - - - - 0 - 20 - - - - - 16777215 - 20 - - - - - 75 - true - - - - <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #3.</p></body></html> - - - Qt::RightToLeft - - - - - - - objname:StabilizationSettings - fieldname:CruiseControlFlightModeSwitchPosEnable - index:2 - haslimits:no - scale:1 - - - - - - - - false - - - - 0 - 20 - - - - - 16777215 - 20 - - - - - 75 - true - - - - <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #4.</p></body></html> - - - Qt::RightToLeft - - - - - - - objname:StabilizationSettings - fieldname:CruiseControlFlightModeSwitchPosEnable - index:3 - haslimits:no - scale:1 - - - - - - - - false - - - - 0 - 20 - - - - - 16777215 - 20 - - - - - 75 - true - - - - <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #5.</p></body></html> - - - Qt::RightToLeft - - - - - - - objname:StabilizationSettings - fieldname:CruiseControlFlightModeSwitchPosEnable - index:4 - haslimits:no - scale:1 - - - - - - - - false - - - - 0 - 20 - - - - - 16777215 - 20 - - - - - 75 - true - - - - <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #6.</p></body></html> - - - Qt::RightToLeft - - - - - - - objname:StabilizationSettings - fieldname:CruiseControlFlightModeSwitchPosEnable - index:5 - haslimits:no - scale:1 - - - - - - - @@ -1538,21 +1202,12 @@ margin:1px; QFrame::Raised - - 1 - - - 1 - - - 1 - - - 1 - 10 + + 1 + @@ -1770,16 +1425,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread - - 1 - - - 1 - - - 1 - - + 1 @@ -1867,7 +1513,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread - + 75 @@ -1881,16 +1527,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread - - 1 - - - 1 - - - 1 - - + 1 @@ -2075,16 +1712,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread Arming Settings - - 0 - - - 0 - - - 0 - - + 0 @@ -2164,21 +1792,12 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread 0 0 - 768 - 742 + 774 + 753 - - 12 - - - 12 - - - 12 - - + 12 diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 9221d7393..d3e8656da 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -328,8 +328,8 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration() data.FlightModePosition[0] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1; data.FlightModePosition[1] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2; data.FlightModePosition[2] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED3; - data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD; - data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_POSITIONHOLD; + data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL; + data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL; data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL; modeSettings->setData(data); addModifiedObject(modeSettings, tr("Writing flight mode settings 1/2")); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 9c6dc8d67..f5db470dd 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -62,6 +62,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/overosyncstats.h \ $$UAVOBJECT_SYNTHETICS/overosyncsettings.h \ $$UAVOBJECT_SYNTHETICS/systemsettings.h \ + $$UAVOBJECT_SYNTHETICS/stabilizationstatus.h \ $$UAVOBJECT_SYNTHETICS/stabilizationsettings.h \ $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.h \ $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.h \ @@ -163,6 +164,7 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/overosyncstats.cpp \ $$UAVOBJECT_SYNTHETICS/overosyncsettings.cpp \ $$UAVOBJECT_SYNTHETICS/systemsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/stabilizationstatus.cpp \ $$UAVOBJECT_SYNTHETICS/stabilizationsettings.cpp \ $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.cpp \ $$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.cpp \ diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index b9b6aa211..e2ea535f6 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -6,20 +6,38 @@ + elementnames="Roll,Pitch,Yaw,Thrust" + options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,Altitude,VerticalVelocity,CruiseControl" + defaultvalue="Attitude,Attitude,AxisLock,CruiseControl" + limits="%NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\ + %0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:Altitude:VerticalVelocity,\ + %0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:Altitude:VerticalVelocity;" + /> + elementnames="Roll,Pitch,Yaw,Thrust" + options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,Altitude,VerticalVelocity,CruiseControl" + defaultvalue="Attitude,Attitude,Rate,CruiseControl" + limits="%NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\ + %0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:Altitude:VerticalVelocity,\ + %0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:Altitude:VerticalVelocity;" + /> + elementnames="Roll,Pitch,Yaw,Thrust" + options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,Altitude,VerticalVelocity,CruiseControl" + defaultvalue="Rate,Rate,Rate,None" + limits="%NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:Altitude:VerticalVelocity:CruiseControl; \ + %NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\ + %0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:Altitude:VerticalVelocity,\ + %0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:Altitude:VerticalVelocity;" + /> @@ -27,32 +45,32 @@ units="" type="enum" elements="6" - options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI" - defaultvalue="Stabilized1,Stabilized2,Stabilized3,AltitudeHold,AltitudeVario,Manual" + options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI" + defaultvalue="Stabilized1,Stabilized2,Stabilized3,Manual,Manual,Manual" limits="\ - %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ \ - %0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/> + %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ + %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI"/> diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index a7009f4c4..f24408c5c 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 70bdfe9c0..38279a96b 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -5,9 +5,8 @@ - - - + + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index d9702c75e..51d6497bd 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -37,7 +37,6 @@ - diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml new file mode 100644 index 000000000..f68f8e30b --- /dev/null +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -0,0 +1,28 @@ + + + Contains status information to control submodules for stabilization. + + + + + Roll + Pitch + Yaw + Thrust + + + + + Roll + Pitch + Yaw + Thrust + + + + + + + + + From e9d1a2af4bd2f63f19f3f13dd57d7298e8b93561 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 26 Apr 2014 17:31:20 +0200 Subject: [PATCH 09/67] OP-1309 get rid of this horrible bound() function code duplication throughout entire flight code and put it into libraries/math --- flight/libraries/math/mathmisc.c | 49 ++++++++++++ flight/libraries/math/mathmisc.h | 39 +++++++++ flight/libraries/math/pid.c | 21 +---- flight/modules/CameraStab/camerastab.c | 15 +--- .../fixedwingpathfollower.c | 77 +++++++----------- flight/modules/Stabilization/stabilization.c | 67 +++++++--------- flight/modules/Stabilization/virtualflybar.c | 18 +---- .../VtolPathFollower/vtolpathfollower.c | 80 ++++++++----------- .../coptercontrol/firmware/inc/openpilot.h | 1 + .../oplinkmini/firmware/inc/openpilot.h | 1 + .../boards/osd/firmware/inc/openpilot.h | 1 + .../revolution/firmware/inc/openpilot.h | 1 + .../boards/revoproto/firmware/inc/openpilot.h | 1 + .../targets/boards/simposix/firmware/Makefile | 1 + .../boards/simposix/firmware/inc/openpilot.h | 1 + make/apps-defs.mk | 1 + 16 files changed, 197 insertions(+), 177 deletions(-) create mode 100644 flight/libraries/math/mathmisc.c create mode 100644 flight/libraries/math/mathmisc.h diff --git a/flight/libraries/math/mathmisc.c b/flight/libraries/math/mathmisc.c new file mode 100644 index 000000000..d54d18908 --- /dev/null +++ b/flight/libraries/math/mathmisc.c @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Reuseable math functions + * @{ + * + * @file mathmisc.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Reuseable math functions + * + * @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 + */ + + +// returns min(boundary1,boundary2) if valmax(boundary1,boundary2) +// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2) +float boundf(float val, float boundary1, float boundary2) +{ + if (boundary1 > boundary2) { + float tmp = boundary2; + boundary2 = boundary1; + boundary1 = tmp; + } + if (!(val >= boundary1)) { + val = boundary1; + } + if (!(val <= boundary2)) { + val = boundary2; + } + return val; +} diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h new file mode 100644 index 000000000..c989ccffe --- /dev/null +++ b/flight/libraries/math/mathmisc.h @@ -0,0 +1,39 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Reuseable math functions + * @{ + * + * @file mathmisc.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Reuseable math functions + * + * @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 + */ + +#ifndef MATHMISC_H +#define MATHMISC_H + +// returns min(boundary1,boundary2) if valmax(boundary1,boundary2) +// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2) +float boundf(float val, float boundary1, float boundary2); + +#endif /* MATHMISC_H */ diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index 450b86cbb..c9bcf1494 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -30,11 +30,9 @@ #include "openpilot.h" #include "pid.h" +#include #include -// ! Private method -static float bound(float val, float range); - // ! Store the shared time constant for the derivative cutoff. static float deriv_tau = 7.9577e-3f; @@ -52,7 +50,7 @@ float pid_apply(struct pid *pid, const float err, float dT) { // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (pid->i * dT * 1000.0f); - pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f); // Calculate DT1 term float diff = (err - pid->lastErr); @@ -84,7 +82,7 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f); - pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f); // Calculate DT1 term, float dterm = 0; @@ -142,16 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) pid->d = d; pid->iLim = iLim; } - -/** - * Bound input value between limits - */ -static float bound(float val, float range) -{ - if (val < -range) { - val = -range; - } else if (val > range) { - val = range; - } - return val; -} diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index d6e9aed26..d57ec8bb3 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -79,7 +79,6 @@ static struct CameraStab_data { // Private functions static void attitudeUpdated(UAVObjEvent *ev); -static float bound(float val, float limit); #ifdef USE_GIMBAL_FF static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab); @@ -186,8 +185,9 @@ static void attitudeUpdated(UAVObjEvent *ev) input_rate = accessory.AccessoryVal * cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i]; if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) { - csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, - cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]); + csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis, + -cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i], + cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]); } break; default: @@ -229,7 +229,7 @@ static void attitudeUpdated(UAVObjEvent *ev) // bounding for elevon mixing occurs on the unmixed output // to limit the range of the mixed output you must limit the range // of both the unmixed pitch and unmixed roll - float output = bound((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], 1.0f); + float output = boundf((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], -1.0f, 1.0f); // set output channels switch (i) { @@ -282,13 +282,6 @@ static void attitudeUpdated(UAVObjEvent *ev) } } -float bound(float val, float limit) -{ - return (val > limit) ? limit : - (val < -limit) ? -limit : - val; -} - #ifdef USE_GIMBAL_FF void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab) { diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 9a2e52651..65cb9bffa 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -85,7 +85,6 @@ static void updatePathVelocity(); static uint8_t updateFixedDesiredAttitude(); static void updateFixedAttitude(); static void airspeedStateUpdatedCb(UAVObjEvent *ev); -static float bound(float val, float min, float max); /** * Initialise the module, called on startup @@ -277,9 +276,9 @@ static void updatePathVelocity() case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - bound(progress.fractional_progress, 0, 1); + boundf(progress.fractional_progress, 0, 1); altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * - bound(progress.fractional_progress, 0, 1); + boundf(progress.fractional_progress, 0, 1); break; } // make sure groundspeed is not zero @@ -427,15 +426,15 @@ static uint8_t updateFixedDesiredAttitude() // Desired ground speed groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias, - fixedwingpathfollowerSettings.HorizontalVelMin, - fixedwingpathfollowerSettings.HorizontalVelMax); + indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias, + fixedwingpathfollowerSettings.HorizontalVelMin, + fixedwingpathfollowerSettings.HorizontalVelMax); // Airspeed error airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; // Vertical speed error - descentspeedDesired = bound( + descentspeedDesired = boundf( velocityDesired.Down, -fixedwingpathfollowerSettings.VerticalVelMax, fixedwingpathfollowerSettings.VerticalVelMax); @@ -473,14 +472,14 @@ static uint8_t updateFixedDesiredAttitude() */ // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant. if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) { - powerIntegral = bound(powerIntegral + -descentspeedError * dT, - -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki, - fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki - ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); + powerIntegral = boundf(powerIntegral + -descentspeedError * dT, + -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki, + fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki + ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); } else { powerIntegral = 0; } // Compute the cross feed from vertical speed to pitch, with saturation - float speedErrorToPowerCommandComponent = bound( + float speedErrorToPowerCommandComponent = boundf( (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp, -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max, fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max @@ -497,9 +496,9 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerStatus.Command.Power = powerCommand; // set thrust - stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand, - fixedwingpathfollowerSettings.ThrustLimit.Min, - fixedwingpathfollowerSettings.ThrustLimit.Max); + stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand, + fixedwingpathfollowerSettings.ThrustLimit.Min, + fixedwingpathfollowerSettings.ThrustLimit.Max); // Error condition: plane cannot hold altitude at current speed. fixedwingpathfollowerStatus.Errors.Lowpower = 0; @@ -529,16 +528,16 @@ static uint8_t updateFixedDesiredAttitude() if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) { // Integrate with saturation - airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT, - -fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki, - fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki); + airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT, + -fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki, + fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki); } // Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp, - -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max, - fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max - ); + float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp, + -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max, + fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max + ); // Compute the pitch command as err*Kp + errInt*Ki + X_feed. pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp @@ -549,9 +548,9 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt; fixedwingpathfollowerStatus.Command.Speed = pitchCommand; - stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand, - fixedwingpathfollowerSettings.PitchLimit.Min, - fixedwingpathfollowerSettings.PitchLimit.Max); + stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand, + fixedwingpathfollowerSettings.PitchLimit.Min, + fixedwingpathfollowerSettings.PitchLimit.Max); // Error condition: high speed dive fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0; @@ -606,9 +605,9 @@ static uint8_t updateFixedDesiredAttitude() courseError -= 360.0f; } - courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki, - -fixedwingpathfollowerSettings.CoursePI.ILimit, - fixedwingpathfollowerSettings.CoursePI.ILimit); + courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki, + -fixedwingpathfollowerSettings.CoursePI.ILimit, + fixedwingpathfollowerSettings.CoursePI.ILimit); courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp + courseIntegral); @@ -616,10 +615,10 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral; fixedwingpathfollowerStatus.Command.Course = courseCommand; - stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral + - courseCommand, - fixedwingpathfollowerSettings.RollLimit.Min, - fixedwingpathfollowerSettings.RollLimit.Max); + stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral + + courseCommand, + fixedwingpathfollowerSettings.RollLimit.Min, + fixedwingpathfollowerSettings.RollLimit.Max); // TODO: find a check to determine loss of directional control. Likely needs some check of derivative @@ -642,20 +641,6 @@ static uint8_t updateFixedDesiredAttitude() return result; } - -/** - * Bound input value between limits - */ -static float bound(float val, float min, float max) -{ - if (val < min) { - val = min; - } else if (val > max) { - val = max; - } - return val; -} - static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 88eeeda3e..9b72e54ca 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -116,7 +116,6 @@ static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_F // Private functions static void stabilizationTask(void *parameters); -static float bound(float val, float range); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent *ev); static void BankUpdatedCb(UAVObjEvent *ev); @@ -342,11 +341,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Store to rate desired variable for storing to UAVO rateDesiredAxis[i] = - bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); + boundf(stabDesiredAxis[i], -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); break; @@ -358,12 +357,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], - cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]); + rateDesiredAxis[i] = boundf(rateDesiredAxis[i], + -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i], + cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); break; @@ -382,7 +382,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute what Rate mode would give for this stick angle's rate // Save Rate's rate in a temp for later merging with Attitude's rate float rateDesiredAxisRate; - rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f) + rateDesiredAxisRate = boundf(stabDesiredAxis[i], -1.0f, 1.0f) * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]; // Compute what Attitude mode would give for this stick angle's rate @@ -398,9 +398,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute the outer loop just like Attitude mode does float rateDesiredAxisAttitude; rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT); - rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude, - cast_struct_to_array(stabBank.ManualRate, - stabBank.ManualRate.Roll)[i]); + rateDesiredAxisAttitude = boundf(rateDesiredAxisAttitude, + -cast_struct_to_array(stabBank.ManualRate, + stabBank.ManualRate.Roll)[i], + cast_struct_to_array(stabBank.ManualRate, + stabBank.ManualRate.Roll)[i]); // Compute the weighted average rate desired // Using max() rather than sqrt() for cpu speed; @@ -452,7 +454,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // actuatorDesiredAxis[i] is the weighted average actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); break; } @@ -479,12 +481,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } float weak_leveling = local_error[i] * weak_leveling_kp; - weak_leveling = bound(weak_leveling, weak_leveling_max); + weak_leveling = boundf(weak_leveling, -weak_leveling_max, weak_leveling_max); // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling; actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); break; } @@ -501,26 +503,28 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } else { // For weaker commands or no command simply attitude lock (almost) on no gyro change axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT; - axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); + axis_lock_accum[i] = boundf(axis_lock_accum[i], -max_axis_lock, max_axis_lock); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } - rateDesiredAxis[i] = bound(rateDesiredAxis[i], - cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); + rateDesiredAxis[i] = boundf(rateDesiredAxis[i], + -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], + cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(stabDesiredAxis[i], - cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); + rateDesiredAxis[i] = boundf(stabDesiredAxis[i], + -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], + cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); // Run the relay controller which also estimates the oscillation parameters stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); break; @@ -531,17 +535,18 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute the outer loop like attitude mode rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], - cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]); + rateDesiredAxis[i] = boundf(rateDesiredAxis[i], + -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i], + cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]); // Run the relay controller which also estimates the oscillation parameters stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = boundf(stabDesiredAxis[i], -1.0f, 1.0f); break; default: error = true; @@ -662,20 +667,6 @@ static void ZeroPids(void) } -/** - * Bound input value between limits - */ -static float bound(float val, float range) -{ - if (val < -range) { - return -range; - } else if (val > range) { - return range; - } - return val; -} - - static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index 211b7b864..29d793475 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -40,9 +40,6 @@ static float vbar_integral[MAX_AXES]; static float vbar_decay = 0.991f; -// ! Private methods -static float bound(float val, float range); - int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings) { float gyro_gain = 1.0f; @@ -54,7 +51,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float // Track the angle of the virtual flybar which includes a slow decay vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT; - vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); + vbar_integral[axis] = boundf(vbar_integral[axis], -settings->VbarMaxAngle, settings->VbarMaxAngle); // Command signal can indicate how much to disregard the gyro feedback (fast flips) if (settings->VbarGyroSuppress > 0) { @@ -105,16 +102,3 @@ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) return 0; } - -/** - * Bound input value between limits - */ -static float bound(float val, float range) -{ - if (val < -range) { - val = -range; - } else if (val > range) { - val = range; - } - return val; -} diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 3c65e1b29..c4ee9a8f9 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -101,7 +101,6 @@ static void updatePathVelocity(); static void updateEndpointVelocity(); static void updateFixedAttitude(float *attitude); static void updateVtolDesiredAttitude(bool yaw_attitude); -static float bound(float val, float min, float max); static bool vtolpathfollower_enabled; static void accessoryUpdated(UAVObjEvent *ev); @@ -394,7 +393,7 @@ static void updatePathVelocity() break; case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: - groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1); + groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { groundspeed = 0; } @@ -403,7 +402,7 @@ static void updatePathVelocity() case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); + + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { groundspeed = 0; } @@ -427,14 +426,14 @@ static void updatePathVelocity() velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale; - float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1); + float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1); float downError = altitudeSetpoint - positionState.Down; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, - -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); + downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, + -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); - velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); + velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus pathStatus.error = progress.error; @@ -472,15 +471,15 @@ void updateEndpointVelocity() // Compute desired north command northError = pathDesired.End.North - positionState.North; - northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); + northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, + -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral); eastError = pathDesired.End.East - positionState.East; - eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); + eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, + -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral); // Limit the maximum velocity @@ -494,11 +493,11 @@ void updateEndpointVelocity() velocityDesired.East = eastCommand * scale; downError = pathDesired.End.Down - positionState.Down; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, - -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); + downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, + -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); - velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); + velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); VelocityDesiredSet(&velocityDesired); } @@ -595,18 +594,18 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) // Compute desired north command northError = velocityDesired.North - northVel; - northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.ILimit); + northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, + -vtolpathfollowerSettings.HorizontalVelPID.ILimit, + vtolpathfollowerSettings.HorizontalVelPID.ILimit); northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired east command eastError = velocityDesired.East - eastVel; - eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.ILimit); + eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, + -vtolpathfollowerSettings.HorizontalVelPID.ILimit, + vtolpathfollowerSettings.HorizontalVelPID.ILimit); eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); @@ -615,22 +614,22 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) downError = velocityDesired.Down - downVel; // Must flip this sign downError = -downError; - downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki, - -vtolpathfollowerSettings.VerticalVelPID.ILimit, - vtolpathfollowerSettings.VerticalVelPID.ILimit); + downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki, + -vtolpathfollowerSettings.VerticalVelPID.ILimit, + vtolpathfollowerSettings.VerticalVelPID.ILimit); downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd); - stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1); + stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1); // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // craft should move similarly for 5 deg roll versus 5 deg pitch - stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + - -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + - eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + + -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + + eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) { // For now override thrust with manual control. Disable at your risk, quad goes to China. @@ -692,19 +691,6 @@ static void updateNedAccel() NedAccelSet(&accelData); } -/** - * Bound input value between limits - */ -static float bound(float val, float min, float max) -{ - if (val < min) { - val = min; - } else if (val > max) { - val = max; - } - return val; -} - static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); diff --git a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h index 26cfb9a98..5576baca6 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h @@ -39,6 +39,7 @@ #include #include "alarms.h" +#include /* Global Functions */ void OpenPilotInit(void); diff --git a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h index 89e1e909c..42c876722 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h @@ -40,6 +40,7 @@ #include #include "alarms.h" +#include /* Global Functions */ void OpenPilotInit(void); diff --git a/flight/targets/boards/osd/firmware/inc/openpilot.h b/flight/targets/boards/osd/firmware/inc/openpilot.h index 54e6a5d30..04b62f62e 100644 --- a/flight/targets/boards/osd/firmware/inc/openpilot.h +++ b/flight/targets/boards/osd/firmware/inc/openpilot.h @@ -39,6 +39,7 @@ #include #include "alarms.h" +#include /* Global Functions */ void OpenPilotInit(void); diff --git a/flight/targets/boards/revolution/firmware/inc/openpilot.h b/flight/targets/boards/revolution/firmware/inc/openpilot.h index 89e1e909c..42c876722 100644 --- a/flight/targets/boards/revolution/firmware/inc/openpilot.h +++ b/flight/targets/boards/revolution/firmware/inc/openpilot.h @@ -40,6 +40,7 @@ #include #include "alarms.h" +#include /* Global Functions */ void OpenPilotInit(void); diff --git a/flight/targets/boards/revoproto/firmware/inc/openpilot.h b/flight/targets/boards/revoproto/firmware/inc/openpilot.h index 89e1e909c..42c876722 100644 --- a/flight/targets/boards/revoproto/firmware/inc/openpilot.h +++ b/flight/targets/boards/revoproto/firmware/inc/openpilot.h @@ -40,6 +40,7 @@ #include #include "alarms.h" +#include /* Global Functions */ void OpenPilotInit(void); diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index c6ffd2cc8..bc05d3b69 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -96,6 +96,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c +SRC += $(MATHLIB)/mathmisc.c SRC += $(PIOSCORECOMMON)/pios_task_monitor.c SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c diff --git a/flight/targets/boards/simposix/firmware/inc/openpilot.h b/flight/targets/boards/simposix/firmware/inc/openpilot.h index 89e1e909c..42c876722 100644 --- a/flight/targets/boards/simposix/firmware/inc/openpilot.h +++ b/flight/targets/boards/simposix/firmware/inc/openpilot.h @@ -40,6 +40,7 @@ #include #include "alarms.h" +#include /* Global Functions */ void OpenPilotInit(void); diff --git a/make/apps-defs.mk b/make/apps-defs.mk index 20853c568..27595fa7f 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -106,6 +106,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c +SRC += $(MATHLIB)/mathmisc.c SRC += $(FLIGHTLIB)/printf-stdarg.c ## Modules From 77f2444fa4efc382568e2341d17979fd3698c090 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 26 Apr 2014 20:33:25 +0200 Subject: [PATCH 10/67] OP-1309 renamed stabilization mode NONE to MANUAL --- flight/libraries/sanitycheck.c | 15 +++---- .../fixedwingpathfollower.c | 2 +- .../modules/ManualControl/inc/manualcontrol.h | 6 +-- .../modules/ManualControl/stabilizedhandler.c | 6 +-- flight/modules/Stabilization/stabilization.c | 2 +- .../flightmodesettings.xml | 42 +++++++++---------- .../stabilizationdesired.xml | 2 +- 7 files changed, 38 insertions(+), 37 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index bf0e37014..d46304662 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -213,7 +213,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop // (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"! if (multirotor) { for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) { - if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) { + if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) { return SYSTEMALARMS_ALARM_ERROR; } } @@ -221,7 +221,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop // coptercontrol cannot do altitude holding if (coptercontrol) { - if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDE + if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY ) { return SYSTEMALARMS_ALARM_ERROR; @@ -230,22 +230,23 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop // check that thrust modes are only set to thrust axis for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) { - if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDE + if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY ) { return SYSTEMALARMS_ALARM_ERROR; } } - if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE - || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDE + if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL + || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY )) { return SYSTEMALARMS_ALARM_ERROR; } // Warning: This assumes that certain conditions in the XML file are met. That - // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel - // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE + // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel + // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL + // (this is checked at compile time by static constraint manualcontrol.h) return SYSTEMALARMS_ALARM_OK; } diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 65cb9bffa..d969f556a 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -632,7 +632,7 @@ static uint8_t updateFixedDesiredAttitude() stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 4f5fdcd32..bce5a6724 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -81,7 +81,7 @@ void pathPlannerHandler(bool newinit); */ #define assumptions1 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ @@ -90,7 +90,7 @@ void pathPlannerHandler(bool newinit); #define assumptions3 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ @@ -99,7 +99,7 @@ void pathPlannerHandler(bool newinit); #define assumptions5 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index d7912d936..e98de70d4 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -86,7 +86,7 @@ void stabilizedHandler(bool newinit) } stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax : @@ -98,7 +98,7 @@ void stabilizedHandler(bool newinit) 0; // this is an invalid mode stabilization.Pitch = - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : @@ -120,7 +120,7 @@ void stabilizedHandler(bool newinit) } else { stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax : diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 9b72e54ca..166360921 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -545,7 +545,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) break; - case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: + case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL: actuatorDesiredAxis[i] = boundf(stabDesiredAxis[i], -1.0f, 1.0f); break; default: diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index e2ea535f6..753f4e7e9 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -7,36 +7,36 @@ diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 38279a96b..963dd1440 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + From 33acbc42093e98eaf53be8694ee0832cfcdcc720 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 26 Apr 2014 20:57:39 +0200 Subject: [PATCH 11/67] OP-1309 added new thrust mode default to setup wizard --- .../src/plugins/setupwizard/vehicleconfigurationhelper.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index d3e8656da..0990089a0 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -318,12 +318,15 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration() data.Stabilization1Settings[0] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE; data.Stabilization1Settings[1] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE; data.Stabilization1Settings[2] = FlightModeSettings::STABILIZATION1SETTINGS_AXISLOCK; + data.Stabilization1Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL; data.Stabilization2Settings[0] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE; data.Stabilization2Settings[1] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE; data.Stabilization2Settings[2] = FlightModeSettings::STABILIZATION2SETTINGS_RATE; + data.Stabilization2Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL; data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; + data.Stabilization3Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL; data2.FlightModeNumber = 3; data.FlightModePosition[0] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1; data.FlightModePosition[1] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2; From 3f7c9e3679fd159db45a57b56843da740679d3ff Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 26 Apr 2014 22:37:49 +0200 Subject: [PATCH 12/67] OP-1309 inner stabilization loop in new incarnation - is not used yet until outer loop rewritten as well --- .../modules/Stabilization/inc/cruisecontrol.h | 38 +++ flight/modules/Stabilization/inc/innerloop.h | 38 +++ .../modules/Stabilization/inc/stabilization.h | 11 +- flight/modules/Stabilization/innerloop.c | 231 ++++++++++++++++++ flight/modules/Stabilization/relay_tuning.c | 2 +- flight/modules/Stabilization/stabilization.c | 2 + flight/modules/Stabilization/virtualflybar.c | 8 +- shared/uavobjectdefinition/callbackinfo.xml | 6 + 8 files changed, 330 insertions(+), 6 deletions(-) create mode 100644 flight/modules/Stabilization/inc/cruisecontrol.h create mode 100644 flight/modules/Stabilization/inc/innerloop.h create mode 100644 flight/modules/Stabilization/innerloop.c diff --git a/flight/modules/Stabilization/inc/cruisecontrol.h b/flight/modules/Stabilization/inc/cruisecontrol.h new file mode 100644 index 000000000..a212cd230 --- /dev/null +++ b/flight/modules/Stabilization/inc/cruisecontrol.h @@ -0,0 +1,38 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief cruisecontrol mode + * @note This file implements the logic for a cruisecontrol + * @{ + * + * @file cruisecontrol.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 + */ + +#ifndef CRUISECONTROL_H +#define CRUISECONTROL_H + +float cruisecontrol_apply_factor(float raw); + +#endif /* CRUISECONTROL_H */ diff --git a/flight/modules/Stabilization/inc/innerloop.h b/flight/modules/Stabilization/inc/innerloop.h new file mode 100644 index 000000000..379ecba32 --- /dev/null +++ b/flight/modules/Stabilization/inc/innerloop.h @@ -0,0 +1,38 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief innerloop mode + * @note This file implements the logic for a innerloop + * @{ + * + * @file innerloop.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 + */ + +#ifndef INNERLOOP_H +#define INNERLOOP_H + +void stabilizationInnerloopInit(); + +#endif /* INNERLOOP_H */ diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 396a91e89..dab1a616b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -33,10 +33,19 @@ #ifndef STABILIZATION_H #define STABILIZATION_H -enum { ROLL, PITCH, YAW, MAX_AXES }; +#include int32_t StabilizationInitialize(); +typedef struct { + StabilizationSettingsData settings; + float gyro_alpha; +} StabilizationData; + + +extern StabilizationData stabSettings; + + #endif // STABILIZATION_H /** diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c new file mode 100644 index 000000000..5d49d325b --- /dev/null +++ b/flight/modules/Stabilization/innerloop.c @@ -0,0 +1,231 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Stabilization PID loops in an airframe type independent manner + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" + * @{ + * + * @file rateloop.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Private constants +#define STACK_SIZE_BYTES 256 + +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL + +#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f + +#define AXES 4 + +// Private variables +static DelayedCallbackInfo *callbackHandle; +static struct pid pids[3]; +static float gyro_filtered[3] = { 0, 0, 0 }; +static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; +static PiOSDeltatimeConfig timeval; +static float speedScaleFactor = 1.0f; + +// Private functions +static void stabilizationInnerloopTask(); +static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); +static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); +#ifdef REVOLUTION +static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); +#endif + +void stabilizationInnerloopInit() +{ + RateDesiredInitialize(); + ActuatorDesiredInitialize(); + GyroStateInitialize(); + StabilizationStatusInitialize(); + FlightStatusInitialize(); + StabilizationBankInitialize(); +#ifdef REVOLUTION + AirspeedStateInitialize(); + AirspeedStateConnectCallback(AirSpeedUpdatedCb); +#endif + PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); + + callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES); + GyroStateConnectCallback(GyroStateUpdatedCb); + StabilizationBankConnectCallback(BankUpdatedCb); + BankUpdatedCb(NULL); +} + + +/** + * WARNING! This callback executes with critical flight control priority every + * time a gyroscope update happens do NOT put any time consuming calculations + * in this loop unless they really have to execute with every gyro update + */ +static void stabilizationInnerloopTask() +{ + RateDesiredData rateDesired; + ActuatorDesiredData actuator; + StabilizationStatusInnerLoopData enabled; + FlightStatusControlChainData cchain; + + RateDesiredGet(&rateDesired); + ActuatorDesiredGet(&actuator); + StabilizationStatusInnerLoopGet(&enabled); + FlightStatusControlChainGet(&cchain); + float *rate = &rateDesired.Roll; + float *actuatorDesiredAxis = &actuator.Roll; + int t; + float dT; + dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + + for (t = 0; t < AXES; t++) { + bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); + previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t]; + + if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) { + if (reinit) { + pids[t].iAccumulator = 0; + } + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { + case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR: + stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings); + break; + case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING: + stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit); + break; + case STABILIZATIONSTATUS_INNERLOOP_RATE: + actuatorDesiredAxis[t] = pid_apply_setpoint(&pids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); + break; + case STABILIZATIONSTATUS_INNERLOOP_DIRECT: + default: + actuatorDesiredAxis[t] = rate[t]; + break; + } + } else { + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { + case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL: + actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]); + break; + case STABILIZATIONSTATUS_INNERLOOP_DIRECT: + default: + actuatorDesiredAxis[t] = rate[t]; + break; + } + } + + actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f); + } + if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + ActuatorDesiredSet(&actuator); + } else { + // Force all axes to reinitialize when engaged + for (t = 0; t < AXES; t++) { + previous_mode[t] = 255; + } + } +} + + +static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + GyroStateData gyroState; + + GyroStateGet(&gyroState); + + gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha); + gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha); + gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha); + + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); +} + +static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + StabilizationBankData bank; + + StabilizationBankGet(&bank); + + // Set the roll rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], bank.RollRatePID.Kp, + bank.RollRatePID.Ki, + bank.RollRatePID.Kd, + bank.RollRatePID.ILimit); + + // Set the pitch rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], bank.PitchRatePID.Kp, + bank.PitchRatePID.Ki, + bank.PitchRatePID.Kd, + bank.PitchRatePID.ILimit); + + // Set the yaw rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], bank.YawRatePID.Kp, + bank.YawRatePID.Ki, + bank.YawRatePID.Kd, + bank.YawRatePID.ILimit); +} + +#ifdef REVOLUTION +static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes + AirspeedStateData airspeedState; + + AirspeedStateGet(&airspeedState); + if (stabSettings.settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) { + // feature has been turned off + speedScaleFactor = 1.0f; + } else { + // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2 + speedScaleFactor = boundf((stabSettings.settings.ScaleToAirspeed * stabSettings.settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed), + stabSettings.settings.ScaleToAirspeedLimits.Min, + stabSettings.settings.ScaleToAirspeedLimits.Max); + } +} +#endif + +/** + * @} + * @} + */ diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index 6d5f2c36d..316340f1b 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -63,7 +63,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) portTickType thisTime = xTaskGetTickCount(); - static bool rateRelayRunning[MAX_AXES]; + static bool rateRelayRunning[3]; // This indicates the current estimate of the smoothed error. So when it is high // we are waiting for it to go low. diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 166360921..5159a574e 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -71,6 +71,8 @@ #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f +typedef enum { ROLL = 0, PITCH = 1, YAW = 2, MAX_AXES = 3 } blaenum; + #define MAX_QUEUE_SIZE 1 #if defined(PIOS_STABILIZATION_STACK_SIZE) diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index 29d793475..cc1a284f1 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -37,7 +37,7 @@ #include "stabilizationsettings.h" // ! Private variables -static float vbar_integral[MAX_AXES]; +static float vbar_integral[3]; static float vbar_decay = 0.991f; int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings) @@ -61,15 +61,15 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float // Get the settings for the correct axis switch (axis) { - case ROLL: + case 0: kp = settings->VbarRollPI.Kp; ki = settings->VbarRollPI.Ki; break; - case PITCH: + case 1: kp = settings->VbarPitchPI.Kp; ki = settings->VbarPitchPI.Ki;; break; - case YAW: + case 2: kp = settings->VbarYawPI.Kp; ki = settings->VbarYawPI.Ki; break; diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml index 80015361c..8624867c4 100644 --- a/shared/uavobjectdefinition/callbackinfo.xml +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -6,6 +6,8 @@ EventDispatcher StateEstimation AltitudeHold + Stabilization0 + Stabilization1 PathPlanner0 PathPlanner1 ManualControl @@ -16,6 +18,8 @@ EventDispatcher StateEstimation AltitudeHold + Stabilization0 + Stabilization1 PathPlanner0 PathPlanner1 ManualControl @@ -30,6 +34,8 @@ EventDispatcher StateEstimation AltitudeHold + Stabilization0 + Stabilization1 PathPlanner0 PathPlanner1 ManualControl From 844fcffec7f6cd01e20c1d91ab94bf788f37628c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 27 Apr 2014 07:19:59 +0200 Subject: [PATCH 13/67] OP-1309 added cruisecontrol function library and outer loop template --- flight/modules/Stabilization/cruisecontrol.c | 94 ++++++++++ .../modules/Stabilization/inc/cruisecontrol.h | 4 + flight/modules/Stabilization/inc/outerloop.h | 38 ++++ .../modules/Stabilization/inc/stabilization.h | 10 + flight/modules/Stabilization/innerloop.c | 5 +- flight/modules/Stabilization/outerloop.c | 172 ++++++++++++++++++ flight/modules/Stabilization/stabilization.c | 1 + .../stabilizationstatus.xml | 2 +- 8 files changed, 324 insertions(+), 2 deletions(-) create mode 100644 flight/modules/Stabilization/cruisecontrol.c create mode 100644 flight/modules/Stabilization/inc/outerloop.h create mode 100644 flight/modules/Stabilization/outerloop.c diff --git a/flight/modules/Stabilization/cruisecontrol.c b/flight/modules/Stabilization/cruisecontrol.c new file mode 100644 index 000000000..9b86608c8 --- /dev/null +++ b/flight/modules/Stabilization/cruisecontrol.c @@ -0,0 +1,94 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief cruisecontrol mode + * @note This file implements the logic for a cruisecontrol + * @{ + * + * @file cruisecontrol.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 +#include + +static float cruisecontrol_factor = 1.0f; + +void cruisecontrol_compute_factor(AttitudeStateData *attitude) +{ + float angleCosine; + + // get attitude state and calculate angle + // spherical right triangle + // 0 <= acosf() <= Pi + + // angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch))); + // more efficient calculation: see coordinateconversion.h Quaternion2R() + angleCosine = (attitude->q1 * attitude->q1) - (attitude->q2 * attitude->q2) - (attitude->q3 * attitude->q3) + (attitude->q4 * attitude->q4); + + // if past the cutoff angle (60 to 180 (180 means never)) + if (angleCosine < stabSettings.cruiseControl.cruise_control_max_angle_cosine) { + // -1 reversed collective, 0 zero power, or 1 normal power + // these are all unboosted + cruisecontrol_factor = stabSettings.cruiseControl.cruise_control_inverted_power_switch; + } else { + // avoid singularity + if (angleCosine > -1e-4f && angleCosine < 1e-4f) { + cruisecontrol_factor = stabSettings.cruiseControl.cruise_control_max_power_factor; + } else { + cruisecontrol_factor = boundf(1.0f / angleCosine, + -stabSettings.cruiseControl.cruise_control_max_power_factor, + stabSettings.cruiseControl.cruise_control_max_power_factor); + } + // factor in the power trim, no effect at 1.0, linear effect increases with factor + cruisecontrol_factor = (cruisecontrol_factor - 1.0f) * stabSettings.cruiseControl.cruise_control_power_trim + 1.0f; + // if inverted and they want negative boost + if (angleCosine < 0.0f && stabSettings.cruiseControl.cruise_control_inverted_power_switch == (int8_t)-1) { + cruisecontrol_factor = -cruisecontrol_factor; + // WARNING: might not go well together with power_trim adjustment above, WARNING: might have bad side effects regarding wound up integral accumulators in all 3 axis at transition stage! + // as long as thrust is getting reversed + // we may as well do pitch and yaw for a complete "invert switch" + // WARNING: currently not implementable in this control scheme, needs to be done elsewhere! + // actuatorDesired.Pitch = -actuatorDesired.Pitch; + // actuatorDesired.Yaw = -actuatorDesired.Yaw; + } + } +} + + +float cruisecontrol_apply_factor(float raw) +{ + if (stabSettings.cruiseControl.cruise_control_max_power_factor > 0.0001f) { + // don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors + if (raw > stabSettings.cruiseControl.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 + raw = boundf((raw - stabSettings.cruiseControl.cruise_control_neutral_thrust) * cruisecontrol_factor + stabSettings.cruiseControl.cruise_control_neutral_thrust, + stabSettings.cruiseControl.cruise_control_min_thrust, + stabSettings.cruiseControl.cruise_control_max_thrust); + } + } + return raw; +} diff --git a/flight/modules/Stabilization/inc/cruisecontrol.h b/flight/modules/Stabilization/inc/cruisecontrol.h index a212cd230..fdad479dc 100644 --- a/flight/modules/Stabilization/inc/cruisecontrol.h +++ b/flight/modules/Stabilization/inc/cruisecontrol.h @@ -33,6 +33,10 @@ #ifndef CRUISECONTROL_H #define CRUISECONTROL_H +#include +#include + +void cruisecontrol_compute_factor(AttitudeStateData *attitude); float cruisecontrol_apply_factor(float raw); #endif /* CRUISECONTROL_H */ diff --git a/flight/modules/Stabilization/inc/outerloop.h b/flight/modules/Stabilization/inc/outerloop.h new file mode 100644 index 000000000..6a81deddb --- /dev/null +++ b/flight/modules/Stabilization/inc/outerloop.h @@ -0,0 +1,38 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief outerloop mode + * @note This file implements the logic for a outerloop + * @{ + * + * @file outerloop.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 + */ + +#ifndef OUTERLOOP_H +#define OUTERLOOP_H + +void stabilizationOuterloopInit(); + +#endif /* OUTERLOOP_H */ diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index dab1a616b..d9962309b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -33,6 +33,7 @@ #ifndef STABILIZATION_H #define STABILIZATION_H +#include #include int32_t StabilizationInitialize(); @@ -40,6 +41,15 @@ int32_t StabilizationInitialize(); typedef struct { StabilizationSettingsData settings; float gyro_alpha; + struct { + float cruise_control_min_thrust; + float cruise_control_max_thrust; + float cruise_control_max_angle_cosine; + float cruise_control_max_power_factor; + float cruise_control_power_trim; + int8_t cruise_control_inverted_power_switch; // WARNING: currently -1 is not fully implemented !!! + float cruise_control_neutral_thrust; + } cruiseControl; } StabilizationData; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 5d49d325b..44a16d733 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -8,7 +8,7 @@ * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" * @{ * - * @file rateloop.c + * @file innerloop.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief Attitude stabilization module. * @@ -157,6 +157,9 @@ static void stabilizationInnerloopTask() actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f); } + + actuator.UpdateTime = dT * 1000; + if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { ActuatorDesiredSet(&actuator); } else { diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c new file mode 100644 index 000000000..52db1a1b5 --- /dev/null +++ b/flight/modules/Stabilization/outerloop.c @@ -0,0 +1,172 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Stabilization PID loops in an airframe type independent manner + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" + * @{ + * + * @file outerloop.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 +#include +#include +#include +#include +// #include +// #include +#include +// #include +#include + +#include +#include + +// Private constants +#define STACK_SIZE_BYTES 256 + +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR + +#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f + +#define AXES 4 + +// Private variables +static DelayedCallbackInfo *callbackHandle; +static struct pid pids[3]; +static AttitudeStateData attitude; +static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; +static PiOSDeltatimeConfig timeval; + +// Private functions +static void stabilizationOuterloopTask(); +static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); +static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); + +void stabilizationOuterloopInit() +{ + RateDesiredInitialize(); + StabilizationDesiredInitialize(); + AttitudeStateInitialize(); + StabilizationStatusInitialize(); + StabilizationBankInitialize(); + PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); + + callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES); + AttitudeStateConnectCallback(AttitudeStateUpdatedCb); + StabilizationBankConnectCallback(BankUpdatedCb); + BankUpdatedCb(NULL); +} + + +/** + * WARNING! This callback executes with critical flight control priority every + * time a gyroscope update happens do NOT put any time consuming calculations + * in this loop unless they really have to execute with every gyro update + */ +static void stabilizationOuterloopTask() +{ + RateDesiredData rateDesired; + StabilizationDesiredData stabilizationDesired; + StabilizationStatusOuterLoopData enabled; + + StabilizationDesiredGet(&stabilizationDesired); + RateDesiredGet(&rateDesired); + StabilizationStatusOuterLoopGet(&enabled); + float *stabilizationDesiredAxis = &stabilizationDesired.Roll; + float *rateDesiredAxis = &rateDesired.Roll; + int t; + // float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + + for (t = 0; t < AXES; t++) { + bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); + previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t]; + + if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) { + if (reinit) { + pids[t].iAccumulator = 0; + } + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { + case STABILIZATIONSTATUS_OUTERLOOP_MANUAL: + default: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; + break; + } + } else { + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { + default: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; + break; + } + } + } + + RateDesiredSet(&rateDesired); +} + + +static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! + AttitudeStateGet(&attitude); + + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); +} + +static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + StabilizationBankData bank; + + StabilizationBankGet(&bank); + + // Set the roll rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], bank.RollRatePID.Kp, + bank.RollRatePID.Ki, + bank.RollRatePID.Kd, + bank.RollRatePID.ILimit); + + // Set the pitch rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], bank.PitchRatePID.Kp, + bank.PitchRatePID.Ki, + bank.PitchRatePID.Kd, + bank.PitchRatePID.ILimit); + + // Set the yaw rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], bank.YawRatePID.Kp, + bank.YawRatePID.Ki, + bank.YawRatePID.Kd, + bank.YawRatePID.ILimit); +} + +/** + * @} + * @} + */ diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 5159a574e..b5491793b 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -53,6 +53,7 @@ #include "flightmodesettings.h" #include "taskinfo.h" + // Math libraries #include "CoordinateConversions.h" #include "pid.h" diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index f68f8e30b..89c6923cf 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -3,7 +3,7 @@ Contains status information to control submodules for stabilization. - + Roll Pitch From 93f82652b716f221dd70f9567a1fe2ce9f6c5319 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 27 Apr 2014 15:15:59 +0200 Subject: [PATCH 14/67] OP-1309 outer loop fully implemented - some cleanups and changes in control flow to accomodate new logic --- .../modules/ManualControl/stabilizedhandler.c | 12 +- .../modules/Stabilization/inc/stabilization.h | 3 +- flight/modules/Stabilization/innerloop.c | 51 ++++-- flight/modules/Stabilization/outerloop.c | 152 +++++++++++++++--- .../revolution/firmware/inc/pios_config.h | 2 +- .../stabilizationsettings.xml | 1 + .../stabilizationstatus.xml | 4 +- 7 files changed, 180 insertions(+), 45 deletions(-) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index e98de70d4..a1da0e406 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -88,11 +88,11 @@ void stabilizedHandler(bool newinit) stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax : 0; // this is an invalid mode @@ -100,11 +100,11 @@ void stabilizedHandler(bool newinit) stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : 0; // this is an invalid mode @@ -122,11 +122,11 @@ void stabilizedHandler(bool newinit) stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax : 0; // this is an invalid mode diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index d9962309b..d1e7e34ee 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -49,7 +49,8 @@ typedef struct { float cruise_control_power_trim; int8_t cruise_control_inverted_power_switch; // WARNING: currently -1 is not fully implemented !!! float cruise_control_neutral_thrust; - } cruiseControl; + } cruiseControl; + float rattitude_mode_transition_stick_position; } StabilizationData; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 44a16d733..1ee8323cd 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -65,9 +65,11 @@ static DelayedCallbackInfo *callbackHandle; static struct pid pids[3]; static float gyro_filtered[3] = { 0, 0, 0 }; +static float axis_lock_accum[3] = { 0, 0, 0 }; static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static PiOSDeltatimeConfig timeval; static float speedScaleFactor = 1.0f; +static StabilizationBankData stabBank; // Private functions static void stabilizationInnerloopTask(); @@ -133,9 +135,30 @@ static void stabilizationInnerloopTask() stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings); break; case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING: + rate[t] = boundf(rate[t], + -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t], + cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t] + ); stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit); break; + case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK: + if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) { + // While getting strong commands act like rate mode + axis_lock_accum[t] = 0; + } else { + // For weaker commands or no command simply attitude lock (almost) on no gyro change + axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT; + axis_lock_accum[t] = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock); + rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp; + } + // IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication! + // keep order as it is, RATE must follow! case STABILIZATIONSTATUS_INNERLOOP_RATE: + // limit rate to maximum configured limits (once here instead of 5 times in outer loop) + rate[t] = boundf(rate[t], + -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t], + cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t] + ); actuatorDesiredAxis[t] = pid_apply_setpoint(&pids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); break; case STABILIZATIONSTATUS_INNERLOOP_DIRECT: @@ -186,27 +209,25 @@ static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationBankData bank; - - StabilizationBankGet(&bank); + StabilizationBankGet(&stabBank); // Set the roll rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], bank.RollRatePID.Kp, - bank.RollRatePID.Ki, - bank.RollRatePID.Kd, - bank.RollRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], stabBank.RollRatePID.Kp, + stabBank.RollRatePID.Ki, + stabBank.RollRatePID.Kd, + stabBank.RollRatePID.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], bank.PitchRatePID.Kp, - bank.PitchRatePID.Ki, - bank.PitchRatePID.Kd, - bank.PitchRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], stabBank.PitchRatePID.Kp, + stabBank.PitchRatePID.Ki, + stabBank.PitchRatePID.Kd, + stabBank.PitchRatePID.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], bank.YawRatePID.Kp, - bank.YawRatePID.Ki, - bank.YawRatePID.Kd, - bank.YawRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], stabBank.YawRatePID.Kp, + stabBank.YawRatePID.Ki, + stabBank.YawRatePID.Kd, + stabBank.YawRatePID.ILimit); } #ifdef REVOLUTION diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 52db1a1b5..1943a9c6a 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -37,14 +37,14 @@ #include #include #include -// #include -// #include +#include #include -// #include #include + #include #include +#include // Private constants #define STACK_SIZE_BYTES 256 @@ -63,6 +63,8 @@ static DelayedCallbackInfo *callbackHandle; static struct pid pids[3]; static AttitudeStateData attitude; +static StabilizationBankData stabBank; + static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static PiOSDeltatimeConfig timeval; @@ -94,6 +96,7 @@ void stabilizationOuterloopInit() */ static void stabilizationOuterloopTask() { + AttitudeStateData attitudeState; RateDesiredData rateDesired; StabilizationDesiredData stabilizationDesired; StabilizationStatusOuterLoopData enabled; @@ -104,7 +107,41 @@ static void stabilizationOuterloopTask() float *stabilizationDesiredAxis = &stabilizationDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; int t; - // float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + + float local_error[3]; + { +#if defined(PIOS_QUATERNION_STABILIZATION) + // Quaternion calculation of error in each axis. Uses more memory. + float rpy_desired[3]; + float q_desired[4]; + float q_error[4]; + + rpy_desired[0] = stabilizationDesiredAxis[0]; + rpy_desired[1] = stabilizationDesiredAxis[1]; + rpy_desired[2] = stabilizationDesiredAxis[2]; + + RPY2Quaternion(rpy_desired, q_desired); + quat_inverse(q_desired); + quat_mult(q_desired, &attitudeState.q1, q_error); + quat_inverse(q_error); + Quaternion2RPY(q_error, local_error); + +#else /* if defined(PIOS_QUATERNION_STABILIZATION) */ + // Simpler algorithm for CC, less memory + local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll; + local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch; + local_error[2] = stabilizationDesiredAxis[2] - attitudeState.Yaw; + + // find shortest way + float modulo = fmodf(local_error[2] + 180.0f, 360.0f); + if (modulo < 0) { + local_error[2] = modulo + 180.0f; + } else { + local_error[2] = modulo - 180.0f; + } +#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ + } for (t = 0; t < AXES; t++) { bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); @@ -115,7 +152,81 @@ static void stabilizationOuterloopTask() pids[t].iAccumulator = 0; } switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { - case STABILIZATIONSTATUS_OUTERLOOP_MANUAL: + case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: + rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT); + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: + { + float stickinput[3]; + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabBank.YawMax, -1.0f, 1.0f); + float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t]; + rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT); + // Compute the weighted average rate desired + // Using max() rather than sqrt() for cpu speed; + // - this makes the stick region into a square; + // - this is a feature! + // - hold a roll angle and add just pitch without the stick sensitivity changing + float magnitude = stickinput[t]; + if (t < 2) { + magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1])); + } + + // modify magnitude to move the Att to Rate transition to the place + // specified by the user + // we are looking for where the stick angle == transition angle + // and the Att rate equals the Rate rate + // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] + // == Rate x StickAngle [Rate pulling up according to stick angle] + // * StickAngle [X Ratt proportion] + // so 1-x == x*x or x*x+x-1=0 where xE(0,1) + // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 + // and quadratic formula says that is 0.618033989f + // I tested 14.01 and came up with .61 without even remembering this number + // I thought that moving the P,I, and maxangle terms around would change this value + // and that I would have to take these into account, but varying + // all P's and I's by factors of 1/2 to 2 didn't change it noticeably + // and varying maxangle from 4 to 120 didn't either. + // so for now I'm not taking these into account + // while working with this, it occurred to me that Attitude mode, + // set up with maxangle=190 would be similar to Ratt, and it is. + #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f + + // the following assumes the transition would otherwise be at 0.618033989f + // and THAT assumes that Att ramps up to max roll rate + // when a small number of degrees off of where it should be + + // if below the transition angle (still in attitude mode) + // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ + if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) { + magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position; + } else { + magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position) + * (1.0f - STICK_VALUE_AT_MODE_TRANSITION) + / (1.0f - stabSettings.rattitude_mode_transition_stick_position) + + STICK_VALUE_AT_MODE_TRANSITION; + } + rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate; + } + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + // FIXME: local_error[] is rate - attitude for Weak Leveling + // The only ramifications are: + // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS + // Changing Rate mode max rate currently requires a change to Kp + // That would be changed to Attitude mode max angle affecting Kp + // Also does not take dT into account + { + float rate_input = cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabBank, stabBank.RollMax)[t]; + float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; + weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); + + // Compute desired rate as input biased towards leveling + rateDesiredAxis[t] = rate_input + weak_leveling; + } + break; + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: default: rateDesiredAxis[t] = stabilizationDesiredAxis[t]; break; @@ -130,6 +241,9 @@ static void stabilizationOuterloopTask() } RateDesiredSet(&rateDesired); + + // update cruisecontrol based on attitude + cruisecontrol_compute_factor(&attitudeState); } @@ -143,27 +257,25 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationBankData bank; - - StabilizationBankGet(&bank); + StabilizationBankGet(&stabBank); // Set the roll rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], bank.RollRatePID.Kp, - bank.RollRatePID.Ki, - bank.RollRatePID.Kd, - bank.RollRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], stabBank.RollRatePID.Kp, + stabBank.RollRatePID.Ki, + stabBank.RollRatePID.Kd, + stabBank.RollRatePID.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], bank.PitchRatePID.Kp, - bank.PitchRatePID.Ki, - bank.PitchRatePID.Kd, - bank.PitchRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], stabBank.PitchRatePID.Kp, + stabBank.PitchRatePID.Ki, + stabBank.PitchRatePID.Kd, + stabBank.PitchRatePID.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], bank.YawRatePID.Kp, - bank.YawRatePID.Ki, - bank.YawRatePID.Kd, - bank.YawRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], stabBank.YawRatePID.Kp, + stabBank.YawRatePID.Ki, + stabBank.YawRatePID.Kd, + stabBank.YawRatePID.ILimit); } /** diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index c60e9a453..5b46f059a 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -144,7 +144,7 @@ #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ -/* #define PIOS_QUATERNION_STABILIZATION */ +#define PIOS_QUATERNION_STABILIZATION /* Performance counters */ #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 51d6497bd..a14279df9 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -22,6 +22,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index 89c6923cf..f58200853 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -3,7 +3,7 @@ Contains status information to control submodules for stabilization. - + Roll Pitch @@ -11,7 +11,7 @@ Thrust - + Roll Pitch From 82d2c5a308f604e4e2922b27d4f89435f2c26dc5 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 28 Apr 2014 19:11:00 +0200 Subject: [PATCH 15/67] OP-1309 Implemented control of new stabilization framework (replaces old stabilization module) --- .../modules/Stabilization/inc/stabilization.h | 7 + flight/modules/Stabilization/innerloop.c | 58 +- flight/modules/Stabilization/outerloop.c | 65 +- flight/modules/Stabilization/stabilization.c | 831 ++++-------------- .../boards/coptercontrol/firmware/Makefile | 1 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + 8 files changed, 207 insertions(+), 758 deletions(-) diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index d1e7e34ee..fa0b9694b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -34,12 +34,16 @@ #define STABILIZATION_H #include +#include #include +#include + int32_t StabilizationInitialize(); typedef struct { StabilizationSettingsData settings; + StabilizationBankData stabBank; float gyro_alpha; struct { float cruise_control_min_thrust; @@ -51,11 +55,14 @@ typedef struct { float cruise_control_neutral_thrust; } cruiseControl; float rattitude_mode_transition_stick_position; + struct pid innerPids[3], outerPids[3]; } StabilizationData; extern StabilizationData stabSettings; +#define AXES 4 + #endif // STABILIZATION_H diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 1ee8323cd..6a00c6c3e 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -59,22 +60,17 @@ #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f -#define AXES 4 - // Private variables static DelayedCallbackInfo *callbackHandle; -static struct pid pids[3]; static float gyro_filtered[3] = { 0, 0, 0 }; static float axis_lock_accum[3] = { 0, 0, 0 }; static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static PiOSDeltatimeConfig timeval; static float speedScaleFactor = 1.0f; -static StabilizationBankData stabBank; // Private functions static void stabilizationInnerloopTask(); static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); -static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); #ifdef REVOLUTION static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); #endif @@ -86,7 +82,7 @@ void stabilizationInnerloopInit() GyroStateInitialize(); StabilizationStatusInitialize(); FlightStatusInitialize(); - StabilizationBankInitialize(); + ManualControlCommandInitialize(); #ifdef REVOLUTION AirspeedStateInitialize(); AirspeedStateConnectCallback(AirSpeedUpdatedCb); @@ -95,8 +91,6 @@ void stabilizationInnerloopInit() callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES); GyroStateConnectCallback(GyroStateUpdatedCb); - StabilizationBankConnectCallback(BankUpdatedCb); - BankUpdatedCb(NULL); } @@ -128,7 +122,7 @@ static void stabilizationInnerloopTask() if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) { if (reinit) { - pids[t].iAccumulator = 0; + stabSettings.innerPids[t].iAccumulator = 0; } switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR: @@ -136,8 +130,8 @@ static void stabilizationInnerloopTask() break; case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING: rate[t] = boundf(rate[t], - -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t], - cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t] + -cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t], + cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t] ); stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit); break; @@ -156,10 +150,10 @@ static void stabilizationInnerloopTask() case STABILIZATIONSTATUS_INNERLOOP_RATE: // limit rate to maximum configured limits (once here instead of 5 times in outer loop) rate[t] = boundf(rate[t], - -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t], - cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t] + -cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t], + cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t] ); - actuatorDesiredAxis[t] = pid_apply_setpoint(&pids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); + actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); break; case STABILIZATIONSTATUS_INNERLOOP_DIRECT: default: @@ -191,6 +185,19 @@ static void stabilizationInnerloopTask() previous_mode[t] = 255; } } + { + uint8_t armed; + FlightStatusArmedGet(&armed); + float throttleDesired; + ManualControlCommandThrottleGet(&throttleDesired); + if (armed != FLIGHTSTATUS_ARMED_ARMED || + ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) { + // Force all axes to reinitialize when engaged + for (t = 0; t < AXES; t++) { + previous_mode[t] = 255; + } + } + } } @@ -207,29 +214,6 @@ static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); } -static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - StabilizationBankGet(&stabBank); - - // Set the roll rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], stabBank.RollRatePID.Kp, - stabBank.RollRatePID.Ki, - stabBank.RollRatePID.Kd, - stabBank.RollRatePID.ILimit); - - // Set the pitch rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], stabBank.PitchRatePID.Kp, - stabBank.PitchRatePID.Ki, - stabBank.PitchRatePID.Kd, - stabBank.PitchRatePID.ILimit); - - // Set the yaw rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], stabBank.YawRatePID.Kp, - stabBank.YawRatePID.Ki, - stabBank.YawRatePID.Kd, - stabBank.YawRatePID.ILimit); -} - #ifdef REVOLUTION static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 1943a9c6a..53fbab13a 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include @@ -57,13 +59,9 @@ #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f -#define AXES 4 - // Private variables static DelayedCallbackInfo *callbackHandle; -static struct pid pids[3]; static AttitudeStateData attitude; -static StabilizationBankData stabBank; static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static PiOSDeltatimeConfig timeval; @@ -71,7 +69,6 @@ static PiOSDeltatimeConfig timeval; // Private functions static void stabilizationOuterloopTask(); static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); -static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); void stabilizationOuterloopInit() { @@ -79,13 +76,13 @@ void stabilizationOuterloopInit() StabilizationDesiredInitialize(); AttitudeStateInitialize(); StabilizationStatusInitialize(); - StabilizationBankInitialize(); + FlightStatusInitialize(); + ManualControlCommandInitialize(); + PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES); AttitudeStateConnectCallback(AttitudeStateUpdatedCb); - StabilizationBankConnectCallback(BankUpdatedCb); - BankUpdatedCb(NULL); } @@ -149,20 +146,20 @@ static void stabilizationOuterloopTask() if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) { if (reinit) { - pids[t].iAccumulator = 0; + stabSettings.outerPids[t].iAccumulator = 0; } switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: - rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT); + rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: { float stickinput[3]; - stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabBank.RollMax, -1.0f, 1.0f); - stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabBank.PitchMax, -1.0f, 1.0f); - stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabBank.YawMax, -1.0f, 1.0f); - float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t]; - rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT); + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); + float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]; + rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); // Compute the weighted average rate desired // Using max() rather than sqrt() for cpu speed; // - this makes the stick region into a square; @@ -218,7 +215,7 @@ static void stabilizationOuterloopTask() // That would be changed to Attitude mode max angle affecting Kp // Also does not take dT into account { - float rate_input = cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabBank, stabBank.RollMax)[t]; + float rate_input = cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabSettings.stabBank, stabSettings.stabBank.RollMax)[t]; float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); @@ -241,6 +238,19 @@ static void stabilizationOuterloopTask() } RateDesiredSet(&rateDesired); + { + uint8_t armed; + FlightStatusArmedGet(&armed); + float throttleDesired; + ManualControlCommandThrottleGet(&throttleDesired); + if (armed != FLIGHTSTATUS_ARMED_ARMED || + ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) { + // Force all axes to reinitialize when engaged + for (t = 0; t < AXES; t++) { + previous_mode[t] = 255; + } + } + } // update cruisecontrol based on attitude cruisecontrol_compute_factor(&attitudeState); @@ -255,29 +265,6 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); } -static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - StabilizationBankGet(&stabBank); - - // Set the roll rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], stabBank.RollRatePID.Kp, - stabBank.RollRatePID.Ki, - stabBank.RollRatePID.Kd, - stabBank.RollRatePID.ILimit); - - // Set the pitch rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], stabBank.PitchRatePID.Kp, - stabBank.PitchRatePID.Ki, - stabBank.PitchRatePID.Kd, - stabBank.PitchRatePID.ILimit); - - // Set the yaw rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], stabBank.YawRatePID.Kp, - stabBank.YawRatePID.Ki, - stabBank.YawRatePID.Kd, - stabBank.YawRatePID.ILimit); -} - /** * @} * @} diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index b5491793b..770b84449 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -33,125 +33,56 @@ #include #include -#include "stabilization.h" -#include "stabilizationsettings.h" -#include "stabilizationbank.h" -#include "stabilizationsettingsbank1.h" -#include "stabilizationsettingsbank2.h" -#include "stabilizationsettingsbank3.h" -#include "actuatordesired.h" -#include "ratedesired.h" -#include "relaytuning.h" -#include "relaytuningsettings.h" -#include "stabilizationdesired.h" -#include "attitudestate.h" -#include "airspeedstate.h" -#include "gyrostate.h" -#include "flightstatus.h" -#include "manualcontrolsettings.h" -#include "manualcontrolcommand.h" -#include "flightmodesettings.h" -#include "taskinfo.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -// Math libraries -#include "CoordinateConversions.h" -#include "pid.h" -#include "sin_lookup.h" - -// Includes for various stabilization algorithms -#include "relay_tuning.h" -#include "virtualflybar.h" - -// Includes for various stabilization algorithms -#include "relay_tuning.h" - -// Private constants -#define UPDATE_EXPECTED (1.0f / 666.0f) -#define UPDATE_MIN 1.0e-6f -#define UPDATE_MAX 1.0f -#define UPDATE_ALPHA 1.0e-2f - -typedef enum { ROLL = 0, PITCH = 1, YAW = 2, MAX_AXES = 3 } blaenum; - -#define MAX_QUEUE_SIZE 1 - -#if defined(PIOS_STABILIZATION_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE -#else -#define STACK_SIZE_BYTES 860 -#endif - -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority -#define FAILSAFE_TIMEOUT_MS 30 - -// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode -// The PID_RATE set is used by the attitude portion of Attitude mode -enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX }; -enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET }; -enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET }; +// Public variables +StabilizationData stabSettings; // Private variables -static xTaskHandle taskHandle; -static StabilizationSettingsData settings; -static xQueueHandle queue; -float gyro_alpha = 0; -float axis_lock_accum[3] = { 0, 0, 0 }; -uint8_t max_axis_lock = 0; -uint8_t max_axislock_rate = 0; -float weak_leveling_kp = 0; -uint8_t weak_leveling_max = 0; -bool lowThrottleZeroIntegral; -float vbar_decay = 0.991f; -struct pid pids[PID_MAX]; - -int cur_flight_mode = -1; - -static float rattitude_mode_transition_stick_position; -static float cruise_control_min_thrust; -static float cruise_control_max_thrust; -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 uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; +static int cur_flight_mode = -1; // Private functions -static void stabilizationTask(void *parameters); -static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent *ev); static void BankUpdatedCb(UAVObjEvent *ev); static void SettingsBankUpdatedCb(UAVObjEvent *ev); +static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev); +static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev); /** * Module initialization */ int32_t StabilizationStart() { - // Initialize variables - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - - // Listen for updates. - // AttitudeStateConnectQueue(queue); - GyroStateConnectQueue(queue); - StabilizationSettingsConnectCallback(SettingsUpdatedCb); - SettingsUpdatedCb(StabilizationSettingsHandle()); - + ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb); StabilizationBankConnectCallback(BankUpdatedCb); - StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb); StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb); StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb); + StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb); + SettingsUpdatedCb(StabilizationSettingsHandle()); + StabilizationDesiredUpdatedCb(StabilizationDesiredHandle()); + FlightModeSwitchUpdatedCb(ManualControlCommandHandle()); - - // Start main task - xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle); #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); +// PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); #endif return 0; } @@ -162,642 +93,186 @@ int32_t StabilizationStart() int32_t StabilizationInitialize() { // Initialize variables - ManualControlCommandInitialize(); - ManualControlSettingsInitialize(); - FlightStatusInitialize(); StabilizationDesiredInitialize(); StabilizationSettingsInitialize(); + StabilizationStatusInitialize(); StabilizationBankInitialize(); StabilizationSettingsBank1Initialize(); StabilizationSettingsBank2Initialize(); StabilizationSettingsBank3Initialize(); - ActuatorDesiredInitialize(); -#ifdef DIAG_RATEDESIRED RateDesiredInitialize(); -#endif -#ifdef REVOLUTION - AirspeedStateInitialize(); -#endif + ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch // Code required for relay tuning sin_lookup_initalize(); RelayTuningSettingsInitialize(); RelayTuningInitialize(); + stabilizationOuterloopInit(); + stabilizationInnerloopInit(); + pid_zero(&stabSettings.outerPids[0]); + pid_zero(&stabSettings.outerPids[1]); + pid_zero(&stabSettings.outerPids[2]); + pid_zero(&stabSettings.innerPids[0]); + pid_zero(&stabSettings.innerPids[1]); + pid_zero(&stabSettings.innerPids[2]); return 0; } MODULE_INITCALL(StabilizationInitialize, StabilizationStart); -/** - * Module task - */ -static void stabilizationTask(__attribute__((unused)) void *parameters) +static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - UAVObjEvent ev; - PiOSDeltatimeConfig timeval; - - PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); - - ActuatorDesiredData actuatorDesired; - StabilizationDesiredData stabDesired; - float throttleDesired; - RateDesiredData rateDesired; - AttitudeStateData attitudeState; - GyroStateData gyroStateData; - FlightStatusData flightStatus; - StabilizationBankData stabBank; - - -#ifdef REVOLUTION - AirspeedStateData airspeedState; -#endif - - SettingsUpdatedCb((UAVObjEvent *)NULL); - - // Main task loop - ZeroPids(); - while (1) { - float dT; - -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); -#endif - - // Wait until the Gyro object is updated, if a timeout then go to failsafe - if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) { - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); - continue; - } - - dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); - FlightStatusGet(&flightStatus); - StabilizationDesiredGet(&stabDesired); - ManualControlCommandThrottleGet(&throttleDesired); - AttitudeStateGet(&attitudeState); - GyroStateGet(&gyroStateData); - StabilizationBankGet(&stabBank); -#ifdef DIAG_RATEDESIRED - RateDesiredGet(&rateDesired); -#endif - uint8_t flight_mode_switch_position; - ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position); - - if (cur_flight_mode != flight_mode_switch_position) { - cur_flight_mode = flight_mode_switch_position; - SettingsBankUpdatedCb(NULL); - } - -#ifdef REVOLUTION - float speedScaleFactor; - // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes - AirspeedStateGet(&airspeedState); - if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) { - // feature has been turned off - speedScaleFactor = 1.0f; - } else { - // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2 - speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed); - if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) { - speedScaleFactor = settings.ScaleToAirspeedLimits.Min; - } - if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) { - speedScaleFactor = settings.ScaleToAirspeedLimits.Max; - } - } -#else - const float speedScaleFactor = 1.0f; -#endif - -#if defined(PIOS_QUATERNION_STABILIZATION) - // Quaternion calculation of error in each axis. Uses more memory. - float rpy_desired[3]; - float q_desired[4]; - float q_error[4]; - float local_error[3]; - - // Essentially zero errors for anything in rate or none - if (stabDesired.StabilizationMode.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { - rpy_desired[0] = stabDesired.Roll; - } else { - rpy_desired[0] = attitudeState.Roll; - } - - if (stabDesired.StabilizationMode.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { - rpy_desired[1] = stabDesired.Pitch; - } else { - rpy_desired[1] = attitudeState.Pitch; - } - - if (stabDesired.StabilizationMode.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { - rpy_desired[2] = stabDesired.Yaw; - } else { - rpy_desired[2] = attitudeState.Yaw; - } - - RPY2Quaternion(rpy_desired, q_desired); - quat_inverse(q_desired); - quat_mult(q_desired, &attitudeState.q1, q_error); - quat_inverse(q_error); - Quaternion2RPY(q_error, local_error); - -#else /* if defined(PIOS_QUATERNION_STABILIZATION) */ - // Simpler algorithm for CC, less memory - float local_error[3] = { stabDesired.Roll - attitudeState.Roll, - stabDesired.Pitch - attitudeState.Pitch, - stabDesired.Yaw - attitudeState.Yaw }; - // find shortest way - float modulo = fmodf(local_error[2] + 180.0f, 360.0f); - if (modulo < 0) { - local_error[2] = modulo + 180.0f; - } else { - local_error[2] = modulo - 180.0f; - } -#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ - - float gyro_filtered[3]; - gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyroStateData.x * (1 - gyro_alpha); - gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha); - gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha); - - float *stabDesiredAxis = &stabDesired.Roll; - float *actuatorDesiredAxis = &actuatorDesired.Roll; - float *rateDesiredAxis = &rateDesired.Roll; - - ActuatorDesiredGet(&actuatorDesired); - - // A flag to track which stabilization mode each axis is in - static uint8_t previous_mode[MAX_AXES] = { 255, 255, 255 }; - bool error = false; - - // Run the selected stabilization algorithm on each axis: - for (uint8_t i = 0; i < MAX_AXES; i++) { - // Check whether this axis mode needs to be reinitialized - bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]); - previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]; - - // Apply the selected control law - switch (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]) { - case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: - if (reinit) { - pids[PID_RATE_ROLL + i].iAccumulator = 0; - } - - // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = - boundf(stabDesiredAxis[i], -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); - - // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); - - break; - - case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - if (reinit) { - pids[PID_ROLL + i].iAccumulator = 0; - pids[PID_RATE_ROLL + i].iAccumulator = 0; - } - - // Compute the outer loop - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = boundf(rateDesiredAxis[i], - -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i], - cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]); - - // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); - - break; - - case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: - // A parameterization from Attitude mode at center stick - // - to Rate mode at full stick - // This is done by parameterizing to use the rotation rate that Attitude mode - // - would use at center stick to using the rotation rate that Rate mode - // - would use at full stick in a weighted average sort of way. - { - if (reinit) { - pids[PID_ROLL + i].iAccumulator = 0; - pids[PID_RATE_ROLL + i].iAccumulator = 0; - } - - // Compute what Rate mode would give for this stick angle's rate - // Save Rate's rate in a temp for later merging with Attitude's rate - float rateDesiredAxisRate; - rateDesiredAxisRate = boundf(stabDesiredAxis[i], -1.0f, 1.0f) - * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]; - - // Compute what Attitude mode would give for this stick angle's rate - - // stabDesired for this mode is [-1.0f,+1.0f] - // - multiply by Attitude mode max angle to get desired angle - // - subtract off the actual angle to get the angle error - // This is what local_error[] holds for Attitude mode - float attitude_error = stabDesiredAxis[i] - * cast_struct_to_array(stabBank.RollMax, stabBank.RollMax)[i] - - cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i]; - - // Compute the outer loop just like Attitude mode does - float rateDesiredAxisAttitude; - rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT); - rateDesiredAxisAttitude = boundf(rateDesiredAxisAttitude, - -cast_struct_to_array(stabBank.ManualRate, - stabBank.ManualRate.Roll)[i], - cast_struct_to_array(stabBank.ManualRate, - stabBank.ManualRate.Roll)[i]); - - // Compute the weighted average rate desired - // Using max() rather than sqrt() for cpu speed; - // - this makes the stick region into a square; - // - this is a feature! - // - hold a roll angle and add just pitch without the stick sensitivity changing - // magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch); - float magnitude; - magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch)); - - // modify magnitude to move the Att to Rate transition to the place - // specified by the user - // we are looking for where the stick angle == transition angle - // and the Att rate equals the Rate rate - // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] - // == Rate x StickAngle [Rate pulling up according to stick angle] - // * StickAngle [X Ratt proportion] - // so 1-x == x*x or x*x+x-1=0 where xE(0,1) - // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 - // and quadratic formula says that is 0.618033989f - // I tested 14.01 and came up with .61 without even remembering this number - // I thought that moving the P,I, and maxangle terms around would change this value - // and that I would have to take these into account, but varying - // all P's and I's by factors of 1/2 to 2 didn't change it noticeably - // and varying maxangle from 4 to 120 didn't either. - // so for now I'm not taking these into account - // while working with this, it occurred to me that Attitude mode, - // set up with maxangle=190 would be similar to Ratt, and it is. - #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f - - // the following assumes the transition would otherwise be at 0.618033989f - // and THAT assumes that Att ramps up to max roll rate - // when a small number of degrees off of where it should be - - // if below the transition angle (still in attitude mode) - // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ - if (magnitude <= rattitude_mode_transition_stick_position) { - magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position; - } else { - magnitude = (magnitude - rattitude_mode_transition_stick_position) - * (1.0f - STICK_VALUE_AT_MODE_TRANSITION) - / (1.0f - rattitude_mode_transition_stick_position) - + STICK_VALUE_AT_MODE_TRANSITION; - } - rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude - + magnitude * rateDesiredAxisRate; - - // Compute the inner loop for the averaged rate - // actuatorDesiredAxis[i] is the weighted average - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, - rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); - - break; - } - - case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: - // Store for debugging output - rateDesiredAxis[i] = stabDesiredAxis[i]; - - // Run a virtual flybar stabilization algorithm on this axis - stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); - - break; - - case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: - // FIXME: local_error[] is rate - attitude for Weak Leveling - // The only ramifications are: - // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS - // Changing Rate mode max rate currently requires a change to Kp - // That would be changed to Attitude mode max angle affecting Kp - // Also does not take dT into account - { - if (reinit) { - pids[PID_RATE_ROLL + i].iAccumulator = 0; - } - - float weak_leveling = local_error[i] * weak_leveling_kp; - weak_leveling = boundf(weak_leveling, -weak_leveling_max, weak_leveling_max); - - // Compute desired rate as input biased towards leveling - rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling; - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); - - break; - } - - case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: - if (reinit) { - pids[PID_RATE_ROLL + i].iAccumulator = 0; - } - - if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) { - // While getting strong commands act like rate mode - rateDesiredAxis[i] = stabDesiredAxis[i]; - axis_lock_accum[i] = 0; - } else { - // For weaker commands or no command simply attitude lock (almost) on no gyro change - axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT; - axis_lock_accum[i] = boundf(axis_lock_accum[i], -max_axis_lock, max_axis_lock); - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); - } - - rateDesiredAxis[i] = boundf(rateDesiredAxis[i], - -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], - cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); - - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); - - break; - - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: - // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = boundf(stabDesiredAxis[i], - -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], - cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); - - // Run the relay controller which also estimates the oscillation parameters - stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); - - break; - - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: - if (reinit) { - pids[PID_ROLL + i].iAccumulator = 0; - } - - // Compute the outer loop like attitude mode - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = boundf(rateDesiredAxis[i], - -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i], - cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]); - - // Run the relay controller which also estimates the oscillation parameters - stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f); - - break; - - case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL: - actuatorDesiredAxis[i] = boundf(stabDesiredAxis[i], -1.0f, 1.0f); - break; - default: - error = true; - break; - } - } - - if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) { - stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); - } - -#ifdef DIAG_RATEDESIRED - RateDesiredSet(&rateDesired); -#endif - - // Save dT - actuatorDesired.UpdateTime = dT * 1000; - actuatorDesired.Thrust = stabDesired.Thrust; - - // modify thrust according to 1/cos(bank angle) - // to maintain same altitdue 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 - 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; - float angle; - // get attitude state and calculate angle - // do it every 8th iteration to save CPU - if ((toggle++ & 7) == 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; - } 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; - } - } - // 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" - actuatorDesired.Pitch = -actuatorDesired.Pitch; - actuatorDesired.Yaw = -actuatorDesired.Yaw; - } - } - } - - // 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; - if (actuatorDesired.Thrust > cruise_control_max_thrust) { - actuatorDesired.Thrust = cruise_control_max_thrust; - } else if (actuatorDesired.Thrust < cruise_control_min_thrust) { - actuatorDesired.Thrust = cruise_control_min_thrust; - } - } - } - - if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - ActuatorDesiredSet(&actuatorDesired); - } else { - // Force all axes to reinitialize when engaged - for (uint8_t i = 0; i < MAX_AXES; i++) { - previous_mode[i] = 255; - } - } - - if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || - (lowThrottleZeroIntegral && throttleDesired < 0)) { - // Force all axes to reinitialize when engaged - for (uint8_t i = 0; i < MAX_AXES; i++) { - previous_mode[i] = 255; - } - } - - // Clear or set alarms. Done like this to prevent toggline each cycle - // and hammering system alarms - if (error) { - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + StabilizationStatusData status; + StabilizationDesiredStabilizationModeData mode; + int t; + + StabilizationDesiredStabilizationModeGet(&mode); + for (t = 0; t < AXES; t++) { + switch (cast_struct_to_array(mode, mode.Roll)[t]) { + case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; + cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; + break; } } + StabilizationStatusSet(&status); } - -/** - * Clear the accumulators and derivatives for all the axes - */ -static void ZeroPids(void) +static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - for (uint32_t i = 0; i < PID_MAX; i++) { - pid_zero(&pids[i]); - } + uint8_t fm; + ManualControlCommandFlightModeSwitchPositionGet(&fm); - for (uint8_t i = 0; i < 3; i++) { - axis_lock_accum[i] = 0.0f; + if (fm == cur_flight_mode) { + return; } + cur_flight_mode = fm; + SettingsBankUpdatedCb(NULL); } - static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { return; } - if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) || - (settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) || - (settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) || - settings.FlightModeMap[cur_flight_mode] > 2)) { + if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) || + (stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) || + (stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) || + stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) { return; } - StabilizationBankData bank; - switch (settings.FlightModeMap[cur_flight_mode]) { + switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) { case 0: - StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank); + StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank); break; case 1: - StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank); + StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank); break; case 2: - StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank); + StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank); break; } - StabilizationBankSet(&bank); + StabilizationBankSet(&stabSettings.stabBank); } static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationBankData bank; - - StabilizationBankGet(&bank); - -// this code will be needed if any other modules alter stabilizationbank -/* - StabilizationBankData curBank; - if(flight_mode < 0) return; - - switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode]) - { - case 0: - StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank); - if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0) - { - StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank); - } - break; - - case 1: - StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank); - if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0) - { - StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank); - } - break; - - case 2: - StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank); - if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0) - { - StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank); - } - break; - - default: - return; //invalid bank number - } - */ - + StabilizationBankGet(&stabSettings.stabBank); // Set the roll rate PID constants - pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp, - bank.RollRatePID.Ki, - bank.RollRatePID.Kd, - bank.RollRatePID.ILimit); + pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp, + stabSettings.stabBank.RollRatePID.Ki, + stabSettings.stabBank.RollRatePID.Kd, + stabSettings.stabBank.RollRatePID.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[PID_RATE_PITCH], bank.PitchRatePID.Kp, - bank.PitchRatePID.Ki, - bank.PitchRatePID.Kd, - bank.PitchRatePID.ILimit); + pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp, + stabSettings.stabBank.PitchRatePID.Ki, + stabSettings.stabBank.PitchRatePID.Kd, + stabSettings.stabBank.PitchRatePID.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[PID_RATE_YAW], bank.YawRatePID.Kp, - bank.YawRatePID.Ki, - bank.YawRatePID.Kd, - bank.YawRatePID.ILimit); + pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp, + stabSettings.stabBank.YawRatePID.Ki, + stabSettings.stabBank.YawRatePID.Kd, + stabSettings.stabBank.YawRatePID.ILimit); // Set the roll attitude PI constants - pid_configure(&pids[PID_ROLL], bank.RollPI.Kp, - bank.RollPI.Ki, + pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp, + stabSettings.stabBank.RollPI.Ki, 0, - bank.RollPI.ILimit); + stabSettings.stabBank.RollPI.ILimit); // Set the pitch attitude PI constants - pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp, - bank.PitchPI.Ki, + pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp, + stabSettings.stabBank.PitchPI.Ki, 0, - bank.PitchPI.ILimit); + stabSettings.stabBank.PitchPI.ILimit); // Set the yaw attitude PI constants - pid_configure(&pids[PID_YAW], bank.YawPI.Kp, - bank.YawPI.Ki, + pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp, + stabSettings.stabBank.YawPI.Ki, 0, - bank.YawPI.ILimit); + stabSettings.stabBank.YawPI.ILimit); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationSettingsGet(&settings); + // needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL + StabilizationSettingsGet(&stabSettings.settings); // Set up the derivative term - pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); - - // Maximum deviation to accumulate for axis lock - max_axis_lock = settings.MaxAxisLock; - max_axislock_rate = settings.MaxAxisLockRate; - - // Settings for weak leveling - weak_leveling_kp = settings.WeakLevelingKp; - weak_leveling_max = settings.MaxWeakLevelingRate; - - // Whether to zero the PID integrals while thrust is low - lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; + pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma); // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant @@ -805,37 +280,29 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this // calculation const float fakeDt = 0.0025f; - if (settings.GyroTau < 0.0001f) { - gyro_alpha = 0; // not trusting this to resolve to 0 + if (stabSettings.settings.GyroTau < 0.0001f) { + stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0 } else { - gyro_alpha = expf(-fakeDt / settings.GyroTau); + stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau); } - // Compute time constant for vbar decay term based on a tau - vbar_decay = expf(-fakeDt / settings.VbarTau); - // force flight mode update cur_flight_mode = -1; // Rattitude stick angle where the attitude to rate transition happens - if (settings.RattitudeModeTransition < (uint8_t)10) { - rattitude_mode_transition_stick_position = 10.0f / 100.0f; + if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) { + stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f; } else { - rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f; + stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 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; - - // memcpy( // disabled because removed from uavobject for refactoring (CRITICAL! doesnt fly, just to make it compile!!!) - // cruise_control_flight_mode_switch_pos_enable, - // settings.CruiseControlFlightModeSwitchPosEnable, - // sizeof(cruise_control_flight_mode_switch_pos_enable)); + stabSettings.cruiseControl.cruise_control_min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f; + stabSettings.cruiseControl.cruise_control_max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f; + stabSettings.cruiseControl.cruise_control_max_angle_cosine = cos_lookup_deg(stabSettings.settings.CruiseControlMaxAngle); + stabSettings.cruiseControl.cruise_control_max_power_factor = stabSettings.settings.CruiseControlMaxPowerFactor; + stabSettings.cruiseControl.cruise_control_power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f; + stabSettings.cruiseControl.cruise_control_inverted_power_switch = stabSettings.settings.CruiseControlInvertedPowerSwitch; + stabSettings.cruiseControl.cruise_control_neutral_thrust = (float)stabSettings.settings.CruiseControlNeutralThrust / 100.0f; } /** diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 6c0038305..62adbea68 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -87,6 +87,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c + SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c SRC += $(OPUAVSYNTHDIR)/actuatordesired.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 72f51cbec..e6a69a8fb 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings UAVOBJSRCFILENAMES += stabilizationsettingsbank1 UAVOBJSRCFILENAMES += stabilizationsettingsbank2 UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus UAVOBJSRCFILENAMES += stabilizationbank UAVOBJSRCFILENAMES += systemalarms UAVOBJSRCFILENAMES += systemsettings diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 72f51cbec..e6a69a8fb 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings UAVOBJSRCFILENAMES += stabilizationsettingsbank1 UAVOBJSRCFILENAMES += stabilizationsettingsbank2 UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus UAVOBJSRCFILENAMES += stabilizationbank UAVOBJSRCFILENAMES += systemalarms UAVOBJSRCFILENAMES += systemsettings diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 7044cada6..fef38b35e 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -86,6 +86,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings UAVOBJSRCFILENAMES += stabilizationsettingsbank1 UAVOBJSRCFILENAMES += stabilizationsettingsbank2 UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus UAVOBJSRCFILENAMES += stabilizationbank UAVOBJSRCFILENAMES += systemalarms UAVOBJSRCFILENAMES += systemsettings From a9a2c6f76938bd44b50de7f99af5f0e8f31179f3 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 29 Apr 2014 01:06:51 +0200 Subject: [PATCH 16/67] OP-1309 Reimplemented alarm handling and stabilization watchdog - Stabilization refactoring nearly done. needs testing and AltitudeHold reimplementations --- .../modules/Stabilization/inc/stabilization.h | 9 +++- flight/modules/Stabilization/innerloop.c | 50 +++++++++++++++++++ flight/modules/Stabilization/outerloop.c | 1 + flight/modules/Stabilization/stabilization.c | 2 +- 4 files changed, 59 insertions(+), 3 deletions(-) diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index fa0b9694b..3ac8e9092 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -53,7 +53,11 @@ typedef struct { float cruise_control_power_trim; int8_t cruise_control_inverted_power_switch; // WARNING: currently -1 is not fully implemented !!! float cruise_control_neutral_thrust; - } cruiseControl; + } cruiseControl; + struct { + int8_t gyroupdates; + int8_t rateupdates; + } monitor; float rattitude_mode_transition_stick_position; struct pid innerPids[3], outerPids[3]; } StabilizationData; @@ -61,7 +65,8 @@ typedef struct { extern StabilizationData stabSettings; -#define AXES 4 +#define AXES 4 +#define FAILSAFE_TIMEOUT_MS 30 #endif // STABILIZATION_H diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 6a00c6c3e..b26db7aa0 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -91,6 +91,9 @@ void stabilizationInnerloopInit() callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES); GyroStateConnectCallback(GyroStateUpdatedCb); + + // schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared + PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER); } @@ -101,6 +104,51 @@ void stabilizationInnerloopInit() */ static void stabilizationInnerloopTask() { + // watchdog and error handling + { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); +#endif + uint8_t errorlevel = 0; + // check if outer loop keeps executing + if (stabSettings.monitor.rateupdates > -64) { + stabSettings.monitor.rateupdates--; + } + if (stabSettings.monitor.rateupdates < -3) { + // warning if rate loop skipped more than 2 beats + errorlevel |= 1; + } + if (stabSettings.monitor.rateupdates < -7) { + // error if rate loop skipped more than 7 beats + errorlevel |= 2; + } + // check if gyro keeps updating + if (stabSettings.monitor.gyroupdates < 1) { + // critical if gyro didn't update at all! + errorlevel |= 4; + } + if (stabSettings.monitor.gyroupdates > 1) { + // warning if we missed a gyro update + errorlevel |= 1; + } + if (stabSettings.monitor.gyroupdates > 3) { + // error if we missed 3 gyro updates + errorlevel |= 2; + } + stabSettings.monitor.gyroupdates = 0; + + if (errorlevel & 4) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL); + } else if (errorlevel & 2) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR); + } else if (errorlevel & 1) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + } + } + + RateDesiredData rateDesired; ActuatorDesiredData actuator; StabilizationStatusInnerLoopData enabled; @@ -198,6 +246,7 @@ static void stabilizationInnerloopTask() } } } + PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER); } @@ -212,6 +261,7 @@ static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha); PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + stabSettings.monitor.gyroupdates++; } #ifdef REVOLUTION diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 53fbab13a..9237d574e 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -254,6 +254,7 @@ static void stabilizationOuterloopTask() // update cruisecontrol based on attitude cruisecontrol_compute_factor(&attitudeState); + stabSettings.monitor.rateupdates = 0; } diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 770b84449..d78281257 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -82,7 +82,7 @@ int32_t StabilizationStart() FlightModeSwitchUpdatedCb(ManualControlCommandHandle()); #ifdef PIOS_INCLUDE_WDG -// PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); + PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); #endif return 0; } From 1632eeb58f04cf5e85e3e59f5c3cce681fbc159c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 29 Apr 2014 15:51:56 +0200 Subject: [PATCH 17/67] OP-1309 some small fixes to Stabilization as result of simulator testing --- flight/libraries/sanitycheck.c | 1 + flight/modules/Stabilization/innerloop.c | 20 +++++++++++--------- flight/modules/Stabilization/outerloop.c | 2 +- shared/uavobjectdefinition/ratedesired.xml | 1 + 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index d46304662..55d5e272c 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -239,6 +239,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY + || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL )) { return SYSTEMALARMS_ALARM_ERROR; } diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index b26db7aa0..95373b48d 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -109,39 +109,41 @@ static void stabilizationInnerloopTask() #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); #endif - uint8_t errorlevel = 0; + bool warn = false; + bool error = false; + bool crit = false; // check if outer loop keeps executing if (stabSettings.monitor.rateupdates > -64) { stabSettings.monitor.rateupdates--; } if (stabSettings.monitor.rateupdates < -3) { // warning if rate loop skipped more than 2 beats - errorlevel |= 1; + warn = true; } if (stabSettings.monitor.rateupdates < -7) { // error if rate loop skipped more than 7 beats - errorlevel |= 2; + error = true; } // check if gyro keeps updating if (stabSettings.monitor.gyroupdates < 1) { // critical if gyro didn't update at all! - errorlevel |= 4; + crit = true; } if (stabSettings.monitor.gyroupdates > 1) { // warning if we missed a gyro update - errorlevel |= 1; + warn = true; } if (stabSettings.monitor.gyroupdates > 3) { // error if we missed 3 gyro updates - errorlevel |= 2; + error = true; } stabSettings.monitor.gyroupdates = 0; - if (errorlevel & 4) { + if (crit) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL); - } else if (errorlevel & 2) { + } else if (error) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR); - } else if (errorlevel & 1) { + } else if (warn) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 9237d574e..c10077a10 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -98,6 +98,7 @@ static void stabilizationOuterloopTask() StabilizationDesiredData stabilizationDesired; StabilizationStatusOuterLoopData enabled; + AttitudeStateGet(&attitudeState); StabilizationDesiredGet(&stabilizationDesired); RateDesiredGet(&rateDesired); StabilizationStatusOuterLoopGet(&enabled); @@ -139,7 +140,6 @@ static void stabilizationOuterloopTask() } #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ } - for (t = 0; t < AXES; t++) { bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t]; diff --git a/shared/uavobjectdefinition/ratedesired.xml b/shared/uavobjectdefinition/ratedesired.xml index 6c297c3e5..bad05ed67 100644 --- a/shared/uavobjectdefinition/ratedesired.xml +++ b/shared/uavobjectdefinition/ratedesired.xml @@ -4,6 +4,7 @@ + From 59bdab697aa56ebec6dc0c217a7eb17d41cb7b99 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 29 Apr 2014 18:17:29 +0200 Subject: [PATCH 18/67] OP-1309 some CPU and memory optimizations to get it to run on CC --- flight/modules/Stabilization/cruisecontrol.c | 6 ++++++ flight/modules/Stabilization/inc/stabilization.h | 11 +++++++++++ flight/modules/Stabilization/innerloop.c | 10 ++++------ flight/modules/Stabilization/outerloop.c | 12 +++++++----- .../boards/coptercontrol/firmware/inc/pios_config.h | 2 +- shared/uavobjectdefinition/stabilizationstatus.xml | 2 +- 6 files changed, 30 insertions(+), 13 deletions(-) diff --git a/flight/modules/Stabilization/cruisecontrol.c b/flight/modules/Stabilization/cruisecontrol.c index 9b86608c8..c7444faa9 100644 --- a/flight/modules/Stabilization/cruisecontrol.c +++ b/flight/modules/Stabilization/cruisecontrol.c @@ -38,6 +38,12 @@ static float cruisecontrol_factor = 1.0f; void cruisecontrol_compute_factor(AttitudeStateData *attitude) { + // safe CPU, only execute every 8th attitude update + static uint8_t cpusafer = 0; + + if (cpusafer++ % (8 / OUTERLOOP_SKIPCOUNT)) { + return; + } float angleCosine; // get attitude state and calculate angle diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 3ac8e9092..a857fba9b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -68,6 +68,17 @@ extern StabilizationData stabSettings; #define AXES 4 #define FAILSAFE_TIMEOUT_MS 30 +#ifndef PIOS_STABILIZATION_STACK_SIZE +#define STACK_SIZE_BYTES 800 +#else +#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE +#endif + +// must be same as eventdispatcher to avoid needing additional mutexes +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +// outer loop only executes every 4th uavobject update to safe CPU +#define OUTERLOOP_SKIPCOUNT 4 #endif // STABILIZATION_H diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 95373b48d..b04556cb6 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -50,9 +50,7 @@ #include // Private constants -#define STACK_SIZE_BYTES 256 -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL #define UPDATE_EXPECTED (1.0f / 666.0f) @@ -116,12 +114,12 @@ static void stabilizationInnerloopTask() if (stabSettings.monitor.rateupdates > -64) { stabSettings.monitor.rateupdates--; } - if (stabSettings.monitor.rateupdates < -3) { - // warning if rate loop skipped more than 2 beats + if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) { + // warning if rate loop skipped more than 2 execution warn = true; } - if (stabSettings.monitor.rateupdates < -7) { - // error if rate loop skipped more than 7 beats + if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) { + // error if rate loop skipped more than 4 executions error = true; } // check if gyro keeps updating diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index c10077a10..853684634 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -49,9 +49,7 @@ #include // Private constants -#define STACK_SIZE_BYTES 256 -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define UPDATE_EXPECTED (1.0f / 666.0f) @@ -260,10 +258,14 @@ static void stabilizationOuterloopTask() static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! - AttitudeStateGet(&attitude); + // to reduce CPU utilisation, outer loop is not executed every state update + static uint8_t cpusafer = 0; - PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + if ((cpusafer++ % OUTERLOOP_SKIPCOUNT) == 0) { + // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! + AttitudeStateGet(&attitude); + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + } } /** diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 13697942d..d3a1aa2f9 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -160,7 +160,7 @@ #define PIOS_ACTUATOR_STACK_SIZE 820 #define PIOS_MANUAL_STACK_SIZE 635 #define PIOS_RECEIVER_STACK_SIZE 620 -#define PIOS_STABILIZATION_STACK_SIZE 780 +#define PIOS_STABILIZATION_STACK_SIZE 400 #ifdef DIAG_TASKS #define PIOS_SYSTEM_STACK_SIZE 740 diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index f58200853..0a3788b76 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -22,7 +22,7 @@ - + From 2f572995ac47e07fe13df9670beee681b21bf165 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 29 Apr 2014 18:36:53 +0200 Subject: [PATCH 19/67] OP-1309 turned boundf() static inline as requested --- flight/libraries/math/mathmisc.c | 19 +------------------ flight/libraries/math/mathmisc.h | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 19 deletions(-) diff --git a/flight/libraries/math/mathmisc.c b/flight/libraries/math/mathmisc.c index d54d18908..25ddc9559 100644 --- a/flight/libraries/math/mathmisc.c +++ b/flight/libraries/math/mathmisc.c @@ -29,21 +29,4 @@ */ -// returns min(boundary1,boundary2) if valmax(boundary1,boundary2) -// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2) -float boundf(float val, float boundary1, float boundary2) -{ - if (boundary1 > boundary2) { - float tmp = boundary2; - boundary2 = boundary1; - boundary1 = tmp; - } - if (!(val >= boundary1)) { - val = boundary1; - } - if (!(val <= boundary2)) { - val = boundary2; - } - return val; -} +// space deliberately left empty, any non inline misc math functions can go here diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index c989ccffe..573743c51 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -34,6 +34,20 @@ // returns min(boundary1,boundary2) if valmax(boundary1,boundary2) // returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2) -float boundf(float val, float boundary1, float boundary2); +static inline float boundf(float val, float boundary1, float boundary2) +{ + if (boundary1 > boundary2) { + float tmp = boundary2; + boundary2 = boundary1; + boundary1 = tmp; + } + if (!(val >= boundary1)) { + val = boundary1; + } + if (!(val <= boundary2)) { + val = boundary2; + } + return val; +} #endif /* MATHMISC_H */ From 66f2f8e8401c83a41e19eec163dee49524051aba Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 29 Apr 2014 19:55:34 +0200 Subject: [PATCH 20/67] OP-1309 more performant implementation of boundf() --- flight/libraries/math/mathmisc.h | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index 573743c51..b4c8b75b8 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -37,15 +37,17 @@ static inline float boundf(float val, float boundary1, float boundary2) { if (boundary1 > boundary2) { - float tmp = boundary2; - boundary2 = boundary1; - boundary1 = tmp; - } - if (!(val >= boundary1)) { - val = boundary1; - } - if (!(val <= boundary2)) { - val = boundary2; + if (!(val >= boundary2)) { + return boundary2; + } else if (!(val <= boundary1)) { + return boundary1; + } + } else { + if (!(val >= boundary1)) { + return boundary1; + } else if (!(val <= boundary2)) { + return boundary2; + } } return val; } From 567b86167fd3346766894c6c525fa34cd26102e2 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Tue, 29 Apr 2014 21:25:23 +0200 Subject: [PATCH 21/67] OP-1303 Corrects the red-cross (bis) --- .../diagrams/default/system-health.svg | 41 +++++++++++-------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index fdf08b021..0d90149cb 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -27,15 +27,15 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="7.4629555" - inkscape:cx="49.128572" - inkscape:cy="46.308281" - inkscape:current-layer="foreground" + inkscape:zoom="14.925911" + inkscape:cx="31.919641" + inkscape:cy="57.332322" + inkscape:current-layer="layer24" id="namedview3608" showgrid="true" - inkscape:window-width="1366" - inkscape:window-height="712" - inkscape:window-x="-4" + inkscape:window-width="1280" + inkscape:window-height="988" + inkscape:window-x="1596" inkscape:window-y="-4" inkscape:window-maximized="1" showguides="true" @@ -1913,10 +1913,11 @@ inkscape:groupmode="layer" id="layer5" inkscape:label="GPS-Error" - style="display:none"> + style="display:inline"> + inkscape:label="#g3406" + transform="matrix(1.0377771,0,0,1,-0.27728602,0)"> 5 - - 0.000100000000000 @@ -15943,7 +15941,6 @@ border-radius: 5; element:Kp haslimits:no scale:1 - buttongroup:5,20 @@ -17355,7 +17352,6 @@ border-radius: 5; 26 26 26 - @@ -19421,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)); @@ -19946,7 +19941,6 @@ border-radius: 5; 39 39 39 - @@ -21981,7 +21975,6 @@ border-radius: 5; - @@ -24186,6 +24179,12 @@ border-radius: 5; + + + 75 + true + + 9 @@ -24200,30 +24199,23 @@ border-radius: 5; 9 - + + + + 75 + 0 + + + + Qt::StrongFocus + - <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> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - 0 - - - -3.000000000000000 - - - 2.000000000000000 + <html><head/><body><p>CP helis can set this to Reversed to automatically reverse the direction of thrust when inverted. Fixed pitch copters, including multicopters, should leave this set at Unreversed. Unreversed direction with Boosted power may be dangerous because it adds power and the thrust direction moves the aircraft towards the ground.</p></body></html> objname:StabilizationSettings - fieldname:CruiseControlInvertedPowerSwitch + fieldname:CruiseControlInvertedThrustReversing haslimits:no scale:1 buttongroup:16 @@ -24231,6 +24223,39 @@ border-radius: 5; + + + + + 0 + 0 + + + + + 145 + 16 + + + + + 75 + 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)); +color: rgb(255, 255, 255); +border-radius: 5; + + + InvrtdThrustRev + + + Qt::AlignCenter + + + @@ -24247,7 +24272,7 @@ border-radius: 5; - <html><head/><body><p>This is the bank angle that CruiseControl goes into inverted / power disabled mode. The power for inverted mode is controlled by CruiseControlInvertedPowerSwitch</p></body></html> + <html><head/><body><p>The bank angle where CruiseControl goes into inverted power mode. InvertedThrustReverse and InvertedPower control the direction and amount of power when in inverted mode.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -24259,7 +24284,7 @@ border-radius: 5; 0 - 180.000000000000000 + 255.000000000000000 105.000000000000000 @@ -24278,7 +24303,7 @@ border-radius: 5; - <html><head/><body><p>Really just a safety limit. 4.0 means it will not use more than 4 times the power the throttle/collective stick is requesting.</p></body></html> + <html><head/><body><p>Really just a safety limit. 3.0 means it will not use more than 3 times the power the throttle/collective stick is requesting.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -24290,13 +24315,13 @@ border-radius: 5; 2 - 0.000000000000000 + 1.000000000000000 50.000000000000000 - 0.250000000000000 + 0.100000000000000 3.000000000000000 @@ -24310,11 +24335,10 @@ border-radius: 5; buttongroup:16 - - + 0 @@ -24323,7 +24347,7 @@ border-radius: 5; - 140 + 155 16 @@ -24339,84 +24363,13 @@ color: rgb(255, 255, 255); border-radius: 5; - PowerTrim + PowerDelayComp Qt::AlignCenter - - - - <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 - - - true - - - 5 - - - 1.000000000000000 - - - 0.001000000000000 - - - 0.250000000000000 - - - - objname:StabilizationSettings - fieldname:CruiseControlPowerDelayComp - haslimits:no - scale:1 - buttongroup:16 - - - - - - - - <html><head/><body><p>If you find that banging the stick around a lot makes the copter climb a bit, adjust this number down a little.</p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - 2 - - - 80.000000000000000 - - - 120.000000000000000 - - - 0.250000000000000 - - - 100.000000000000000 - - - - objname:StabilizationSettings - fieldname:CruiseControlPowerTrim - haslimits:no - scale:1 - buttongroup:16 - - - - @@ -24460,7 +24413,7 @@ border-radius: 5; - 140 + 90 16 @@ -24493,7 +24446,7 @@ border-radius: 5; - 140 + 92 16 @@ -24519,7 +24472,7 @@ border-radius: 5; - <html><head/><body><p>Throttle/Collective stick below this disables Cruise Control. Also, by default Cruise Control forces the use of this value for thrust when the copter is inverted. For safety, never set this so low that the trimmed throttle/collective stick cannot get below it.</p></body></html> + <html><head/><body><p>Throttle/Collective stick below this amount disables Cruise Control. Also, by default Cruise Control forces the use of this value for thrust when InvertedPower setting is Zero and the copter is inverted. CP helis probably want this set to -100%. For safety with fixed pitch copters (including multicopters), never set this so low that the trimmed throttle stick cannot get below it or your motor(s) will still run with the throttle stick all the way down. Fixed pitch throttle sticks go from -100 to 0 in the first tiny bit of throttle stick (and then up to 100 using the rest of the throttle range), so for example, a lot of &quot;high throttle trim&quot; will keep the stick from ever commanding less than 5% so it won't be possible to stop the motors with the throttle stick. Banking the copter in your hand in this state will make the motors speed up.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -24530,6 +24483,12 @@ border-radius: 5; 0 + + -100.000000000000000 + + + 49.000000000000000 + 5.000000000000000 @@ -24544,43 +24503,10 @@ border-radius: 5; - - - - - 0 - 0 - - - - - 140 - 16 - - - - - 75 - 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)); -color: rgb(255, 255, 255); -border-radius: 5; - - - PowerDelayComp - - - Qt::AlignCenter - - - - <html><head/><body><p>Multi-copters should probably use 90% to 95% to leave some headroom for stabilization. CP helis can set this to 100%.</p></body></html> + <html><head/><body><p>Multi-copters should probably use 80% to 90% to leave some headroom for stabilization. CP helis can set this to 100%.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -24591,6 +24517,9 @@ border-radius: 5; 0 + + 51.000000000000000 + 90.000000000000000 @@ -24615,7 +24544,7 @@ border-radius: 5; - 140 + 97 16 @@ -24638,8 +24567,24 @@ border-radius: 5; - - + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 90 + 11 + + + + + + 0 @@ -24648,7 +24593,68 @@ border-radius: 5; - 140 + 97 + 16 + + + + + 75 + 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)); +color: rgb(255, 255, 255); +border-radius: 5; + + + PowerTrim + + + Qt::AlignCenter + + + + + + + <html><head/><body><p>Moves the inverted mode power switching this fraction of a second into the future to compensate for slow responding power systems. Used to more accurately time the changing of power when entering/exiting inverted mode. Set this to equal the fraction of a second it takes for your vehicle to go from full negative thrust (10% positive thrust for multis, full negative thrust (-90%) for CP helis) to full positive thrust (+90%). Increase this if for example continuous left flips &quot;walk off&quot; to the left because power is changed too late.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 5 + + + 2.000000000000000 + + + 0.001000000000000 + + + 0.250000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlPowerDelayComp + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 120 16 @@ -24671,21 +24677,58 @@ border-radius: 5; - - - - Qt::Horizontal + + + + <html><head/><body><p>If you find that quickly moving the stick around a lot makes the copter climb a bit, adjust this number down a little. It will be a compromise between climbing a little with lots of stick motion and descending a little with minimal stick motion.</p></body></html> - - QSizePolicy::Fixed + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 90 - 11 - + + true - + + 2 + + + 50.000000000000000 + + + 200.000000000000000 + + + 0.250000000000000 + + + 100.000000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlPowerTrim + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + <html><head/><body><p>The amount of power used when in inverted mode. Zero (min throttle stick for fixed pitch copters includding multicopters, neutral collective for CP), Normal (uses stick value), or Boosted (boosted according to bank angle). Beginning multicopter pilots should leave this set to Zero to automatically reduce throttle during flips. Boosted power with Unreversed direction may be dangerous because it adds power and the thrust direction moves the aircraft towards the ground.</p></body></html> + + + + objname:StabilizationSettings + fieldname:CruiseControlInvertedPowerOutput + haslimits:no + scale:1 + buttongroup:16 + + + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 09edbfd18..1636544f4 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -30,15 +30,17 @@ - + - + + + From b7a2931f2112ed16eadb13e2e719f1cc0565ea9f Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 29 Apr 2014 20:46:57 -0400 Subject: [PATCH 23/67] OP-1259 Change a constant from a testing value to release value --- flight/modules/Stabilization/stabilization.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 828ede3aa..d5d9c3a91 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -690,8 +690,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // exaggerate the length of time the inverted to upright // transition holds full throttle and reduce the length // of time for full throttle when going upright to inverted. - if (actuatorDesired.Thrust > 0.95f) { -// change this to 0.7 + if (actuatorDesired.Thrust > 0.7f) { float thrust; thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust); From 0a5811fcf9bc19061a716efec78e9ca30d5441c6 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Wed, 30 Apr 2014 13:52:37 +0200 Subject: [PATCH 24/67] OP-1303 Change the receiver and actuator boxes --- .../diagrams/default/system-health.svg | 724 ++++++++++-------- 1 file changed, 391 insertions(+), 333 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 0d90149cb..a2671330c 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -27,15 +27,15 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="14.925911" - inkscape:cx="31.919641" - inkscape:cy="57.332322" - inkscape:current-layer="layer24" + inkscape:zoom="7.4629556" + inkscape:cx="49.946716" + inkscape:cy="37.450437" + inkscape:current-layer="background" id="namedview3608" showgrid="true" - inkscape:window-width="1280" - inkscape:window-height="988" - inkscape:window-x="1596" + inkscape:window-width="1366" + inkscape:window-height="712" + inkscape:window-x="-4" inkscape:window-y="-4" inkscape:window-maximized="1" showguides="true" @@ -705,7 +705,7 @@ id="rect4550-8-7-82" width="10.470912" height="8.5405388" - x="533.97754" + x="547.97754" y="411.38937" ry="1" /> @@ -779,7 +779,7 @@ id="OutOfMemory" width="10.470912" height="8.5405388" - x="530.5553" + x="544.5553" y="411.4689" ry="1" inkscape:label="#rect4550-8-7" /> @@ -788,7 +788,7 @@ id="StackOverflow" width="10.470912" height="8.5405388" - x="542.32214" + x="556.32214" y="411.4689" ry="1" inkscape:label="#rect4550-8-1" /> @@ -797,7 +797,7 @@ id="EventSystem" width="10.470912" height="8.5405388" - x="554.08905" + x="568.08905" y="411.4689" ry="1" inkscape:label="#rect4550-8-15" /> @@ -806,7 +806,7 @@ id="I2C" width="10.470912" height="8.5405388" - x="565.8559" + x="579.8559" y="411.4689" ry="1" inkscape:label="#rect4550-8-2" /> @@ -849,56 +849,56 @@ xml:space="preserve" style="font-size:3.38406372px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#f4d7d7;fill-opacity:1;stroke:none;display:inline;font-family:Sans;-inkscape-font-specification:Sans Bold" x="-447.34424" - y="452.20279" + y="464.44702" id="text4641-5-2-1" sodipodi:linespacing="125%" transform="matrix(0,-0.87458357,1.1434013,0,0,0)">Misc. + y="464.44702">Misc. Link + y="464.46683">Link Power + y="486.72632">Power Syst. + y="464.46683">Syst. @@ -907,7 +907,7 @@ id="rect4550-8-1-4-21-9" width="13.310432" height="10.308098" - x="547.6792" + x="561.6792" y="382.58969" ry="1" /> @@ -924,7 +924,7 @@ id="FlightTime" width="15.447426" height="10.265867" - x="560.46124" + x="574.46124" y="396.60678" ry="1" inkscape:label="#rect4550-8-1-4-21-2-9" /> @@ -933,7 +933,7 @@ id="Telemetry" width="18.966711" height="10.202584" - x="518.13593" + x="532.13593" y="396.63843" ry="1" inkscape:label="#rect4550-8-1-4-21-5" /> @@ -1013,34 +1013,90 @@ id="Battery" width="15.447426" height="10.265867" - x="543.75928" + x="557.75928" y="396.60678" ry="1" inkscape:label="#rect4550-8-1-4-21-2-9-8" /> - + + I/O + + + + + @@ -1162,13 +1217,13 @@ inkscape:groupmode="layer" id="layer13" inkscape:label="BootFault-OK" - style="display:none"> + style="display:inline"> @@ -1176,15 +1231,14 @@ inkscape:groupmode="layer" id="layer15" inkscape:label="Battery-OK" - style="display:none"> + style="display:inline"> @@ -1192,15 +1246,14 @@ inkscape:groupmode="layer" id="layer14" inkscape:label="Telemetry-OK" - style="display:none"> + style="display:inline"> @@ -1208,15 +1261,14 @@ inkscape:groupmode="layer" id="layer16" inkscape:label="FlightTime-OK" - style="display:none"> + style="display:inline"> @@ -1224,13 +1276,13 @@ inkscape:groupmode="layer" id="layer21" inkscape:label="I2C-OK" - style="display:none"> + style="display:inline"> @@ -1239,13 +1291,13 @@ inkscape:groupmode="layer" id="layer20" inkscape:label="EventSystem-OK" - style="display:none"> + style="display:inline"> @@ -1254,13 +1306,13 @@ inkscape:groupmode="layer" id="layer19" inkscape:label="StackOverflow-OK" - style="display:none"> + style="display:inline"> @@ -1269,13 +1321,13 @@ inkscape:groupmode="layer" id="layer18" inkscape:label="OutOfMemory-OK" - style="display:none"> + style="display:inline"> @@ -1284,13 +1336,13 @@ inkscape:groupmode="layer" id="layer17" inkscape:label="CPUOverload-OK" - style="display:none"> + style="display:inline"> @@ -1299,39 +1351,39 @@ inkscape:groupmode="layer" id="layer22" inkscape:label="Receiver-OK" - style="display:none"> + style="display:inline"> + width="18.963711" + height="10.260815" + x="7.6226416" + y="38.370422" + ry="0.99888694" + inkscape:label="#rect4550" + rx="0.88121402" /> @@ -1453,13 +1504,13 @@ inkscape:groupmode="layer" id="g5065" inkscape:label="BootFault-Warning" - style="display:none"> + style="display:inline"> @@ -1467,15 +1518,14 @@ inkscape:groupmode="layer" id="g5069" inkscape:label="Battery-Warning" - style="display:none"> + style="display:inline"> @@ -1483,15 +1533,14 @@ inkscape:groupmode="layer" id="g5072" inkscape:label="Telemetry-Warning" - style="display:none"> + style="display:inline"> @@ -1499,15 +1548,14 @@ inkscape:groupmode="layer" id="g5075" inkscape:label="FlightTime-Warning" - style="display:none"> + style="display:inline"> @@ -1515,13 +1563,13 @@ inkscape:groupmode="layer" id="g5078" inkscape:label="I2C-Warning" - style="display:none"> + style="display:inline"> @@ -1530,13 +1578,13 @@ inkscape:groupmode="layer" id="g5081" inkscape:label="EventSystem-Warning" - style="display:none"> + style="display:inline"> @@ -1545,13 +1593,13 @@ inkscape:groupmode="layer" id="g5084" inkscape:label="StackOverflow-Warning" - style="display:none"> + style="display:inline"> @@ -1560,13 +1608,13 @@ inkscape:groupmode="layer" id="g5087" inkscape:label="OutOfMemory-Warning" - style="display:none"> + style="display:inline"> @@ -1575,13 +1623,13 @@ inkscape:groupmode="layer" id="g5090" inkscape:label="CPUOverload-Warning" - style="display:none"> + style="display:inline"> @@ -1590,17 +1638,17 @@ inkscape:groupmode="layer" id="g5093" inkscape:label="Receiver-Warning" - style="display:none"> + style="display:inline"> + width="18.821587" + height="10.21344" + x="7.7214546" + y="38.403919" + ry="0.79517722" + inkscape:label="#rect4550" + rx="0.8158862" /> + width="18.91176" + height="10.037488" + x="7.6274734" + y="52.645531" + ry="0.92773163" + inkscape:label="#rect4550" + rx="0.9812547" /> @@ -1744,13 +1791,13 @@ inkscape:groupmode="layer" id="g5441" inkscape:label="BootFault-Critical" - style="display:none"> + style="display:inline"> @@ -1758,15 +1805,14 @@ inkscape:groupmode="layer" id="g5445" inkscape:label="Battery-Critical" - style="display:none"> + style="display:inline"> @@ -1774,15 +1820,14 @@ inkscape:groupmode="layer" id="g5448" inkscape:label="Telemetry-Critical" - style="display:none"> + style="display:inline"> @@ -1790,15 +1835,14 @@ inkscape:groupmode="layer" id="g5451" inkscape:label="FlightTime-Critical" - style="display:none"> + style="display:inline"> @@ -1806,13 +1850,13 @@ inkscape:groupmode="layer" id="g5454" inkscape:label="I2C-Critical" - style="display:none"> + style="display:inline"> @@ -1821,13 +1865,13 @@ inkscape:groupmode="layer" id="g5457" inkscape:label="EventSystem-Critical" - style="display:none"> + style="display:inline"> @@ -1836,13 +1880,13 @@ inkscape:groupmode="layer" id="g5460" inkscape:label="StackOverflow-Critical" - style="display:none"> + style="display:inline"> @@ -1851,13 +1895,13 @@ inkscape:groupmode="layer" id="g5463" inkscape:label="OutOfMemory-Critical" - style="display:none"> + style="display:inline"> @@ -1866,13 +1910,13 @@ inkscape:groupmode="layer" id="g5466" inkscape:label="CPUOverload-Critical" - style="display:none"> + style="display:inline"> @@ -1881,17 +1925,17 @@ inkscape:groupmode="layer" id="g5469" inkscape:label="Receiver-Critical" - style="display:none"> + style="display:inline"> + width="18.940025" + height="10.213441" + x="7.6267047" + y="38.417793" + ry="1.1877477" + inkscape:label="#rect4550" + rx="1.2082177" /> + width="18.874826" + height="10.081664" + x="7.663065" + y="52.603611" + ry="0.77868479" + inkscape:label="#rect4550" + rx="0.79499185" /> diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 63858419d..3e922afcd 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -35,6 +35,7 @@ #define DFU_DEBUG true +const int UploaderGadgetWidget::BOARD_EVENT_TIMEOUT = 20000; const int UploaderGadgetWidget::AUTOUPDATE_CLOSE_TIMEOUT = 7000; TimedDialog::TimedDialog(const QString &title, const QString &labelText, int timeout, QWidget *parent, Qt::WindowFlags flags) : @@ -75,6 +76,48 @@ void TimedDialog::perform() } } +ConnectionWaiter::ConnectionWaiter(int targetDeviceCount, int timeout, QWidget *parent) : QObject(parent), eventLoop(this), timer(this), timeout(timeout), elapsed(0), targetDeviceCount(targetDeviceCount), result(ConnectionWaiter::Ok) +{ +} + +int ConnectionWaiter::exec() { + connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(deviceEvent())); + connect(USBMonitor::instance(), SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(deviceEvent())); + + connect(&timer, SIGNAL(timeout()), this, SLOT(perform())); + timer.start(1000); + + emit timeChanged(0); + eventLoop.exec(); + + return result; +} + +void ConnectionWaiter::quit() { + disconnect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(deviceEvent())); + disconnect(USBMonitor::instance(), SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(deviceEvent())); + timer.stop(); + eventLoop.exit(); +} + +void ConnectionWaiter::perform() +{ + ++elapsed; + emit timeChanged(elapsed); + int remaining = timeout - elapsed * 1000; + if (remaining <= 0) { + result = ConnectionWaiter::TimedOut; + quit(); + } +} + +void ConnectionWaiter::deviceEvent() +{ + if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() == targetDeviceCount) { + quit(); + } +} + UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) { m_config = new Ui_UploaderWidget(); @@ -82,9 +125,8 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) currentStep = IAP_STATE_READY; resetOnly = false; dfu = NULL; - m_timer = 0; - m_progress = 0; msg = new QErrorMessage(this); + // Listen to autopilot connection events ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); TelemetryManager *telMngr = pm->getObject(); @@ -124,7 +166,6 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) } } - bool sortPorts(const QSerialPortInfo &s1, const QSerialPortInfo &s2) { return s1.portName() < s2.portName(); @@ -569,31 +610,25 @@ bool UploaderGadgetWidget::autoUpdate() delete dfu; dfu = NULL; } - QEventLoop loop; - QTimer timer; - timer.setSingleShot(true); - connect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); - while (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { - emit autoUpdateSignal(WAITING_DISCONNECT, QVariant()); - if (QMessageBox::warning(this, tr("OpenPilot Uploader"), tr("Please disconnect your OpenPilot board"), QMessageBox::Ok, QMessageBox::Cancel) == QMessageBox::Cancel) { - emit autoUpdateSignal(FAILURE, QVariant()); + + if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { + // wait for all boards to be disconnected + ConnectionWaiter waiter(0, BOARD_EVENT_TIMEOUT); + connect(&waiter, SIGNAL(timeChanged(int)), this, SLOT(autoUpdateDisconnectProgress(int))); + if (waiter.exec() == ConnectionWaiter::TimedOut) { + emit autoUpdateSignal(FAILURE, QVariant(tr("Timed out while waiting for all boards to be disconnected!"))); return false; } - sleep(500); } - emit autoUpdateSignal(WAITING_CONNECT, 0); - autoUpdateConnectTimeout = 0; - m_timer = new QTimer(this); - connect(m_timer, SIGNAL(timeout()), this, SLOT(performAuto())); - m_timer->start(1000); - connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &m_eventloop, SLOT(quit())); - m_eventloop.exec(); - if (!m_timer->isActive()) { - m_timer->stop(); - emit autoUpdateSignal(FAILURE, QVariant()); + + // wait for a board to connect + ConnectionWaiter waiter(1, BOARD_EVENT_TIMEOUT); + connect(&waiter, SIGNAL(timeChanged(int)), this, SLOT(autoUpdateConnectProgress(int))); + if (waiter.exec() == ConnectionWaiter::TimedOut) { + emit autoUpdateSignal(FAILURE, QVariant(tr("Timed out while waiting for a board to be connected!"))); return false; } - m_timer->stop(); + dfu = new DFUObject(DFU_DEBUG, false, QString()); dfu->AbortOperation(); emit autoUpdateSignal(JUMP_TO_BL, QVariant()); @@ -618,6 +653,7 @@ bool UploaderGadgetWidget::autoUpdate() emit autoUpdateSignal(FAILURE, QVariant()); return false; } + QString filename; emit autoUpdateSignal(LOADING_FW, QVariant()); switch (dfu->devices[0].ID) { @@ -654,9 +690,10 @@ bool UploaderGadgetWidget::autoUpdate() emit autoUpdateSignal(FAILURE, QVariant()); return false; } + QEventLoop eventLoop; firmware = file.readAll(); connect(dfu, SIGNAL(progressUpdated(int)), this, SLOT(autoUpdateProgress(int))); - connect(dfu, SIGNAL(uploadFinished(OP_DFU::Status)), &m_eventloop, SLOT(quit())); + connect(dfu, SIGNAL(uploadFinished(OP_DFU::Status)), &eventLoop, SLOT(quit())); emit autoUpdateSignal(UPLOADING_FW, QVariant()); if (!dfu->enterDFU(0)) { emit autoUpdateSignal(FAILURE, QVariant()); @@ -667,7 +704,7 @@ bool UploaderGadgetWidget::autoUpdate() emit autoUpdateSignal(FAILURE, QVariant()); return false; } - m_eventloop.exec(); + eventLoop.exec(); QByteArray desc = firmware.right(100); emit autoUpdateSignal(UPLOADING_DESC, QVariant()); if (dfu->UploadDescription(desc) != OP_DFU::Last_operation_Success) { @@ -679,11 +716,26 @@ bool UploaderGadgetWidget::autoUpdate() return true; } -void UploaderGadgetWidget::autoUpdateProgress(int value) +void UploaderGadgetWidget::autoUpdateDisconnectProgress(int value) +{ + emit autoUpdateSignal(WAITING_DISCONNECT, value); +} + +void UploaderGadgetWidget::autoUpdateConnectProgress(int value) +{ + emit autoUpdateSignal(WAITING_CONNECT, value); +} + +void UploaderGadgetWidget::autoUpdateFlashProgress(int value) { emit autoUpdateSignal(UPLOADING_FW, value); } +void UploaderGadgetWidget::autoUpdateProgress(int value) +{ + autoUpdateFlashProgress(value); +} + /** Attempt a guided procedure to put both boards in BL mode when the system is not bootable @@ -798,16 +850,6 @@ void UploaderGadgetWidget::systemRescue() currentStep = IAP_STATE_BOOTLOADER; } -void UploaderGadgetWidget::performAuto() -{ - ++autoUpdateConnectTimeout; - emit autoUpdateSignal(WAITING_CONNECT, autoUpdateConnectTimeout * 5); - if (autoUpdateConnectTimeout == 20) { - m_timer->stop(); - m_eventloop.exit(); - } -} - void UploaderGadgetWidget::uploadStarted() { m_config->haltButton->setEnabled(false); @@ -852,6 +894,7 @@ void UploaderGadgetWidget::startAutoUpdate() m_config->splitter->setEnabled(false); m_config->autoUpdateGroupBox->setVisible(true); m_config->autoUpdateOkButton->setEnabled(false); + connect(this, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant)), this, SLOT(autoUpdateStatus(uploader::AutoUpdateStep, QVariant))); autoUpdate(); } @@ -860,14 +903,13 @@ void UploaderGadgetWidget::finishAutoUpdate() { disconnect(this, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant)), this, SLOT(autoUpdateStatus(uploader::AutoUpdateStep, QVariant))); m_config->autoUpdateOkButton->setEnabled(true); - connect(&autoUpdateCloseTimer, SIGNAL(timeout()), this, SLOT(closeAutoUpdate())); - autoUpdateCloseTimer.start(AUTOUPDATE_CLOSE_TIMEOUT); + + // wait a bit and "close" auto update + QTimer::singleShot(AUTOUPDATE_CLOSE_TIMEOUT, this, SLOT(closeAutoUpdate())); } void UploaderGadgetWidget::closeAutoUpdate() { - autoUpdateCloseTimer.stop(); - disconnect(&autoUpdateCloseTimer, SIGNAL(timeout()), this, SLOT(closeAutoUpdate())); m_config->autoUpdateGroupBox->setVisible(false); m_config->buttonFrame->setEnabled(true); m_config->splitter->setEnabled(true); @@ -875,13 +917,22 @@ void UploaderGadgetWidget::closeAutoUpdate() void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVariant value) { + QString msg; + int remaining; switch (status) { case uploader::WAITING_DISCONNECT: m_config->autoUpdateLabel->setText("Waiting for all OpenPilot boards to be disconnected from USB."); + m_config->autoUpdateProgressBar->setMaximum(BOARD_EVENT_TIMEOUT / 1000); + m_config->autoUpdateProgressBar->setValue(value.toInt()); + remaining = m_config->autoUpdateProgressBar->maximum() - m_config->autoUpdateProgressBar->value(); + m_config->autoUpdateProgressBar->setFormat(tr("Timing out in %1 seconds").arg(remaining)); break; case uploader::WAITING_CONNECT: m_config->autoUpdateLabel->setText("Please connect the OpenPilot board to the USB port."); + m_config->autoUpdateProgressBar->setMaximum(BOARD_EVENT_TIMEOUT / 1000); m_config->autoUpdateProgressBar->setValue(value.toInt()); + remaining = m_config->autoUpdateProgressBar->maximum() - m_config->autoUpdateProgressBar->value(); + m_config->autoUpdateProgressBar->setFormat(tr("Timing out in %1 seconds").arg(remaining)); break; case uploader::JUMP_TO_BL: m_config->autoUpdateProgressBar->setValue(0); @@ -892,6 +943,8 @@ void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVa break; case uploader::UPLOADING_FW: m_config->autoUpdateLabel->setText("Uploading firmware to the board."); + m_config->autoUpdateProgressBar->setFormat("%p%"); + m_config->autoUpdateProgressBar->setMaximum(100); m_config->autoUpdateProgressBar->setValue(value.toInt()); break; case uploader::UPLOADING_DESC: @@ -901,11 +954,18 @@ void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVa m_config->autoUpdateLabel->setText("Rebooting the board."); break; case uploader::SUCCESS: - m_config->autoUpdateLabel->setText("Board was updated successfully, press OK to finish."); + m_config->autoUpdateProgressBar->setValue(m_config->autoUpdateProgressBar->maximum()); + msg = tr("Board was updated successfully.") + " " + tr("Press OK to finish."); + m_config->autoUpdateLabel->setText(QString("%1").arg(msg)); finishAutoUpdate(); break; case uploader::FAILURE: - m_config->autoUpdateLabel->setText("Something went wrong, you will have to manually upgrade the board. Press OK to continue."); + QString msg = value.toString(); + if (msg.isEmpty()) { + msg = "Something went wrong, you will have to manually upgrade the board."; + } + msg += " " + tr("Press OK to finish."); + m_config->autoUpdateLabel->setText(QString("%1").arg(msg)); finishAutoUpdate(); break; } @@ -936,17 +996,8 @@ UploaderGadgetWidget::~UploaderGadgetWidget() delete qw; qw = 0; } - if (m_progress) { - delete m_progress; - m_progress = 0; - } - if (m_timer) { - delete m_timer; - m_timer = 0; - } } - /** Shows a message box with an error string. diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index baccc5ae7..f5335ef16 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -81,6 +81,38 @@ private: QProgressBar *bar; }; +// A helper class to wait for board connection and disconnection events +// until a the desired number of connected boards is found +// or until a timeout is reached +class ConnectionWaiter: public QObject { + Q_OBJECT + +public: + ConnectionWaiter(int targetDeviceCount, int timeout, QWidget *parent = 0); + + enum DialogCode { Ok, TimedOut }; + +public slots: + int exec(); + void quit(); + +signals: + void timeChanged(int elapsed); + +private slots: + void perform(); + void deviceEvent(); + +private: + QEventLoop eventLoop; + QTimer timer; + // timeour in ms + int timeout; + // elapsed time in seconds + int elapsed; + int targetDeviceCount; + int result; +}; class UPLOADER_EXPORT UploaderGadgetWidget : public QWidget { Q_OBJECT @@ -88,6 +120,9 @@ class UPLOADER_EXPORT UploaderGadgetWidget : public QWidget { public: UploaderGadgetWidget(QWidget *parent = 0); ~UploaderGadgetWidget(); + + static const int BOARD_EVENT_TIMEOUT; + void log(QString str); bool autoUpdateCapable(); @@ -97,7 +132,11 @@ public slots: void populate(); void openHelp(); bool autoUpdate(); + void autoUpdateDisconnectProgress(int); + void autoUpdateConnectProgress(int); + // autoUpdateProgress is deprecated: use autoUpdateFlashProgress instead void autoUpdateProgress(int); + void autoUpdateFlashProgress(int); signals: void autoUpdateSignal(uploader::AutoUpdateStep, QVariant); @@ -109,17 +148,13 @@ private: bool resetOnly; void clearLog(); QString getPortDevice(const QString &friendName); - QProgressDialog *m_progress; - QTimer *m_timer; QLineEdit *openFileNameLE; - QEventLoop m_eventloop; QErrorMessage *msg; void connectSignalSlot(QWidget *widget); - int autoUpdateConnectTimeout; FlightStatus *getFlightStatus(); void bootButtonsSetEnable(bool enabled); static const int AUTOUPDATE_CLOSE_TIMEOUT; - QTimer autoUpdateCloseTimer; + private slots: void onPhysicalHWConnect(); void versionMatchCheck(); @@ -134,7 +169,6 @@ private slots: void commonSystemBoot(bool safeboot = false, bool erase = false); void systemRescue(); void getSerialPorts(); - void performAuto(); void uploadStarted(); void uploadEnded(bool succeed); void downloadStarted(); From 7fef044325cacacd4bdce1d39da992165d4eae5c Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 1 May 2014 23:14:35 +0200 Subject: [PATCH 39/67] OP-1174 wizard upgrade page - removed now uneeded fix that caused the wizard dialog to flicker --- .../setupwizard/pages/autoupdatepage.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index 7c3b9fc63..ef5fc46fe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -39,8 +39,6 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu { switch (status) { case uploader::WAITING_DISCONNECT: - getWizard()->setWindowFlags(getWizard()->windowFlags() & ~Qt::WindowStaysOnTopHint); - getWizard()->setWindowIcon(qApp->windowIcon()); disableButtons(); getWizard()->show(); ui->statusLabel->setText("Waiting for all OP boards to be disconnected"); @@ -49,8 +47,15 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu ui->levellinProgressBar->setValue(value.toInt()); break; case uploader::WAITING_CONNECT: - getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); - getWizard()->setWindowIcon(qApp->windowIcon()); + // Note: + // following two lines of code were probably added to fix an issue when uploader opened a popup requesting + // user to disconnect all boards + // Side effect is that the wizard dialog flickers + // the uploader was changed to avoid popups alltogether and that fix is not need anymore + // same commented fix can be found in FAILURE case and they are kept for future ref. + //getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); + //getWizard()->setWindowIcon(qApp->windowIcon()); + // End of Note disableButtons(); getWizard()->show(); ui->statusLabel->setText("Please connect the board to the USB port (don't use external supply)"); @@ -81,8 +86,8 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu ui->statusLabel->setText("Board updated, please press 'Next' to continue"); break; case uploader::FAILURE: - getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); - getWizard()->setWindowIcon(qApp->windowIcon()); + //getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); + //getWizard()->setWindowIcon(qApp->windowIcon()); enableButtons(true); getWizard()->show(); QString msg = value.toString(); From fababeeeee973c497a8df6e28eec7fdfa789e539 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 1 May 2014 23:19:50 +0200 Subject: [PATCH 40/67] OP-1174 wizard upgrade page - made all strings translatable --- .../setupwizard/pages/autoupdatepage.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index ef5fc46fe..42a9dcd5c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -41,7 +41,7 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu case uploader::WAITING_DISCONNECT: disableButtons(); getWizard()->show(); - ui->statusLabel->setText("Waiting for all OP boards to be disconnected"); + ui->statusLabel->setText(tr("Waiting for all OP boards to be disconnected.")); // TODO get rid of magic number 20s ui->levellinProgressBar->setMaximum(20); ui->levellinProgressBar->setValue(value.toInt()); @@ -58,32 +58,32 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu // End of Note disableButtons(); getWizard()->show(); - ui->statusLabel->setText("Please connect the board to the USB port (don't use external supply)"); + ui->statusLabel->setText(tr("Please connect the board to the USB port (don't use external supply).")); // TODO get rid of magic number 20s ui->levellinProgressBar->setMaximum(20); ui->levellinProgressBar->setValue(value.toInt()); break; case uploader::JUMP_TO_BL: ui->levellinProgressBar->setValue(0); - ui->statusLabel->setText("Board going into bootloader mode"); + ui->statusLabel->setText(tr("Board going into bootloader mode.")); break; case uploader::LOADING_FW: - ui->statusLabel->setText("Loading firmware"); + ui->statusLabel->setText(tr("Loading firmware.")); break; case uploader::UPLOADING_FW: - ui->statusLabel->setText("Uploading firmware"); + ui->statusLabel->setText(tr("Uploading firmware.")); ui->levellinProgressBar->setMaximum(100); ui->levellinProgressBar->setValue(value.toInt()); break; case uploader::UPLOADING_DESC: - ui->statusLabel->setText("Uploading description"); + ui->statusLabel->setText(tr("Uploading description.")); break; case uploader::BOOTING: - ui->statusLabel->setText("Booting the board"); + ui->statusLabel->setText(tr("Booting the board.")); break; case uploader::SUCCESS: enableButtons(true); - ui->statusLabel->setText("Board updated, please press 'Next' to continue"); + ui->statusLabel->setText(tr("Board updated, please press 'Next' to continue.")); break; case uploader::FAILURE: //getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); @@ -92,7 +92,7 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu getWizard()->show(); QString msg = value.toString(); if (msg.isEmpty()) { - msg = "Something went wrong, you will have to manually upgrade the board using the uploader plugin"; + msg = tr("Something went wrong, you will have to manually upgrade the board using the uploader plugin."); } ui->statusLabel->setText(msg); break; From 9132e3d44f9103d2026d55edbe604e96c3a4d1e6 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 1 May 2014 23:21:57 +0200 Subject: [PATCH 41/67] OP-1174 uploader gadget - removed deprecated code and minor renaming --- .../src/plugins/uploader/uploadergadgetwidget.cpp | 13 ++++--------- .../src/plugins/uploader/uploadergadgetwidget.h | 2 -- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 3e922afcd..a9eed6366 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -289,9 +289,9 @@ void UploaderGadgetWidget::onAutopilotDisconnect() static void sleep(int ms) { - QEventLoop eventloop; - QTimer::singleShot(ms, &eventloop, SLOT(quit())); - eventloop.exec(); + QEventLoop eventLoop; + QTimer::singleShot(ms, &eventLoop, SLOT(quit())); + eventLoop.exec(); } /** @@ -692,7 +692,7 @@ bool UploaderGadgetWidget::autoUpdate() } QEventLoop eventLoop; firmware = file.readAll(); - connect(dfu, SIGNAL(progressUpdated(int)), this, SLOT(autoUpdateProgress(int))); + connect(dfu, SIGNAL(progressUpdated(int)), this, SLOT(autoUpdateFlashProgress(int))); connect(dfu, SIGNAL(uploadFinished(OP_DFU::Status)), &eventLoop, SLOT(quit())); emit autoUpdateSignal(UPLOADING_FW, QVariant()); if (!dfu->enterDFU(0)) { @@ -731,11 +731,6 @@ void UploaderGadgetWidget::autoUpdateFlashProgress(int value) emit autoUpdateSignal(UPLOADING_FW, value); } -void UploaderGadgetWidget::autoUpdateProgress(int value) -{ - autoUpdateFlashProgress(value); -} - /** Attempt a guided procedure to put both boards in BL mode when the system is not bootable diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index f5335ef16..50ce58ac2 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -134,8 +134,6 @@ public slots: bool autoUpdate(); void autoUpdateDisconnectProgress(int); void autoUpdateConnectProgress(int); - // autoUpdateProgress is deprecated: use autoUpdateFlashProgress instead - void autoUpdateProgress(int); void autoUpdateFlashProgress(int); signals: From 6dd88da4921abf2e25aca69183ecadfbecf1db3e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 1 May 2014 23:48:17 +0200 Subject: [PATCH 42/67] OP-1174 uploader gadget - made all strings translatable --- .../plugins/uploader/uploadergadgetwidget.cpp | 71 ++++++++++--------- 1 file changed, 36 insertions(+), 35 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index a9eed6366..f93919cd6 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -220,7 +220,7 @@ FlightStatus *UploaderGadgetWidget::getFlightStatus() Q_ASSERT(pm); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager); - FlightStatus *status = dynamic_cast(objManager->getObject(QString("FlightStatus"))); + FlightStatus *status = dynamic_cast(objManager->getObject("FlightStatus")); Q_ASSERT(status); return status; } @@ -267,7 +267,7 @@ void UploaderGadgetWidget::populate() } RunningDeviceWidget *dw = new RunningDeviceWidget(this); dw->populate(); - m_config->systemElements->addTab(dw, QString("Connected Device")); + m_config->systemElements->addTab(dw, tr("Connected Device")); } /** @@ -305,7 +305,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVObject *fwIAP = dynamic_cast(objManager->getObject(QString("FirmwareIAPObj"))); + UAVObject *fwIAP = dynamic_cast(objManager->getObject("FirmwareIAPObj")); switch (currentStep) { case IAP_STATE_READY: @@ -319,12 +319,12 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) connect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); currentStep = IAP_STATE_STEP_1; clearLog(); - log(QString("IAP Step 1")); + log("IAP Step 1"); fwIAP->updated(); break; case IAP_STATE_STEP_1: if (!success) { - log(QString("Oops, failure step 1")); + log("Oops, failure step 1"); log("Reset did NOT happen"); currentStep = IAP_STATE_READY; disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); @@ -334,12 +334,12 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) sleep(600); fwIAP->getField("Command")->setValue("2233"); currentStep = IAP_STATE_STEP_2; - log(QString("IAP Step 2")); + log("IAP Step 2"); fwIAP->updated(); break; case IAP_STATE_STEP_2: if (!success) { - log(QString("Oops, failure step 2")); + log("Oops, failure step 2"); log("Reset did NOT happen"); currentStep = IAP_STATE_READY; disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); @@ -349,7 +349,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) sleep(600); fwIAP->getField("Command")->setValue("3344"); currentStep = IAP_STEP_RESET; - log(QString("IAP Step 3")); + log("IAP Step 3"); fwIAP->updated(); break; case IAP_STEP_RESET: @@ -373,7 +373,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) cm->suspendPolling(); sleep(200); log("Board Halt"); - m_config->boardStatus->setText("Bootloader"); + m_config->boardStatus->setText(tr("Bootloader")); if (dlj.startsWith("USB")) { m_config->telemetryLink->setCurrentIndex(m_config->telemetryLink->findText("USB")); } else { @@ -399,7 +399,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) dfu = NULL; cm->resumePolling(); currentStep = IAP_STATE_READY; - m_config->boardStatus->setText("Bootloader?"); + m_config->boardStatus->setText(tr("Bootloader?")); m_config->haltButton->setEnabled(true); return; } @@ -410,14 +410,14 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) dfu = NULL; cm->resumePolling(); currentStep = IAP_STATE_READY; - m_config->boardStatus->setText("Bootloader?"); + m_config->boardStatus->setText(tr("Bootloader?")); return; } // dfu.StatusRequest(); sleep(500); dfu->findDevices(); - log(QString("Found ") + QString::number(dfu->numberOfDevices) + QString(" device(s).")); + log(QString("Found %1 device(s).").arg(QString::number(dfu->numberOfDevices))); if (dfu->numberOfDevices < 0 || dfu->numberOfDevices > 3) { log("Inconsistent number of devices! Aborting"); delete dfu; @@ -437,7 +437,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) dw->setDeviceID(i); dw->setDfu(dfu); dw->populate(); - m_config->systemElements->addTab(dw, QString("Device") + QString::number(i)); + m_config->systemElements->addTab(dw, tr("Device") + QString::number(i)); } // Need to re-enable in case we were not connected @@ -467,9 +467,9 @@ void UploaderGadgetWidget::systemHalt() goToBootloader(); } else { QMessageBox mbox(this); - mbox.setText(QString(tr("The controller board is armed and can not be halted.\n\n" + mbox.setText(tr("The controller board is armed and can not be halted.\n\n" "Please make sure the board is not armed and then press halt again to proceed\n" - "or use the rescue option to force a firmware upgrade."))); + "or use the rescue option to force a firmware upgrade.")); mbox.setStandardButtons(QMessageBox::Ok); mbox.setIcon(QMessageBox::Warning); mbox.exec(); @@ -498,9 +498,9 @@ void UploaderGadgetWidget::systemReset() goToBootloader(); } else { QMessageBox mbox(this); - mbox.setText(QString(tr("The controller board is armed and can not be reset.\n\n" + mbox.setText(tr("The controller board is armed and can not be reset.\n\n" "Please make sure the board is not armed and then press reset again to proceed\n" - "or power cycle to force a board reset."))); + "or power cycle to force a board reset.")); mbox.setStandardButtons(QMessageBox::Ok); mbox.setIcon(QMessageBox::Warning); mbox.exec(); @@ -573,7 +573,7 @@ void UploaderGadgetWidget::commonSystemBoot(bool safeboot, bool erase) cm->resumePolling(); m_config->rescueButton->setEnabled(true); m_config->telemetryLink->setEnabled(true); - m_config->boardStatus->setText("Running"); + m_config->boardStatus->setText(tr("Running")); if (currentStep == IAP_STATE_BOOTLOADER) { // Freeze the tabs, they are not useful anymore and their buttons // will cause segfaults or weird stuff if we use them. @@ -767,10 +767,10 @@ void UploaderGadgetWidget::systemRescue() log("**********************************************************"); log("You will be prompted to first connect USB, then system power"); - // Check if board is connected and, if yes, prompt user to disconnect board + // Check if boards are connected and, if yes, prompt user to disconnect them all if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { QString labelText = QString("

%1

").arg(tr("Please disconnect your OpenPilot board.")); - TimedDialog progressDlg(tr("System Rescue"), labelText, 20); + TimedDialog progressDlg(tr("System Rescue"), labelText, BOARD_EVENT_TIMEOUT / 1000); connect(USBMonitor::instance(), SIGNAL(deviceRemoved(USBPortInfo)), &progressDlg, SLOT(accept())); switch(progressDlg.exec()) { case TimedDialog::Rejected: @@ -778,15 +778,15 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; case TimedDialog::TimedOut: - QMessageBox::warning(this, tr("System Rescue"), tr("No board disconnection was detected!")); + QMessageBox::warning(this, tr("System Rescue"), tr("Timed out while waiting for all boards to be disconnected!")); m_config->rescueButton->setEnabled(true); return; } } // Now prompt user to connect board - QString labelText = QString("

%1
%2

").arg(tr("Please connect your OpenPilot board.")).arg(tr("Board must be connected by USB!")); - TimedDialog progressDlg(tr("System Rescue"), labelText, 20); + QString labelText = QString("

%1
%2

").arg(tr("Please connect your OpenPilot board.")).arg(tr("Board must be connected to the USB port!")); + TimedDialog progressDlg(tr("System Rescue"), labelText, BOARD_EVENT_TIMEOUT / 1000); connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &progressDlg, SLOT(accept())); switch(progressDlg.exec()) { case TimedDialog::Rejected: @@ -794,7 +794,7 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; case TimedDialog::TimedOut: - QMessageBox::warning(this, tr("System Rescue"), tr("No board connection was detected!")); + QMessageBox::warning(this, tr("System Rescue"), tr("Timed out while waiting for a board to be connected!")); m_config->rescueButton->setEnabled(true); return; } @@ -834,7 +834,7 @@ void UploaderGadgetWidget::systemRescue() dw->setDeviceID(i); dw->setDfu(dfu); dw->populate(); - m_config->systemElements->addTab(dw, QString("Device") + QString::number(i)); + m_config->systemElements->addTab(dw, tr("Device") + QString::number(i)); } m_config->haltButton->setEnabled(false); m_config->resetButton->setEnabled(false); @@ -916,14 +916,14 @@ void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVa int remaining; switch (status) { case uploader::WAITING_DISCONNECT: - m_config->autoUpdateLabel->setText("Waiting for all OpenPilot boards to be disconnected from USB."); + m_config->autoUpdateLabel->setText(tr("Waiting for all OpenPilot boards to be disconnected from USB.")); m_config->autoUpdateProgressBar->setMaximum(BOARD_EVENT_TIMEOUT / 1000); m_config->autoUpdateProgressBar->setValue(value.toInt()); remaining = m_config->autoUpdateProgressBar->maximum() - m_config->autoUpdateProgressBar->value(); m_config->autoUpdateProgressBar->setFormat(tr("Timing out in %1 seconds").arg(remaining)); break; case uploader::WAITING_CONNECT: - m_config->autoUpdateLabel->setText("Please connect the OpenPilot board to the USB port."); + m_config->autoUpdateLabel->setText(tr("Please connect the OpenPilot board to the USB port.")); m_config->autoUpdateProgressBar->setMaximum(BOARD_EVENT_TIMEOUT / 1000); m_config->autoUpdateProgressBar->setValue(value.toInt()); remaining = m_config->autoUpdateProgressBar->maximum() - m_config->autoUpdateProgressBar->value(); @@ -931,33 +931,34 @@ void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVa break; case uploader::JUMP_TO_BL: m_config->autoUpdateProgressBar->setValue(0); - m_config->autoUpdateLabel->setText("Bringing the board into boot loader mode."); + m_config->autoUpdateLabel->setText(tr("Bringing the board into boot loader mode.")); break; case uploader::LOADING_FW: - m_config->autoUpdateLabel->setText("Preparing to upload firmware to the board."); + m_config->autoUpdateLabel->setText(tr("Preparing to upload firmware to the board.")); break; case uploader::UPLOADING_FW: - m_config->autoUpdateLabel->setText("Uploading firmware to the board."); + m_config->autoUpdateLabel->setText(tr("Uploading firmware to the board.")); m_config->autoUpdateProgressBar->setFormat("%p%"); m_config->autoUpdateProgressBar->setMaximum(100); m_config->autoUpdateProgressBar->setValue(value.toInt()); break; case uploader::UPLOADING_DESC: - m_config->autoUpdateLabel->setText("Uploading description of the new firmware to the board."); + m_config->autoUpdateLabel->setText(tr("Uploading description of the new firmware to the board.")); break; case uploader::BOOTING: - m_config->autoUpdateLabel->setText("Rebooting the board."); + m_config->autoUpdateLabel->setText(tr("Rebooting the board.")); break; case uploader::SUCCESS: m_config->autoUpdateProgressBar->setValue(m_config->autoUpdateProgressBar->maximum()); - msg = tr("Board was updated successfully.") + " " + tr("Press OK to finish."); + msg = tr("Board was updated successfully."); + msg += " " + tr("Press OK to finish."); m_config->autoUpdateLabel->setText(QString("%1").arg(msg)); finishAutoUpdate(); break; case uploader::FAILURE: - QString msg = value.toString(); + msg = value.toString(); if (msg.isEmpty()) { - msg = "Something went wrong, you will have to manually upgrade the board."; + msg = tr("Something went wrong, you will have to manually upgrade the board."); } msg += " " + tr("Press OK to finish."); m_config->autoUpdateLabel->setText(QString("%1").arg(msg)); From 9573e141aad5b5854175b42371968cf5c13ff50d Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 2 May 2014 08:53:40 +0200 Subject: [PATCH 43/67] OP-1174 auto update wizard - error messages are now shown in red --- .../src/plugins/setupwizard/pages/autoupdatepage.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index 42a9dcd5c..a1eb2ad8f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -37,6 +37,7 @@ void AutoUpdatePage::enableButtons(bool enable = false) void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant value) { + QString msg; switch (status) { case uploader::WAITING_DISCONNECT: disableButtons(); @@ -94,7 +95,7 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu if (msg.isEmpty()) { msg = tr("Something went wrong, you will have to manually upgrade the board using the uploader plugin."); } - ui->statusLabel->setText(msg); + ui->statusLabel->setText(QString("%1").arg(msg)); break; } } From f4f3f7494804622d60ba91dcc6f35f65ab614422 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 2 May 2014 08:55:46 +0200 Subject: [PATCH 44/67] OP-1174 uploader gadget - now wait for all boards to be disconnected (used to wait for *one* board to be disconnected). This was acheived by reusing new ConnectionWaiter. --- .../plugins/uploader/uploadergadgetwidget.cpp | 50 +++++++++---------- .../plugins/uploader/uploadergadgetwidget.h | 13 ++--- 2 files changed, 28 insertions(+), 35 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index f93919cd6..a7ce9534e 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #define DFU_DEBUG true @@ -51,19 +52,6 @@ TimedDialog::TimedDialog(const QString &title, const QString &labelText, int tim setBar(bar); } -int TimedDialog::exec() { - QTimer timer(this); - connect(&timer, SIGNAL(timeout()), this, SLOT(perform())); - - setValue(0); - - timer.start(1000); - int result = QProgressDialog::exec(); - timer.stop(); - - return result; -} - void TimedDialog::perform() { setValue(value() + 1); @@ -71,8 +59,7 @@ void TimedDialog::perform() if (remaining > 0) { bar->setFormat(tr("Timing out in %1 seconds").arg(remaining)); } else { - setResult(TimedDialog::TimedOut); - close(); + bar->setFormat(tr("Timed out after %1 seconds").arg(bar->maximum())); } } @@ -93,6 +80,11 @@ int ConnectionWaiter::exec() { return result; } +void ConnectionWaiter::cancel() { + quit(); + result = ConnectionWaiter::Canceled; +} + void ConnectionWaiter::quit() { disconnect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(deviceEvent())); disconnect(USBMonitor::instance(), SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(deviceEvent())); @@ -118,6 +110,14 @@ void ConnectionWaiter::deviceEvent() } } +int ConnectionWaiter::openDialog(const QString &title, const QString &labelText, int targetDeviceCount, int timeout, QWidget *parent, Qt::WindowFlags flags) { + TimedDialog dlg(title, labelText, timeout / 1000, parent, flags); + ConnectionWaiter waiter(targetDeviceCount, timeout, parent); + connect(&dlg, SIGNAL(canceled()), &waiter, SLOT(cancel())); + connect(&waiter, SIGNAL(timeChanged(int)), &dlg, SLOT(perform())); + return waiter.exec(); +} + UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) { m_config = new Ui_UploaderWidget(); @@ -770,14 +770,12 @@ void UploaderGadgetWidget::systemRescue() // Check if boards are connected and, if yes, prompt user to disconnect them all if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { QString labelText = QString("

%1

").arg(tr("Please disconnect your OpenPilot board.")); - TimedDialog progressDlg(tr("System Rescue"), labelText, BOARD_EVENT_TIMEOUT / 1000); - connect(USBMonitor::instance(), SIGNAL(deviceRemoved(USBPortInfo)), &progressDlg, SLOT(accept())); - switch(progressDlg.exec()) { - case TimedDialog::Rejected: - // user canceled dialog + int result = ConnectionWaiter::openDialog(tr("System Rescue"), labelText, 0, BOARD_EVENT_TIMEOUT, this); + switch(result) { + case ConnectionWaiter::Canceled: m_config->rescueButton->setEnabled(true); return; - case TimedDialog::TimedOut: + case ConnectionWaiter::TimedOut: QMessageBox::warning(this, tr("System Rescue"), tr("Timed out while waiting for all boards to be disconnected!")); m_config->rescueButton->setEnabled(true); return; @@ -786,14 +784,12 @@ void UploaderGadgetWidget::systemRescue() // Now prompt user to connect board QString labelText = QString("

%1
%2

").arg(tr("Please connect your OpenPilot board.")).arg(tr("Board must be connected to the USB port!")); - TimedDialog progressDlg(tr("System Rescue"), labelText, BOARD_EVENT_TIMEOUT / 1000); - connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &progressDlg, SLOT(accept())); - switch(progressDlg.exec()) { - case TimedDialog::Rejected: - // user canceled dialog + int result = ConnectionWaiter::openDialog(tr("System Rescue"), labelText, 1, BOARD_EVENT_TIMEOUT, this); + switch(result) { + case ConnectionWaiter::Canceled: m_config->rescueButton->setEnabled(true); return; - case TimedDialog::TimedOut: + case ConnectionWaiter::TimedOut: QMessageBox::warning(this, tr("System Rescue"), tr("Timed out while waiting for a board to be connected!")); m_config->rescueButton->setEnabled(true); return; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 50ce58ac2..97a3abd06 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -62,18 +62,12 @@ using namespace uploader; class FlightStatus; -// A dialog that will time out and close after a given delay class TimedDialog: public QProgressDialog { Q_OBJECT public: TimedDialog(const QString &title, const QString &labelText, int timeout, QWidget *parent = 0, Qt::WindowFlags flags = 0); - enum DialogCode { Rejected, Accepted, TimedOut }; - -public slots: - int exec(); - private slots: void perform(); @@ -90,12 +84,15 @@ class ConnectionWaiter: public QObject { public: ConnectionWaiter(int targetDeviceCount, int timeout, QWidget *parent = 0); - enum DialogCode { Ok, TimedOut }; + enum ResultCode { Ok, Canceled, TimedOut }; public slots: int exec(); + void cancel(); void quit(); + static int openDialog(const QString &title, const QString &labelText, int targetDeviceCount, int timeout, QWidget *parent = 0, Qt::WindowFlags flags = 0); + signals: void timeChanged(int elapsed); @@ -106,7 +103,7 @@ private slots: private: QEventLoop eventLoop; QTimer timer; - // timeour in ms + // timeout in ms int timeout; // elapsed time in seconds int elapsed; From 4b379a1816d8535c87cdc49c94ee150e59915b63 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 2 May 2014 08:56:09 +0200 Subject: [PATCH 45/67] OP-1174 unrelated cleanup :) --- .../src/plugins/coreplugin/mainwindow.cpp | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index 197058f29..12b89cd46 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -743,26 +743,15 @@ void MainWindow::registerDefaultActions() tmpaction->setEnabled(true); connect(tmpaction, SIGNAL(triggered()), this, SLOT(showHelp())); - // About sep -#ifndef Q_WS_MAC // doesn't have the "About" actions in the Help menu + // About GCS Action + // Mac doesn't have the "About" actions in the Help menu +#ifndef Q_WS_MAC tmpaction = new QAction(this); tmpaction->setSeparator(true); cmd = am->registerAction(tmpaction, QLatin1String("QtCreator.Help.Sep.About"), m_globalContext); mhelp->addAction(cmd, Constants::G_HELP_ABOUT); #endif - // About GCS Action -#ifdef Q_WS_MAC - -#else - -#endif - -#ifdef Q_WS_MAC - -#endif - connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotGCS())); - // About Plugins Action tmpaction = new QAction(QIcon(Constants::ICON_PLUGIN), tr("About &Plugins..."), this); cmd = am->registerAction(tmpaction, Constants::ABOUT_PLUGINS, m_globalContext); From 40dcd94b5c2f8a72d1ac2fe7142189c2c49c171c Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 2 May 2014 23:36:05 +0200 Subject: [PATCH 46/67] OP-1174 OP-1131 moved firmware version check from uploader gadget to uploader plugin. This ensures that the check is always done even if the uploader gadget is not displayed. --- .../plugins/uploader/uploadergadgetwidget.cpp | 61 +++------------ .../plugins/uploader/uploadergadgetwidget.h | 34 ++------- .../src/plugins/uploader/uploaderplugin.cpp | 74 +++++++++++++++++-- .../src/plugins/uploader/uploaderplugin.h | 6 ++ 4 files changed, 92 insertions(+), 83 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index a7ce9534e..6ac8ee539 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -25,12 +25,20 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uploadergadgetwidget.h" -#include "version_info/version_info.h" -#include "flightstatus.h" +#include "flightstatus.h" +//#include "delay.h" +#include "devicewidget.h" +#include "runningdevicewidget.h" + +#include +#include #include +#include #include +#include +#include #include #include @@ -125,14 +133,11 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) currentStep = IAP_STATE_READY; resetOnly = false; dfu = NULL; - msg = new QErrorMessage(this); // Listen to autopilot connection events ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); TelemetryManager *telMngr = pm->getObject(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); - connect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); - connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); connect(m_config->haltButton, SIGNAL(clicked()), this, SLOT(systemHalt())); @@ -162,7 +167,6 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) // And check whether by any chance we are not already connected if (telMngr->isConnected()) { onAutopilotConnect(); - versionMatchCheck(); } } @@ -459,10 +463,9 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) void UploaderGadgetWidget::systemHalt() { - FlightStatus *status = getFlightStatus(); - // The board can not be halted when in armed state. // If board is armed, or arming. Show message with notice. + FlightStatus *status = getFlightStatus(); if (status->getArmed() == FlightStatus::ARMED_DISARMED) { goToBootloader(); } else { @@ -1024,48 +1027,6 @@ void UploaderGadgetWidget::info(QString infoString, int infoNumber) m_config->boardStatus->setText(infoString); } -void UploaderGadgetWidget::versionMatchCheck() -{ - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager *utilMngr = pm->getObject(); - deviceDescriptorStruct boardDescription = utilMngr->getBoardDescriptionStruct(); - QByteArray uavoHashArray; - QString uavoHash = VersionInfo::uavoHashArray(); - - - uavoHash.chop(2); - uavoHash.remove(0, 2); - uavoHash = uavoHash.trimmed(); - bool ok; - foreach(QString str, uavoHash.split(",")) { - uavoHashArray.append(str.toInt(&ok, 16)); - } - - QByteArray fwVersion = boardDescription.uavoHash; - if (fwVersion != uavoHashArray) { - QString gcsDescription = VersionInfo::revision(); - QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); - gcsGitHash.remove(QRegExp("^[0]*")); - QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ") + 1, 14); - - QString gcsUavoHashStr; - QString fwUavoHashStr; - foreach(char i, fwVersion) { - fwUavoHashStr.append(QString::number(i, 16).right(2)); - } - foreach(char i, uavoHashArray) { - gcsUavoHashStr.append(QString::number(i, 16).right(2)); - } - QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-" + gcsUavoHashStr.left(8) + ")"; - QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; - - QString warning = QString(tr( - "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " - "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); - msg->showMessage(warning); - } -} - void UploaderGadgetWidget::openHelp() { QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/AoBZ", QUrl::StrictMode)); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 97a3abd06..153d94051 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -29,38 +29,18 @@ #define UPLOADERGADGETWIDGET_H #include "ui_uploader.h" -#include "delay.h" -#include "devicewidget.h" -#include "runningdevicewidget.h" -#include "op_dfu.h" -#include -#include - -#include "extensionsystem/pluginmanager.h" -#include "uavobjectmanager.h" -#include "uavobject.h" - -#include "coreplugin/icore.h" -#include "coreplugin/connectionmanager.h" - -#include "ophid/inc/ophid_plugin.h" -#include -#include -#include -#include -#include -#include -#include "devicedescriptorstruct.h" -#include -#include -#include #include "uploader_global.h" + #include "enums.h" +#include "op_dfu.h" + +#include using namespace OP_DFU; using namespace uploader; class FlightStatus; +class UAVObject; class TimedDialog: public QProgressDialog { Q_OBJECT @@ -119,6 +99,7 @@ public: ~UploaderGadgetWidget(); static const int BOARD_EVENT_TIMEOUT; + static const int AUTOUPDATE_CLOSE_TIMEOUT; void log(QString str); bool autoUpdateCapable(); @@ -144,15 +125,12 @@ private: void clearLog(); QString getPortDevice(const QString &friendName); QLineEdit *openFileNameLE; - QErrorMessage *msg; void connectSignalSlot(QWidget *widget); FlightStatus *getFlightStatus(); void bootButtonsSetEnable(bool enabled); - static const int AUTOUPDATE_CLOSE_TIMEOUT; private slots: void onPhysicalHWConnect(); - void versionMatchCheck(); void error(QString errorString, int errorNumber); void info(QString infoString, int infoNumber); void goToBootloader(UAVObject * = NULL, bool = false); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp index e58a1eee6..7c4122656 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp @@ -26,11 +26,21 @@ */ #include "uploaderplugin.h" #include "uploadergadgetfactory.h" -#include -#include -#include -UploaderPlugin::UploaderPlugin() +#include "version_info/version_info.h" +#include "devicedescriptorstruct.h" +#include "uavobjectutilmanager.h" + +#include +#include +#include + +#include +#include +#include +#include + +UploaderPlugin::UploaderPlugin() : errorMsg(0) { // Do nothing } @@ -44,8 +54,15 @@ bool UploaderPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); + mf = new UploaderGadgetFactory(this); addAutoReleasedObject(mf); + + // Listen to autopilot connection events + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + TelemetryManager *telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); + return true; } @@ -56,5 +73,52 @@ void UploaderPlugin::extensionsInitialized() void UploaderPlugin::shutdown() { - // Do nothing + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + TelemetryManager *telMngr = pm->getObject(); + disconnect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); +} + +void UploaderPlugin::versionMatchCheck() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectUtilManager *utilMngr = pm->getObject(); + deviceDescriptorStruct boardDescription = utilMngr->getBoardDescriptionStruct(); + + QString uavoHash = VersionInfo::uavoHashArray(); + uavoHash.chop(2); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); + + QByteArray uavoHashArray; + bool ok; + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); + } + + QByteArray fwVersion = 0;//boardDescription.uavoHash; + if (fwVersion != uavoHashArray) { + QString gcsDescription = VersionInfo::revision(); + QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); + gcsGitHash.remove(QRegExp("^[0]*")); + QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ") + 1, 14); + + QString gcsUavoHashStr; + QString fwUavoHashStr; + foreach(char i, fwVersion) { + fwUavoHashStr.append(QString::number(i, 16).right(2)); + } + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); + } + QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-" + gcsUavoHashStr.left(8) + ")"; + QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; + + QString warning = QString(tr( + "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " + "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); + if (!errorMsg) { + errorMsg = new QErrorMessage(Core::ICore::instance()->mainWindow()); + } + errorMsg->showMessage(warning); + } } diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h index 2fb6a19f9..a30102d39 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h @@ -32,6 +32,7 @@ #include "uploader_global.h" class UploaderGadgetFactory; +class QErrorMessage; class UPLOADER_EXPORT UploaderPlugin : public ExtensionSystem::IPlugin { Q_OBJECT @@ -44,8 +45,13 @@ public: void extensionsInitialized(); bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); + +private slots: + void versionMatchCheck(); + private: UploaderGadgetFactory *mf; + QErrorMessage *errorMsg; }; #endif // UPLOADERPLUGIN_H From 81b7063f042ad6b8a8ecd442adcb80d09c852062 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 2 May 2014 23:42:28 +0200 Subject: [PATCH 47/67] OP-1174 OP-1131 moved firmware version check from uploader gadget to uploader plugin - removed test code --- ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp index 7c4122656..6098b9f17 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp @@ -95,7 +95,7 @@ void UploaderPlugin::versionMatchCheck() uavoHashArray.append(str.toInt(&ok, 16)); } - QByteArray fwVersion = 0;//boardDescription.uavoHash; + QByteArray fwVersion = boardDescription.uavoHash; if (fwVersion != uavoHashArray) { QString gcsDescription = VersionInfo::revision(); QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); From 6fe1c9acd254f55838dd86764378b2891543f191 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 2 May 2014 23:46:30 +0200 Subject: [PATCH 48/67] OP-1174 OP-1285 Fixed uploader gadget button tooltip. Was wrongly indicating that the erase feature is available from BL version 4 and earlier instead of later. --- ground/openpilotgcs/src/plugins/uploader/uploader.ui | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader.ui b/ground/openpilotgcs/src/plugins/uploader/uploader.ui index f0423a68a..c05047cbe 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.ui +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.ui @@ -97,7 +97,7 @@ menu on the right. true - <html><head/><body><p>Reboot the board and clear its settings memory.</p><p> Useful if the board cannot boot properly.</p><p> Blue led starts blinking quick for 20-30 seconds than the board will start normally</p><p><br/></p><p>If telemetry is not running, select the link using the dropdown</p><p>menu on the right.</p><p>PLEASE NOTE: Supported with bootloader versions 4.0 and earlier</p></body></html> + <html><head/><body><p>Reboot the board and clear its settings memory.</p><p> Useful if the board cannot boot properly.</p><p> Blue led starts blinking quick for 20-30 seconds than the board will start normally</p><p><br/></p><p>If telemetry is not running, select the link using the dropdown</p><p>menu on the right.</p><p>PLEASE NOTE: Supported with bootloader versions 4.0 and later</p></body></html> Erase settings From 8c0560418de798872073445b03ad56ca51b55853 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 3 May 2014 14:32:46 +0200 Subject: [PATCH 49/67] OP-1174 moved firmware version check (again) from uploader plugin to telemetry plugin --- .../src/plugins/telemetry/telemetry.pro | 1 + .../telemetry/telemetry_dependencies.pri | 3 +- .../src/plugins/telemetry/telemetryplugin.cpp | 227 ++++++------------ .../src/plugins/telemetry/telemetryplugin.h | 5 + .../uploader/uploader_dependencies.pri | 2 +- .../src/plugins/uploader/uploaderplugin.cpp | 65 +---- .../src/plugins/uploader/uploaderplugin.h | 5 - 7 files changed, 79 insertions(+), 229 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetry.pro b/ground/openpilotgcs/src/plugins/telemetry/telemetry.pro index f8e4d2331..42335df16 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetry.pro +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetry.pro @@ -5,6 +5,7 @@ QT += svg include(../../openpilotgcsplugin.pri) include(../../plugins/coreplugin/coreplugin.pri) +include(../../libs/version_info/version_info.pri) include(telemetry_dependencies.pri) HEADERS += telemetry_global.h \ diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetry_dependencies.pri b/ground/openpilotgcs/src/plugins/telemetry/telemetry_dependencies.pri index 883e47127..3ee3a36e1 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetry_dependencies.pri +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetry_dependencies.pri @@ -1,3 +1,4 @@ -include(../../plugins/uavtalk/uavtalk.pri) include(../../plugins/uavobjects/uavobjects.pri) +include(../../plugins/uavobjectutil/uavobjectutil.pri) +include(../../plugins/uavtalk/uavtalk.pri) diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp index 4a68a2734..8640a61c9 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp @@ -28,22 +28,27 @@ #include "telemetryplugin.h" #include "monitorgadgetfactory.h" -#include "extensionsystem/pluginmanager.h" +#include "version_info/version_info.h" #include "uavobjectmanager.h" #include "uavobject.h" -#include "coreplugin/icore.h" -#include "coreplugin/connectionmanager.h" - -#include -#include -#include +#include "uavobjectutilmanager.h" +#include #include #include #include +#include +#include -TelemetryPlugin::TelemetryPlugin() -{} +#include +#include +#include +#include +#include + +TelemetryPlugin::TelemetryPlugin() : errorMsg(0) +{ +} TelemetryPlugin::~TelemetryPlugin() { @@ -81,166 +86,70 @@ bool TelemetryPlugin::initialize(const QStringList & args, QString *errMsg) // add monitor widget to connection manager Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); -// connect(cm, SIGNAL(deviceConnected(QIODevice *)), w, SLOT(telemetryConnected())); -// connect(cm, SIGNAL(deviceDisconnected()), w, SLOT(telemetryDisconnected())); cm->addWidget(w); + // Listen to autopilot connection events + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + TelemetryManager *telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); + return true; } void TelemetryPlugin::extensionsInitialized() -{ -// Core::ICore::instance()->readSettings(this); - - // ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - -// connect(pm, SIGNAL(objectAdded(QObject *)), this, SLOT(onTelemetryManagerAdded(QObject *))); -// _toRemoveNotifications.clear(); -// connectNotifications(); -} - -// void TelemetryPlugin::saveConfig(QSettings *settings, UAVConfigInfo *configInfo) -// { -// configInfo->setVersion(VERSION); -// -// settings->beginWriteArray("Current"); -// settings->setArrayIndex(0); -// currentNotification.saveState(settings); -// settings->endArray(); -// -// settings->beginGroup("listNotifies"); -// settings->remove(""); -// settings->endGroup(); -// -// settings->beginWriteArray("listNotifies"); -// for (int i = 0; i < _notificationList.size(); i++) { -// settings->setArrayIndex(i); -// _notificationList.at(i)->saveState(settings); -// } -// settings->endArray(); -// settings->setValue(QLatin1String("Enable"), enable); -// } - -// void TelemetryPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* configInfo */) -// { -//// Just for migration to the new format. -//// Q_ASSERT(configInfo->version() == UAVConfigVersion()); -// -// settings->beginReadArray("Current"); -// settings->setArrayIndex(0); -// currentNotification.restoreState(settings); -// settings->endArray(); -// -//// read list of notifications from settings -// int size = settings->beginReadArray("listNotifies"); -// for (int i = 0; i < size; ++i) { -// settings->setArrayIndex(i); -// NotificationItem *notification = new NotificationItem; -// notification->restoreState(settings); -// _notificationList.append(notification); -// } -// settings->endArray(); -// setEnable(settings->value(QLatin1String("Enable"), 0).toBool()); -// } - -// void TelemetryPlugin::onTelemetryManagerAdded(QObject *obj) -// { -// telMngr = qobject_cast(obj); -// if (telMngr) { -// connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); -// } -// } - -void TelemetryPlugin::shutdown() { // Do nothing } -// void TelemetryPlugin::onAutopilotDisconnect() -// { -// connectNotifications(); -// } +void TelemetryPlugin::shutdown() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + TelemetryManager *telMngr = pm->getObject(); + disconnect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); +} -///*! -// clear any telemetry timers from previous flight; -// reset will be perform on start of option page -// */ -// void TelemetryPlugin::resetNotification(void) -// { -//// first, reject empty args and unknown fields. -// foreach(NotificationItem * ntf, _notificationList) { -// ntf->disposeTimer(); -// disconnect(ntf->getTimer(), SIGNAL(timeout()), this, SLOT(on_timerRepeated_Notification())); -// ntf->disposeExpireTimer(); -// disconnect(ntf->getExpireTimer(), SIGNAL(timeout()), this, SLOT(on_timerRepeated_Notification())); -// } -// } +void TelemetryPlugin::versionMatchCheck() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectUtilManager *utilMngr = pm->getObject(); + deviceDescriptorStruct boardDescription = utilMngr->getBoardDescriptionStruct(); -// void TelemetryPlugin::connectNotifications() -// { -// foreach(UAVDataObject * obj, lstNotifiedUAVObjects) { -// if (obj != NULL) { -// disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(on_arrived_Notification(UAVObject *))); -// } -// } -// if (phonon.mo != NULL) { -// delete phonon.mo; -// phonon.mo = NULL; -// } -// -// if (!enable) { -// return; -// } -// -// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); -// UAVObjectManager *objManager = pm->getObject(); -// -// lstNotifiedUAVObjects.clear(); -// _pendingNotifications.clear(); -// _notificationList.append(_toRemoveNotifications); -// _toRemoveNotifications.clear(); -// -//// first, reject empty args and unknown fields. -// foreach(NotificationItem * telemetry, _notificationList) { -// telemetry->_isPlayed = false; -// telemetry->isNowPlaying = false; -// -// if (telemetry->mute()) { -// continue; -// } -//// check is all sounds presented for notification, -//// if not - we must not subscribe to it at all -// if (telemetry->toList().isEmpty()) { -// continue; -// } -// -// UAVDataObject *obj = dynamic_cast(objManager->getObject(telemetry->getDataObject())); -// if (obj != NULL) { -// if (!lstNotifiedUAVObjects.contains(obj)) { -// lstNotifiedUAVObjects.append(obj); -// -// connect(obj, SIGNAL(objectUpdated(UAVObject *)), -// this, SLOT(on_arrived_Notification(UAVObject *)), -// Qt::QueuedConnection); -// } -// } else { -// qTelemetryDebug() << "Error: Object is unknown (" << telemetry->getDataObject() << ")."; -// } -// } -// -// if (_notificationList.isEmpty()) { -// return; -// } -//// set notification message to current event -// phonon.mo = Phonon::createPlayer(Phonon::NotificationCategory); -// phonon.mo->clearQueue(); -// phonon.firstPlay = true; -// QList audioOutputDevices = -// Phonon::BackendCapabilities::availableAudioOutputDevices(); -// foreach(Phonon::AudioOutputDevice dev, audioOutputDevices) { -// qTelemetryDebug() << "Telemetry: Audio Output device: " << dev.name() << " - " << dev.description(); -// } -// connect(phonon.mo, SIGNAL(stateChanged(Phonon::State, Phonon::State)), -// this, SLOT(stateChanged(Phonon::State, Phonon::State))); -// } + QString uavoHash = VersionInfo::uavoHashArray(); + uavoHash.chop(2); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); + + QByteArray uavoHashArray; + bool ok; + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); + } + + QByteArray fwVersion = boardDescription.uavoHash; + if (fwVersion != uavoHashArray) { + QString gcsDescription = VersionInfo::revision(); + QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); + gcsGitHash.remove(QRegExp("^[0]*")); + QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ") + 1, 14); + + QString gcsUavoHashStr; + QString fwUavoHashStr; + foreach(char i, fwVersion) { + fwUavoHashStr.append(QString::number(i, 16).right(2)); + } + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); + } + QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-" + gcsUavoHashStr.left(8) + ")"; + QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; + + QString warning = QString(tr( + "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " + "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); + if (!errorMsg) { + errorMsg = new QErrorMessage(Core::ICore::instance()->mainWindow()); + } + errorMsg->showMessage(warning); + } +} diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h index 300c08ad7..a72cb73ec 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h @@ -29,6 +29,7 @@ #include +class QErrorMessage; class MonitorGadgetFactory; class TelemetryPlugin : public ExtensionSystem::IPlugin { @@ -44,8 +45,12 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void shutdown(); +private slots: + void versionMatchCheck(); + private: MonitorGadgetFactory *mf; + QErrorMessage *errorMsg; }; #endif // TELEMETRYPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader_dependencies.pri b/ground/openpilotgcs/src/plugins/uploader/uploader_dependencies.pri index 8d6238f60..0de081d39 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader_dependencies.pri +++ b/ground/openpilotgcs/src/plugins/uploader/uploader_dependencies.pri @@ -1,6 +1,6 @@ include(../../openpilotgcsplugin.pri) include(../../plugins/coreplugin/coreplugin.pri) include(../../plugins/uavobjects/uavobjects.pri) +include(../../plugins/uavobjectutil/uavobjectutil.pri) include(../../plugins/uavtalk/uavtalk.pri) include(../../plugins/ophid/ophid.pri) -include(../../plugins/uavobjectutil/uavobjectutil.pri) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp index 6098b9f17..615c61561 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp @@ -27,20 +27,11 @@ #include "uploaderplugin.h" #include "uploadergadgetfactory.h" -#include "version_info/version_info.h" -#include "devicedescriptorstruct.h" -#include "uavobjectutilmanager.h" - #include -#include -#include #include -#include -#include -#include -UploaderPlugin::UploaderPlugin() : errorMsg(0) +UploaderPlugin::UploaderPlugin() { // Do nothing } @@ -58,11 +49,6 @@ bool UploaderPlugin::initialize(const QStringList & args, QString *errMsg) mf = new UploaderGadgetFactory(this); addAutoReleasedObject(mf); - // Listen to autopilot connection events - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - TelemetryManager *telMngr = pm->getObject(); - connect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); - return true; } @@ -73,52 +59,5 @@ void UploaderPlugin::extensionsInitialized() void UploaderPlugin::shutdown() { - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - TelemetryManager *telMngr = pm->getObject(); - disconnect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); -} - -void UploaderPlugin::versionMatchCheck() -{ - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager *utilMngr = pm->getObject(); - deviceDescriptorStruct boardDescription = utilMngr->getBoardDescriptionStruct(); - - QString uavoHash = VersionInfo::uavoHashArray(); - uavoHash.chop(2); - uavoHash.remove(0, 2); - uavoHash = uavoHash.trimmed(); - - QByteArray uavoHashArray; - bool ok; - foreach(QString str, uavoHash.split(",")) { - uavoHashArray.append(str.toInt(&ok, 16)); - } - - QByteArray fwVersion = boardDescription.uavoHash; - if (fwVersion != uavoHashArray) { - QString gcsDescription = VersionInfo::revision(); - QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); - gcsGitHash.remove(QRegExp("^[0]*")); - QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ") + 1, 14); - - QString gcsUavoHashStr; - QString fwUavoHashStr; - foreach(char i, fwVersion) { - fwUavoHashStr.append(QString::number(i, 16).right(2)); - } - foreach(char i, uavoHashArray) { - gcsUavoHashStr.append(QString::number(i, 16).right(2)); - } - QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-" + gcsUavoHashStr.left(8) + ")"; - QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; - - QString warning = QString(tr( - "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " - "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); - if (!errorMsg) { - errorMsg = new QErrorMessage(Core::ICore::instance()->mainWindow()); - } - errorMsg->showMessage(warning); - } + // Do nothing } diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h index a30102d39..245482736 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h @@ -32,7 +32,6 @@ #include "uploader_global.h" class UploaderGadgetFactory; -class QErrorMessage; class UPLOADER_EXPORT UploaderPlugin : public ExtensionSystem::IPlugin { Q_OBJECT @@ -46,12 +45,8 @@ public: bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); -private slots: - void versionMatchCheck(); - private: UploaderGadgetFactory *mf; - QErrorMessage *errorMsg; }; #endif // UPLOADERPLUGIN_H From e95b25dca82fc4c714d2b526f11435c83da6f809 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 3 May 2014 15:57:00 +0200 Subject: [PATCH 50/67] OP-1309 fixed missing thrust mode assignment in manualcontrol --- flight/modules/ManualControl/stabilizedhandler.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index a1da0e406..1f60fb202 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -133,6 +133,7 @@ void stabilizedHandler(bool newinit) } stabilization.Thrust = cmd.Thrust; + stabilization.StabilizationMode.Thrust = stab_settings[3]; StabilizationDesiredSet(&stabilization); } From 69b5f41e58012de28431a8aef24ad53b576cafdf Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 3 May 2014 17:25:53 +0200 Subject: [PATCH 51/67] OP-1309 correctly set StabilizationMode.Thrust in all other modules that set StabilizationDesired (Autotune,Pathfollowers) --- flight/modules/Autotune/autotune.c | 5 +++-- .../FixedWingPathFollower/fixedwingpathfollower.c | 14 ++++++++------ flight/modules/VtolPathFollower/vtolpathfollower.c | 8 +++++--- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/flight/modules/Autotune/autotune.c b/flight/modules/Autotune/autotune.c index a4c5af745..60166dede 100644 --- a/flight/modules/Autotune/autotune.c +++ b/flight/modules/Autotune/autotune.c @@ -181,8 +181,9 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; } - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_THRUST] = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; stabDesired.Thrust = manualControl.Thrust; switch (state) { diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index d969f556a..d381f27ff 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -355,9 +355,10 @@ static void updateFixedAttitude(float *attitude) stabDesired.Pitch = attitude[1]; stabDesired.Yaw = attitude[2]; stabDesired.Thrust = attitude[3]; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); } @@ -630,9 +631,10 @@ static uint8_t updateFixedDesiredAttitude() stabDesired.Yaw = 0; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index c4ee9a8f9..32664c815 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -515,9 +515,10 @@ static void updateFixedAttitude(float *attitude) stabDesired.Pitch = attitude[1]; stabDesired.Yaw = attitude[2]; stabDesired.Thrust = attitude[3]; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); } @@ -646,6 +647,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; } + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); } From 5b64459eb3e87d2c9f65597ee4ca90f355b27d5f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 3 May 2014 17:41:01 +0200 Subject: [PATCH 52/67] OP-1174 changed fw version check popup from QErrorMessage to QMessageBox. This allows to use a warning icon, etc... And opens the door to more improvments. --- .../src/plugins/telemetry/telemetryplugin.cpp | 34 +++++++++++++------ .../src/plugins/telemetry/telemetryplugin.h | 4 +-- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp index 8640a61c9..2074be9fc 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp @@ -42,11 +42,12 @@ #include #include -#include #include #include +#include +#include -TelemetryPlugin::TelemetryPlugin() : errorMsg(0) +TelemetryPlugin::TelemetryPlugin() : firmwareWarningMessageBox(0) { } @@ -107,6 +108,10 @@ void TelemetryPlugin::shutdown() ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); TelemetryManager *telMngr = pm->getObject(); disconnect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); + + if (firmwareWarningMessageBox) { + delete firmwareWarningMessageBox; + } } void TelemetryPlugin::versionMatchCheck() @@ -127,7 +132,7 @@ void TelemetryPlugin::versionMatchCheck() } QByteArray fwVersion = boardDescription.uavoHash; - if (fwVersion != uavoHashArray) { + if (true) {//fwVersion != uavoHashArray) { QString gcsDescription = VersionInfo::revision(); QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); gcsGitHash.remove(QRegExp("^[0]*")); @@ -141,15 +146,22 @@ void TelemetryPlugin::versionMatchCheck() foreach(char i, uavoHashArray) { gcsUavoHashStr.append(QString::number(i, 16).right(2)); } - QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-" + gcsUavoHashStr.left(8) + ")"; - QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; + QString versionFormat = "%1 (%2-%3)"; + QString gcsVersion = versionFormat.arg(gcsGitDate, gcsGitHash, gcsUavoHashStr.left(8)); + QString fwVersion = versionFormat.arg(boardDescription.gitDate, boardDescription.gitHash, fwUavoHashStr.left(8)); - QString warning = QString(tr( - "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " - "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); - if (!errorMsg) { - errorMsg = new QErrorMessage(Core::ICore::instance()->mainWindow()); + if (!firmwareWarningMessageBox) { + firmwareWarningMessageBox = new QMessageBox(Core::ICore::instance()->mainWindow()); + firmwareWarningMessageBox->setWindowModality(Qt::NonModal); + firmwareWarningMessageBox->setWindowTitle(Core::ICore::instance()->mainWindow()->windowTitle()); + firmwareWarningMessageBox->setIcon(QMessageBox::Warning); + firmwareWarningMessageBox->setStandardButtons(QMessageBox::Ok); + firmwareWarningMessageBox->setText(tr("GCS and firmware versions of the UAV objects set do not match which can cause configuration problems.")); + // should we want to re-introduce the checkbox + //firmwareWarningMessageBox->setCheckBox(new QCheckBox(tr("&Don't show this message again."))); } - errorMsg->showMessage(warning); + QString detailTxt = tr("GCS version: %1").arg(gcsVersion) + "\n" + tr("Firmware version: %1").arg(fwVersion); + firmwareWarningMessageBox->setDetailedText(detailTxt); + firmwareWarningMessageBox->show(); } } diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h index a72cb73ec..6ed3f56d7 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h @@ -29,7 +29,7 @@ #include -class QErrorMessage; +class QMessageBox; class MonitorGadgetFactory; class TelemetryPlugin : public ExtensionSystem::IPlugin { @@ -50,7 +50,7 @@ private slots: private: MonitorGadgetFactory *mf; - QErrorMessage *errorMsg; + QMessageBox *firmwareWarningMessageBox; }; #endif // TELEMETRYPLUGIN_H From 1bca36ab8df6661e5d25aaca861199b9909aa1a7 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 3 May 2014 17:42:19 +0200 Subject: [PATCH 53/67] OP-1174 auto update wizard : removed now (hopefully) uneeded workarounds --- .../src/plugins/setupwizard/pages/autoupdatepage.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index a1eb2ad8f..5c49235dc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -41,26 +41,25 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu switch (status) { case uploader::WAITING_DISCONNECT: disableButtons(); - getWizard()->show(); ui->statusLabel->setText(tr("Waiting for all OP boards to be disconnected.")); - // TODO get rid of magic number 20s + // TODO get rid of magic number 20s (should use UploaderGadgetWidget::BOARD_EVENT_TIMEOUT) ui->levellinProgressBar->setMaximum(20); ui->levellinProgressBar->setValue(value.toInt()); break; case uploader::WAITING_CONNECT: // Note: - // following two lines of code were probably added to fix an issue when uploader opened a popup requesting + // the following commented out lines were probably added to fix an issue when uploader opened a popup requesting // user to disconnect all boards // Side effect is that the wizard dialog flickers // the uploader was changed to avoid popups alltogether and that fix is not need anymore // same commented fix can be found in FAILURE case and they are kept for future ref. //getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); //getWizard()->setWindowIcon(qApp->windowIcon()); + //getWizard()->show(); // End of Note disableButtons(); - getWizard()->show(); ui->statusLabel->setText(tr("Please connect the board to the USB port (don't use external supply).")); - // TODO get rid of magic number 20s + // TODO get rid of magic number 20s (should use UploaderGadgetWidget::BOARD_EVENT_TIMEOUT) ui->levellinProgressBar->setMaximum(20); ui->levellinProgressBar->setValue(value.toInt()); break; @@ -90,7 +89,6 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu //getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); //getWizard()->setWindowIcon(qApp->windowIcon()); enableButtons(true); - getWizard()->show(); QString msg = value.toString(); if (msg.isEmpty()) { msg = tr("Something went wrong, you will have to manually upgrade the board using the uploader plugin."); From 0509595137e4e52b2f4be8eb8e2ac27ebc1210c8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 3 May 2014 18:38:45 +0200 Subject: [PATCH 54/67] OP-1309 present for Laurent: 3 more Stabilized modes (and a bit of cleanup) --- .../modules/ManualControl/inc/manualcontrol.h | 107 +++++++++++++++--- flight/modules/ManualControl/manualcontrol.c | 2 +- .../modules/ManualControl/stabilizedhandler.c | 9 ++ .../src/plugins/config/configinputwidget.cpp | 12 ++ .../openpilotgcs/src/plugins/config/input.ui | 84 ++++++++++++++ .../vehicleconfigurationhelper.cpp | 22 +++- .../flightmodesettings.xml | 37 +++++- shared/uavobjectdefinition/flightstatus.xml | 2 +- 8 files changed, 252 insertions(+), 23 deletions(-) diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index bce5a6724..d25a2c97b 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -83,27 +83,102 @@ void pathPlannerHandler(bool newinit); ( \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + 1 \ + ) + +#define assumptions2 \ + ( \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + 1 \ ) #define assumptions3 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + 1 \ + ) + +#define assumptions4 \ + ( \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + 1 \ ) #define assumptions5 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + 1 \ + ) + +#define assumptions6 \ + ( \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + 1 \ ) #define assumptions_flightmode \ @@ -112,12 +187,16 @@ void pathPlannerHandler(bool newinit); ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED4) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED5) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED6) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) && \ + 1 \ ) #endif // MANUALCONTROL_H diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 16294a920..20f283e27 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -113,7 +113,7 @@ static void commandUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void); -#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode) +#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions_flightmode) /** * Module starting diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 1f60fb202..5dea64f60 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -78,6 +78,15 @@ void stabilizedHandler(bool newinit) case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll); break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: + stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization4Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: + stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization5Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: + stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization6Settings.Roll); + break; default: // Major error, this should not occur because only enter this block when one of these is true AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 8b732c197..a4530978b 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -146,15 +146,27 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Roll, "Roll", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Roll, "Roll", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Roll, "Roll", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Pitch, "Pitch", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Pitch, "Pitch", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Pitch, "Pitch", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Yaw, "Yaw", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Yaw, "Yaw", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Yaw, "Yaw", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Thrust, "Thrust", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Thrust, "Thrust", 1, true); addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Thrust, "Thrust", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Thrust, "Thrust", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Thrust, "Thrust", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Thrust, "Thrust", 1, true); addWidgetBinding("FlightModeSettings", "Arming", ui->armControl); addWidgetBinding("FlightModeSettings", "ArmedTimeout", ui->armTimeout, 0, 1000); diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 86e5d4c00..dd8400a07 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -813,6 +813,27 @@ margin:1px; + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + @@ -855,6 +876,27 @@ margin:1px; + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + @@ -897,6 +939,27 @@ margin:1px; + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + @@ -939,6 +1002,27 @@ margin:1px; + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 0990089a0..bdb18b3b2 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -322,18 +322,30 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration() data.Stabilization2Settings[0] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE; data.Stabilization2Settings[1] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE; data.Stabilization2Settings[2] = FlightModeSettings::STABILIZATION2SETTINGS_RATE; - data.Stabilization2Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL; + data.Stabilization2Settings[3] = FlightModeSettings::STABILIZATION2SETTINGS_MANUAL; data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; - data.Stabilization3Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL; + data.Stabilization3Settings[3] = FlightModeSettings::STABILIZATION3SETTINGS_MANUAL; + data.Stabilization4Settings[0] = FlightModeSettings::STABILIZATION4SETTINGS_ATTITUDE; + data.Stabilization4Settings[1] = FlightModeSettings::STABILIZATION4SETTINGS_ATTITUDE; + data.Stabilization4Settings[2] = FlightModeSettings::STABILIZATION4SETTINGS_AXISLOCK; + data.Stabilization4Settings[3] = FlightModeSettings::STABILIZATION4SETTINGS_CRUISECONTROL; + data.Stabilization5Settings[0] = FlightModeSettings::STABILIZATION5SETTINGS_ATTITUDE; + data.Stabilization5Settings[1] = FlightModeSettings::STABILIZATION5SETTINGS_ATTITUDE; + data.Stabilization5Settings[2] = FlightModeSettings::STABILIZATION5SETTINGS_RATE; + data.Stabilization5Settings[3] = FlightModeSettings::STABILIZATION5SETTINGS_CRUISECONTROL; + data.Stabilization6Settings[0] = FlightModeSettings::STABILIZATION6SETTINGS_RATE; + data.Stabilization6Settings[1] = FlightModeSettings::STABILIZATION6SETTINGS_RATE; + data.Stabilization6Settings[2] = FlightModeSettings::STABILIZATION6SETTINGS_RATE; + data.Stabilization6Settings[3] = FlightModeSettings::STABILIZATION6SETTINGS_CRUISECONTROL; data2.FlightModeNumber = 3; data.FlightModePosition[0] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1; data.FlightModePosition[1] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2; data.FlightModePosition[2] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED3; - data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL; - data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL; - data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL; + data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED4; + data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED5; + data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED6; modeSettings->setData(data); addModifiedObject(modeSettings, tr("Writing flight mode settings 1/2")); controlSettings->setData(data2); diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 753f4e7e9..af68ab276 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -38,6 +38,39 @@ %0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\ %0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;" /> + + + @@ -45,8 +78,8 @@ units="" type="enum" elements="6" - options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI" - defaultvalue="Stabilized1,Stabilized2,Stabilized3,Manual,Manual,Manual" + options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI" + defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6" limits="\ %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index f24408c5c..494cf474e 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + From 80468fda0b0783de96bcef22672a6a98800625e4 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 4 May 2014 11:19:58 +0200 Subject: [PATCH 55/67] OP-1309 Updated sanitycheck to reflect new modes for flight side config validation --- flight/libraries/sanitycheck.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 55d5e272c..7f9027021 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -106,6 +106,15 @@ int32_t configuration_check() case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3: severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor, coptercontrol) : severity; break; + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(4, multirotor, coptercontrol) : severity; + break; + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(5, multirotor, coptercontrol) : severity; + break; + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(6, multirotor, coptercontrol) : severity; + break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { severity = SYSTEMALARMS_ALARM_ERROR; @@ -205,6 +214,15 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop case 3: FlightModeSettingsStabilization3SettingsArrayGet(modes); break; + case 4: + FlightModeSettingsStabilization4SettingsArrayGet(modes); + break; + case 5: + FlightModeSettingsStabilization5SettingsArrayGet(modes); + break; + case 6: + FlightModeSettingsStabilization6SettingsArrayGet(modes); + break; default: return SYSTEMALARMS_ALARM_ERROR; } From 942b53a66b0ac41be86af008c833770f91513e4d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 4 May 2014 11:38:10 +0200 Subject: [PATCH 56/67] OP-1309 removed unnecessary NUMELEM run time check from sanitychecks and put it into manualcontrol.h as a compile time static assert --- flight/libraries/sanitycheck.c | 6 ------ flight/modules/ManualControl/inc/manualcontrol.h | 10 ++++++++++ flight/modules/ManualControl/manualcontrol.c | 2 +- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 7f9027021..03ab299df 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -195,12 +195,6 @@ int32_t configuration_check() */ static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol) { - // Make sure the modes have identical sizes - if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM || - FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) { - return SYSTEMALARMS_ALARM_ERROR; - } - uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM]; // Get the different axis modes for this switch position diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index d25a2c97b..0d5c00d72 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -181,6 +181,16 @@ void pathPlannerHandler(bool newinit); 1 \ ) +#define assumptions7 \ + ( \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_NUMELEM) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_NUMELEM) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_NUMELEM) && \ + 1 \ + ) + #define assumptions_flightmode \ ( \ ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 20f283e27..8ddea6df0 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -113,7 +113,7 @@ static void commandUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void); -#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions_flightmode) +#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) /** * Module starting From fb41258e575e607b41a75e40d9e90afcb6e0e479 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 4 May 2014 12:03:56 +0200 Subject: [PATCH 57/67] OP-1309 fixed missing assignment in manualcontrol FM switch --- flight/modules/ManualControl/manualcontrol.c | 3 ++ .../openpilotgcs/src/plugins/config/input.ui | 30 +++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 8ddea6df0..97676149f 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -193,6 +193,9 @@ static void manualControlTask(void) case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: handler = &handler_STABILIZED; break; #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index dd8400a07..884ac85a5 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -771,6 +771,36 @@ margin:1px; + + + + Stabilized 4 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + Stabilized 5 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + Stabilized 6 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + From d3bf81743489e0fe0625364739d741d5b5556eb2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 4 May 2014 12:17:34 +0200 Subject: [PATCH 58/67] OP-1309 fix typo in stabilizedhandler (manualcontrol) for stabi flightmode 4-6 --- flight/modules/ManualControl/stabilizedhandler.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 5dea64f60..2ab4222f9 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -79,13 +79,13 @@ void stabilizedHandler(bool newinit) stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: - stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization4Settings.Roll); + stab_settings = cast_struct_to_array(settings.Stabilization4Settings, settings.Stabilization4Settings.Roll); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: - stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization5Settings.Roll); + stab_settings = cast_struct_to_array(settings.Stabilization5Settings, settings.Stabilization5Settings.Roll); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: - stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization6Settings.Roll); + stab_settings = cast_struct_to_array(settings.Stabilization6Settings, settings.Stabilization6Settings.Roll); break; default: // Major error, this should not occur because only enter this block when one of these is true From 8abd540eb9b9ebcbd9e2166252aa30de5d29fc9e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 4 May 2014 12:47:40 +0200 Subject: [PATCH 59/67] OP-1309 fixed label alignment issue in input.ui affecting Stabilization Modes --- .../openpilotgcs/src/plugins/config/input.ui | 204 +++++++++++++++--- 1 file changed, 180 insertions(+), 24 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 884ac85a5..e600bb141 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -27,7 +27,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -107,12 +116,21 @@ 0 0 - 774 - 753 + 488 + 281 - + + 12 + + + 12 + + + 12 + + 12 @@ -143,7 +161,16 @@ - + + 12 + + + 12 + + + 12 + + 12 @@ -242,7 +269,16 @@ - + + 12 + + + 12 + + + 12 + + 12 @@ -421,7 +457,16 @@ Flight Mode Switch Settings - + + 0 + + + 0 + + + 0 + + 0 @@ -501,12 +546,21 @@ 0 0 - 774 - 753 + 772 + 751 - + + 12 + + + 12 + + + 12 + + 12 @@ -741,6 +795,18 @@ margin:1px; QFrame::Raised + + 1 + + + 1 + + + 1 + + + 1 + @@ -813,7 +879,16 @@ margin:1px; QFrame::Raised - + + 1 + + + 1 + + + 1 + + 1 @@ -876,7 +951,16 @@ margin:1px; QFrame::Raised - + + 1 + + + 1 + + + 1 + + 1 @@ -939,7 +1023,16 @@ margin:1px; QFrame::Raised - + + 1 + + + 1 + + + 1 + + 1 @@ -1002,7 +1095,16 @@ margin:1px; QFrame::Raised - + + 1 + + + 1 + + + 1 + + 1 @@ -1093,7 +1195,16 @@ margin:1px; Flight Mode Switch Positions - + + 9 + + + 9 + + + 9 + + 9 @@ -1316,12 +1427,21 @@ margin:1px; QFrame::Raised + + 1 + + + 1 + + + 1 + + + 1 + 10 - - 1 - @@ -1539,7 +1659,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread - + + 1 + + + 1 + + + 1 + + 1 @@ -1641,7 +1770,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread - + + 1 + + + 1 + + + 1 + + 1 @@ -1826,7 +1964,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread Arming Settings - + + 0 + + + 0 + + + 0 + + 0 @@ -1906,12 +2053,21 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread 0 0 - 774 - 753 + 407 + 138 - + + 12 + + + 12 + + + 12 + + 12 From 465f57215c3c6079006d4f303386372891e6e932 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 4 May 2014 18:22:28 +0200 Subject: [PATCH 60/67] OP-1309 restored tab index to 0 in input.ui --- ground/openpilotgcs/src/plugins/config/input.ui | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index e600bb141..0df3cf29b 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -17,7 +17,7 @@ - 1 + 0 @@ -116,8 +116,8 @@ 0 0 - 488 - 281 + 772 + 751 From bcb16f7618929ba8fbfaee00b93165ad8cdd14ec Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 4 May 2014 22:51:24 +0200 Subject: [PATCH 61/67] OP-1174 minor uploader ui tweaks : Status text now has minimum size to avoid jumps of the toolbar buttons. Capitalized Erase Settings button. --- ground/openpilotgcs/src/plugins/uploader/uploader.ui | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader.ui b/ground/openpilotgcs/src/plugins/uploader/uploader.ui index c05047cbe..cc143f976 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.ui +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.ui @@ -100,7 +100,7 @@ menu on the right. <html><head/><body><p>Reboot the board and clear its settings memory.</p><p> Useful if the board cannot boot properly.</p><p> Blue led starts blinking quick for 20-30 seconds than the board will start normally</p><p><br/></p><p>If telemetry is not running, select the link using the dropdown</p><p>menu on the right.</p><p>PLEASE NOTE: Supported with bootloader versions 4.0 and later</p></body></html> - Erase settings + Erase Settings @@ -201,6 +201,12 @@ through serial or USB) + + + 70 + 0 + + 75 From f38de84aa2e18091445199402e5c63bcd5109d8e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 4 May 2014 22:52:09 +0200 Subject: [PATCH 62/67] OP-1174 changed title of firmware mismatch popup --- ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp index 2074be9fc..150a56786 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp @@ -153,7 +153,7 @@ void TelemetryPlugin::versionMatchCheck() if (!firmwareWarningMessageBox) { firmwareWarningMessageBox = new QMessageBox(Core::ICore::instance()->mainWindow()); firmwareWarningMessageBox->setWindowModality(Qt::NonModal); - firmwareWarningMessageBox->setWindowTitle(Core::ICore::instance()->mainWindow()->windowTitle()); + firmwareWarningMessageBox->setWindowTitle(tr("Firmware Version Mismatch!")); firmwareWarningMessageBox->setIcon(QMessageBox::Warning); firmwareWarningMessageBox->setStandardButtons(QMessageBox::Ok); firmwareWarningMessageBox->setText(tr("GCS and firmware versions of the UAV objects set do not match which can cause configuration problems.")); From 1e803c292b76d54af2c54c2ed39e415cfa82223a Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 4 May 2014 22:53:14 +0200 Subject: [PATCH 63/67] OP-1174 beautified various uploader popups --- .../plugins/uploader/uploadergadgetwidget.cpp | 98 ++++++++----------- .../plugins/uploader/uploadergadgetwidget.h | 7 +- 2 files changed, 45 insertions(+), 60 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 6ac8ee539..f5732198c 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -469,13 +469,7 @@ void UploaderGadgetWidget::systemHalt() if (status->getArmed() == FlightStatus::ARMED_DISARMED) { goToBootloader(); } else { - QMessageBox mbox(this); - mbox.setText(tr("The controller board is armed and can not be halted.\n\n" - "Please make sure the board is not armed and then press halt again to proceed\n" - "or use the rescue option to force a firmware upgrade.")); - mbox.setStandardButtons(QMessageBox::Ok); - mbox.setIcon(QMessageBox::Warning); - mbox.exec(); + cannotHaltMessageBox(); } } @@ -500,13 +494,7 @@ void UploaderGadgetWidget::systemReset() log("Board Reset initiated."); goToBootloader(); } else { - QMessageBox mbox(this); - mbox.setText(tr("The controller board is armed and can not be reset.\n\n" - "Please make sure the board is not armed and then press reset again to proceed\n" - "or power cycle to force a board reset.")); - mbox.setStandardButtons(QMessageBox::Ok); - mbox.setIcon(QMessageBox::Warning); - mbox.exec(); + cannotResetMessageBox(); } } @@ -522,20 +510,17 @@ void UploaderGadgetWidget::systemSafeBoot() void UploaderGadgetWidget::systemEraseBoot() { - QMessageBox msgBox; - int result; - - msgBox.setWindowTitle(tr("Erase Settings")); - msgBox.setInformativeText(tr("Do you want to erase all settings from the board?\nSettings cannot be recovered after this operation.\nThe board will be restarted and all the setting erased")); - msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel | QMessageBox::Help); - result = msgBox.exec(); - if (result == QMessageBox::Ok) { + switch(confirmEraseSettingsMessageBox()) { + case QMessageBox::Ok: commonSystemBoot(true, true); - } else if (result == QMessageBox::Help) { + break; + case QMessageBox::Help: QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/display/Doc/Erase+board+settings"), QUrl::StrictMode)); + break; } } + /** * Tells the system to boot (from Bootloader state) * @param[in] safeboot Indicates whether the firmware should use the stock HWSettings @@ -993,41 +978,40 @@ UploaderGadgetWidget::~UploaderGadgetWidget() } } -/** - Shows a message box with an error string. - - @param errorString The error string to display. - - @param errorNumber Not used - - */ -void UploaderGadgetWidget::error(QString errorString, int errorNumber) -{ - Q_UNUSED(errorNumber); - - m_config->boardStatus->setText(errorString); - - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText(errorString); - msgBox.exec(); -} - -/** - Shows a message box with an information string. - - @param infoString The information string to display. - - @param infoNumber Not used - - */ -void UploaderGadgetWidget::info(QString infoString, int infoNumber) -{ - Q_UNUSED(infoNumber); - m_config->boardStatus->setText(infoString); -} - void UploaderGadgetWidget::openHelp() { QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/AoBZ", QUrl::StrictMode)); } + +int UploaderGadgetWidget::confirmEraseSettingsMessageBox() +{ + QMessageBox msgBox(this); + msgBox.setWindowTitle(tr("Confirm Settings Erase?")); + msgBox.setIcon(QMessageBox::Question); + msgBox.setText(tr("Do you want to erase all settings from the board?")); + msgBox.setInformativeText(tr("Settings cannot be recovered after this operation.\nThe board will be restarted and all settings erased.")); + msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel | QMessageBox::Help); + return msgBox.exec(); +} + +int UploaderGadgetWidget::cannotHaltMessageBox() +{ + QMessageBox msgBox(this); + msgBox.setWindowTitle(tr("Cannot Halt Board!")); + msgBox.setIcon(QMessageBox::Warning); + msgBox.setText(tr("The controller board is armed and can not be halted.")); + msgBox.setInformativeText(tr("Please make sure the board is not armed and then press Halt again to proceed or use Rescue to force a firmware upgrade.")); + msgBox.setStandardButtons(QMessageBox::Ok); + return msgBox.exec(); +} + +int UploaderGadgetWidget::cannotResetMessageBox() +{ + QMessageBox msgBox(this); + msgBox.setWindowTitle(tr("Cannot Reset Board!")); + msgBox.setIcon(QMessageBox::Warning); + msgBox.setText(tr("The controller board is armed and can not be reset.")); + msgBox.setInformativeText(tr("Please make sure the board is not armed and then press reset again to proceed or power cycle to force a board reset.")); + msgBox.setStandardButtons(QMessageBox::Ok); + return msgBox.exec(); +} diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 153d94051..5e960d245 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -122,17 +122,18 @@ private: DFUObject *dfu; IAPStep currentStep; bool resetOnly; + void clearLog(); QString getPortDevice(const QString &friendName); - QLineEdit *openFileNameLE; void connectSignalSlot(QWidget *widget); FlightStatus *getFlightStatus(); void bootButtonsSetEnable(bool enabled); + int confirmEraseSettingsMessageBox(); + int cannotHaltMessageBox(); + int cannotResetMessageBox(); private slots: void onPhysicalHWConnect(); - void error(QString errorString, int errorNumber); - void info(QString infoString, int infoNumber); void goToBootloader(UAVObject * = NULL, bool = false); void systemHalt(); void systemReset(); From e3e92efc70821f5c64ea988a95c5578273257fd2 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 5 May 2014 00:05:51 +0200 Subject: [PATCH 64/67] OP-1174 uncrustified --- .../setupwizard/pages/autoupdatepage.cpp | 11 +++-- .../src/plugins/telemetry/telemetryplugin.cpp | 13 ++--- .../plugins/uploader/uploadergadgetwidget.cpp | 48 ++++++++++++------- .../plugins/uploader/uploadergadgetwidget.h | 5 +- 4 files changed, 45 insertions(+), 32 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index 5c49235dc..22af09bd1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -38,6 +38,7 @@ void AutoUpdatePage::enableButtons(bool enable = false) void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant value) { QString msg; + switch (status) { case uploader::WAITING_DISCONNECT: disableButtons(); @@ -53,9 +54,9 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu // Side effect is that the wizard dialog flickers // the uploader was changed to avoid popups alltogether and that fix is not need anymore // same commented fix can be found in FAILURE case and they are kept for future ref. - //getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); - //getWizard()->setWindowIcon(qApp->windowIcon()); - //getWizard()->show(); + // getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); + // getWizard()->setWindowIcon(qApp->windowIcon()); + // getWizard()->show(); // End of Note disableButtons(); ui->statusLabel->setText(tr("Please connect the board to the USB port (don't use external supply).")); @@ -86,8 +87,8 @@ void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant valu ui->statusLabel->setText(tr("Board updated, please press 'Next' to continue.")); break; case uploader::FAILURE: - //getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); - //getWizard()->setWindowIcon(qApp->windowIcon()); + // getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); + // getWizard()->setWindowIcon(qApp->windowIcon()); enableButtons(true); QString msg = value.toString(); if (msg.isEmpty()) { diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp index 150a56786..14d136def 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp @@ -48,8 +48,7 @@ #include TelemetryPlugin::TelemetryPlugin() : firmwareWarningMessageBox(0) -{ -} +{} TelemetryPlugin::~TelemetryPlugin() { @@ -107,6 +106,7 @@ void TelemetryPlugin::shutdown() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); TelemetryManager *telMngr = pm->getObject(); + disconnect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); if (firmwareWarningMessageBox) { @@ -121,6 +121,7 @@ void TelemetryPlugin::versionMatchCheck() deviceDescriptorStruct boardDescription = utilMngr->getBoardDescriptionStruct(); QString uavoHash = VersionInfo::uavoHashArray(); + uavoHash.chop(2); uavoHash.remove(0, 2); uavoHash = uavoHash.trimmed(); @@ -132,7 +133,7 @@ void TelemetryPlugin::versionMatchCheck() } QByteArray fwVersion = boardDescription.uavoHash; - if (true) {//fwVersion != uavoHashArray) { + if (fwVersion != uavoHashArray) { QString gcsDescription = VersionInfo::revision(); QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); gcsGitHash.remove(QRegExp("^[0]*")); @@ -147,8 +148,8 @@ void TelemetryPlugin::versionMatchCheck() gcsUavoHashStr.append(QString::number(i, 16).right(2)); } QString versionFormat = "%1 (%2-%3)"; - QString gcsVersion = versionFormat.arg(gcsGitDate, gcsGitHash, gcsUavoHashStr.left(8)); - QString fwVersion = versionFormat.arg(boardDescription.gitDate, boardDescription.gitHash, fwUavoHashStr.left(8)); + QString gcsVersion = versionFormat.arg(gcsGitDate, gcsGitHash, gcsUavoHashStr.left(8)); + QString fwVersion = versionFormat.arg(boardDescription.gitDate, boardDescription.gitHash, fwUavoHashStr.left(8)); if (!firmwareWarningMessageBox) { firmwareWarningMessageBox = new QMessageBox(Core::ICore::instance()->mainWindow()); @@ -158,7 +159,7 @@ void TelemetryPlugin::versionMatchCheck() firmwareWarningMessageBox->setStandardButtons(QMessageBox::Ok); firmwareWarningMessageBox->setText(tr("GCS and firmware versions of the UAV objects set do not match which can cause configuration problems.")); // should we want to re-introduce the checkbox - //firmwareWarningMessageBox->setCheckBox(new QCheckBox(tr("&Don't show this message again."))); + // firmwareWarningMessageBox->setCheckBox(new QCheckBox(tr("&Don't show this message again."))); } QString detailTxt = tr("GCS version: %1").arg(gcsVersion) + "\n" + tr("Firmware version: %1").arg(fwVersion); firmwareWarningMessageBox->setDetailedText(detailTxt); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index f5732198c..cf14ee25e 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -27,7 +27,7 @@ #include "uploadergadgetwidget.h" #include "flightstatus.h" -//#include "delay.h" +#include "delay.h" #include "devicewidget.h" #include "runningdevicewidget.h" @@ -48,7 +48,7 @@ const int UploaderGadgetWidget::BOARD_EVENT_TIMEOUT = 20000; const int UploaderGadgetWidget::AUTOUPDATE_CLOSE_TIMEOUT = 7000; TimedDialog::TimedDialog(const QString &title, const QString &labelText, int timeout, QWidget *parent, Qt::WindowFlags flags) : - QProgressDialog(labelText, tr("Cancel"), 0, timeout, parent, flags), bar(new QProgressBar(this)) + QProgressDialog(labelText, tr("Cancel"), 0, timeout, parent, flags), bar(new QProgressBar(this)) { setWindowTitle(title); setAutoReset(false); @@ -72,10 +72,10 @@ void TimedDialog::perform() } ConnectionWaiter::ConnectionWaiter(int targetDeviceCount, int timeout, QWidget *parent) : QObject(parent), eventLoop(this), timer(this), timeout(timeout), elapsed(0), targetDeviceCount(targetDeviceCount), result(ConnectionWaiter::Ok) -{ -} +{} -int ConnectionWaiter::exec() { +int ConnectionWaiter::exec() +{ connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(deviceEvent())); connect(USBMonitor::instance(), SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(deviceEvent())); @@ -88,12 +88,14 @@ int ConnectionWaiter::exec() { return result; } -void ConnectionWaiter::cancel() { +void ConnectionWaiter::cancel() +{ quit(); result = ConnectionWaiter::Canceled; } -void ConnectionWaiter::quit() { +void ConnectionWaiter::quit() +{ disconnect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(deviceEvent())); disconnect(USBMonitor::instance(), SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(deviceEvent())); timer.stop(); @@ -118,9 +120,11 @@ void ConnectionWaiter::deviceEvent() } } -int ConnectionWaiter::openDialog(const QString &title, const QString &labelText, int targetDeviceCount, int timeout, QWidget *parent, Qt::WindowFlags flags) { +int ConnectionWaiter::openDialog(const QString &title, const QString &labelText, int targetDeviceCount, int timeout, QWidget *parent, Qt::WindowFlags flags) +{ TimedDialog dlg(title, labelText, timeout / 1000, parent, flags); ConnectionWaiter waiter(targetDeviceCount, timeout, parent); + connect(&dlg, SIGNAL(canceled()), &waiter, SLOT(cancel())); connect(&waiter, SIGNAL(timeChanged(int)), &dlg, SLOT(perform())); return waiter.exec(); @@ -140,24 +144,24 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); + connect(cm, SIGNAL(deviceConnected(QIODevice *)), this, SLOT(onPhysicalHWConnect())); + connect(m_config->haltButton, SIGNAL(clicked()), this, SLOT(systemHalt())); connect(m_config->resetButton, SIGNAL(clicked()), this, SLOT(systemReset())); connect(m_config->bootButton, SIGNAL(clicked()), this, SLOT(systemBoot())); connect(m_config->safeBootButton, SIGNAL(clicked()), this, SLOT(systemSafeBoot())); connect(m_config->eraseBootButton, SIGNAL(clicked()), this, SLOT(systemEraseBoot())); connect(m_config->rescueButton, SIGNAL(clicked()), this, SLOT(systemRescue())); - Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); - connect(cm, SIGNAL(deviceConnected(QIODevice *)), this, SLOT(onPhysicalHWConnect())); + getSerialPorts(); - m_config->autoUpdateButton->setEnabled(autoUpdateCapable()); connect(m_config->autoUpdateButton, SIGNAL(clicked()), this, SLOT(startAutoUpdate())); connect(m_config->autoUpdateOkButton, SIGNAL(clicked()), this, SLOT(closeAutoUpdate())); + m_config->autoUpdateButton->setEnabled(autoUpdateCapable()); m_config->autoUpdateGroupBox->setVisible(false); - QIcon rbi; - rbi.addFile(QString(":uploader/images/view-refresh.svg")); - m_config->refreshPorts->setIcon(rbi); + m_config->refreshPorts->setIcon(QIcon(":uploader/images/view-refresh.svg")); bootButtonsSetEnable(false); @@ -295,6 +299,7 @@ static void sleep(int ms) { QEventLoop eventLoop; QTimer::singleShot(ms, &eventLoop, SLOT(quit())); + eventLoop.exec(); } @@ -466,6 +471,7 @@ void UploaderGadgetWidget::systemHalt() // The board can not be halted when in armed state. // If board is armed, or arming. Show message with notice. FlightStatus *status = getFlightStatus(); + if (status->getArmed() == FlightStatus::ARMED_DISARMED) { goToBootloader(); } else { @@ -510,7 +516,7 @@ void UploaderGadgetWidget::systemSafeBoot() void UploaderGadgetWidget::systemEraseBoot() { - switch(confirmEraseSettingsMessageBox()) { + switch (confirmEraseSettingsMessageBox()) { case QMessageBox::Ok: commonSystemBoot(true, true); break; @@ -759,10 +765,11 @@ void UploaderGadgetWidget::systemRescue() if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { QString labelText = QString("

%1

").arg(tr("Please disconnect your OpenPilot board.")); int result = ConnectionWaiter::openDialog(tr("System Rescue"), labelText, 0, BOARD_EVENT_TIMEOUT, this); - switch(result) { + switch (result) { case ConnectionWaiter::Canceled: m_config->rescueButton->setEnabled(true); return; + case ConnectionWaiter::TimedOut: QMessageBox::warning(this, tr("System Rescue"), tr("Timed out while waiting for all boards to be disconnected!")); m_config->rescueButton->setEnabled(true); @@ -773,10 +780,11 @@ void UploaderGadgetWidget::systemRescue() // Now prompt user to connect board QString labelText = QString("

%1
%2

").arg(tr("Please connect your OpenPilot board.")).arg(tr("Board must be connected to the USB port!")); int result = ConnectionWaiter::openDialog(tr("System Rescue"), labelText, 1, BOARD_EVENT_TIMEOUT, this); - switch(result) { + switch (result) { case ConnectionWaiter::Canceled: m_config->rescueButton->setEnabled(true); return; + case ConnectionWaiter::TimedOut: QMessageBox::warning(this, tr("System Rescue"), tr("Timed out while waiting for a board to be connected!")); m_config->rescueButton->setEnabled(true); @@ -898,6 +906,7 @@ void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVa { QString msg; int remaining; + switch (status) { case uploader::WAITING_DISCONNECT: m_config->autoUpdateLabel->setText(tr("Waiting for all OpenPilot boards to be disconnected from USB.")); @@ -934,7 +943,7 @@ void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVa break; case uploader::SUCCESS: m_config->autoUpdateProgressBar->setValue(m_config->autoUpdateProgressBar->maximum()); - msg = tr("Board was updated successfully."); + msg = tr("Board was updated successfully."); msg += " " + tr("Press OK to finish."); m_config->autoUpdateLabel->setText(QString("%1").arg(msg)); finishAutoUpdate(); @@ -986,6 +995,7 @@ void UploaderGadgetWidget::openHelp() int UploaderGadgetWidget::confirmEraseSettingsMessageBox() { QMessageBox msgBox(this); + msgBox.setWindowTitle(tr("Confirm Settings Erase?")); msgBox.setIcon(QMessageBox::Question); msgBox.setText(tr("Do you want to erase all settings from the board?")); @@ -997,6 +1007,7 @@ int UploaderGadgetWidget::confirmEraseSettingsMessageBox() int UploaderGadgetWidget::cannotHaltMessageBox() { QMessageBox msgBox(this); + msgBox.setWindowTitle(tr("Cannot Halt Board!")); msgBox.setIcon(QMessageBox::Warning); msgBox.setText(tr("The controller board is armed and can not be halted.")); @@ -1008,6 +1019,7 @@ int UploaderGadgetWidget::cannotHaltMessageBox() int UploaderGadgetWidget::cannotResetMessageBox() { QMessageBox msgBox(this); + msgBox.setWindowTitle(tr("Cannot Reset Board!")); msgBox.setIcon(QMessageBox::Warning); msgBox.setText(tr("The controller board is armed and can not be reset.")); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 5e960d245..135513cdd 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -42,7 +42,7 @@ using namespace uploader; class FlightStatus; class UAVObject; -class TimedDialog: public QProgressDialog { +class TimedDialog : public QProgressDialog { Q_OBJECT public: @@ -58,7 +58,7 @@ private: // A helper class to wait for board connection and disconnection events // until a the desired number of connected boards is found // or until a timeout is reached -class ConnectionWaiter: public QObject { +class ConnectionWaiter : public QObject { Q_OBJECT public: @@ -151,7 +151,6 @@ private slots: void finishAutoUpdate(); void closeAutoUpdate(); void autoUpdateStatus(uploader::AutoUpdateStep status, QVariant value); - }; #endif // UPLOADERGADGETWIDGET_H From ead576bab4e673ffe20a2a04089ee40744b77db6 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 6 May 2014 21:52:32 -0400 Subject: [PATCH 65/67] OP-1323 GCS label font tweaks --- .../src/plugins/config/camerastabilization.ui | 108 ++++------------ .../src/plugins/config/ccattitude.ui | 12 +- .../openpilotgcs/src/plugins/config/input.ui | 51 ++++---- .../src/plugins/config/revosensors.ui | 16 +-- .../src/plugins/config/stabilization.ui | 116 +++++++++--------- .../openpilotgcs/src/plugins/config/txpid.ui | 24 ++-- 6 files changed, 132 insertions(+), 195 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui index c2b90e525..168346c81 100644 --- a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui @@ -312,21 +312,15 @@ have to define channel output range using Output configuration tab. 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Yaw @@ -341,21 +335,15 @@ margin:1px; 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Pitch Or Servo2 @@ -370,21 +358,15 @@ margin:1px; 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Roll Or Servo1 @@ -426,21 +408,15 @@ margin:1px; 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Yaw @@ -455,21 +431,15 @@ margin:1px; 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Pitch @@ -484,21 +454,15 @@ margin:1px; 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Roll @@ -914,21 +878,15 @@ value. 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Roll @@ -943,21 +901,15 @@ margin:1px; 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Pitch @@ -972,21 +924,15 @@ margin:1px; 0 - 16 - - - - - 16777215 - 16 + 20 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Yaw diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui index 4a8bc7535..1a659982e 100644 --- a/ground/openpilotgcs/src/plugins/config/ccattitude.ui +++ b/ground/openpilotgcs/src/plugins/config/ccattitude.ui @@ -145,8 +145,8 @@ 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Roll @@ -195,8 +195,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Yaw @@ -225,8 +225,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Pitch diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 0df3cf29b..48fee6df8 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -116,8 +116,8 @@ 0 0 - 772 - 751 + 774 + 748 @@ -546,8 +546,8 @@ 0 0 - 772 - 751 + 774 + 748 @@ -660,8 +660,8 @@ 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Thrust @@ -689,8 +689,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Yaw @@ -718,8 +718,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Pitch @@ -763,8 +763,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Roll @@ -1228,8 +1228,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; PID Bank @@ -1243,7 +1243,7 @@ margin:1px; - 132 + 138 20 @@ -1257,8 +1257,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Flight Mode Count @@ -1270,12 +1270,6 @@ margin:1px; - - - 151 - 16777215 - - Number of positions your FlightMode switch has. @@ -1307,12 +1301,13 @@ channel value for each flight mode. + 10 75 true - Avoid "Manual" for multirotors! Never select "Altitude", "VelocityControl" or "CruiseControl" on a fixed wing! + <html><head/><body><p>Avoid &quot;Manual&quot; for multirotors! Never select &quot;ALtitude&quot;, &quot;VelocityControl&quot; or &quot;CruiseControl&quot; on a fixed wing!</p></body></html> Qt::AlignCenter @@ -1395,8 +1390,8 @@ channel value for each flight mode. 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Flight Mode @@ -2053,8 +2048,8 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread 0 0 - 407 - 138 + 504 + 156 diff --git a/ground/openpilotgcs/src/plugins/config/revosensors.ui b/ground/openpilotgcs/src/plugins/config/revosensors.ui index d6f3de21d..614a439c2 100644 --- a/ground/openpilotgcs/src/plugins/config/revosensors.ui +++ b/ground/openpilotgcs/src/plugins/config/revosensors.ui @@ -464,8 +464,8 @@ p, li { white-space: pre-wrap; } 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Roll @@ -487,8 +487,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Yaw @@ -533,8 +533,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Pitch @@ -572,8 +572,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Accelerometers diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index efccd106b..2cbec6bd4 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -7837,7 +7837,7 @@ border-radius: 5; 0 0 - 784 + 565 733 @@ -16299,8 +16299,8 @@ border-radius: 5; 0 0 - 798 - 705 + 829 + 691 @@ -16734,7 +16734,7 @@ border-radius: 5; 0 - 16 + 20 @@ -17263,7 +17263,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Roll @@ -17284,7 +17286,7 @@ border-radius: 5; 0 - 16 + 20 @@ -17813,7 +17815,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Yaw @@ -18088,7 +18092,7 @@ border-radius: 5; 0 - 16 + 20 @@ -18617,7 +18621,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Pitch @@ -18878,21 +18884,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 162 + 20 @@ -19421,7 +19421,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Weak Leveling Kp @@ -19434,21 +19436,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 179 + 20 @@ -19977,7 +19973,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Weak Leveling Rate @@ -19990,21 +19988,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 132 + 20 @@ -20533,7 +20525,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Max Axis Lock @@ -20546,21 +20540,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 176 + 20 @@ -21089,7 +21077,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Max Axis Lock Rate @@ -21125,7 +21115,7 @@ border-radius: 5; - 5 + 25 22 @@ -22039,7 +22029,7 @@ border-radius: 5; 0 - 16 + 20 @@ -22568,7 +22558,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; GyroTau @@ -22589,7 +22581,7 @@ border-radius: 5; 0 - 16 + 20 @@ -23118,7 +23110,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; AccelKp @@ -23204,7 +23198,7 @@ border-radius: 5; 90 - 16 + 20 @@ -23733,7 +23727,9 @@ border-radius: 5; 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; AccelKi @@ -23916,8 +23912,8 @@ border-radius: 5; 0 0 - 798 - 705 + 665 + 435 @@ -24822,8 +24818,8 @@ border-radius: 5; 0 0 - 798 - 705 + 478 + 518 diff --git a/ground/openpilotgcs/src/plugins/config/txpid.ui b/ground/openpilotgcs/src/plugins/config/txpid.ui index 496ffe032..b7a0887a6 100644 --- a/ground/openpilotgcs/src/plugins/config/txpid.ui +++ b/ground/openpilotgcs/src/plugins/config/txpid.ui @@ -214,8 +214,8 @@ Up to 3 separate PID options (or option pairs) can be selected and updated.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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; PID option @@ -243,8 +243,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Control Source @@ -272,8 +272,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Min @@ -301,8 +301,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Max @@ -620,8 +620,8 @@ only when system is armed without disabling the module. 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Min @@ -649,8 +649,8 @@ margin:1px; 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)); color: rgb(255, 255, 255); border-radius: 5; -font: bold 12px; -margin:1px; +margin:1px; +font:bold; Max From a0e661b77e1e89b005907f97cd2bd9a4ea50e354 Mon Sep 17 00:00:00 2001 From: m_thread Date: Wed, 7 May 2014 14:19:20 +0200 Subject: [PATCH 66/67] Disabled malfunctioning PIOS_QUATERNION_STABILIZATION option. --- flight/targets/boards/revolution/firmware/inc/pios_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 5b46f059a..0fac7c930 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -144,7 +144,7 @@ #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ -#define PIOS_QUATERNION_STABILIZATION +// #define PIOS_QUATERNION_STABILIZATION /* Performance counters */ #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 From 5528d79a055ac1cd159e26c457cb71b9a7759dc6 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 7 May 2014 18:23:56 +0200 Subject: [PATCH 67/67] OP-1309 bugfix update - correct initial loading of StabBank --- flight/modules/Stabilization/stabilization.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index a3a8b8173..15b8c3caf 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -81,6 +81,7 @@ int32_t StabilizationStart() SettingsUpdatedCb(StabilizationSettingsHandle()); StabilizationDesiredUpdatedCb(StabilizationDesiredHandle()); FlightModeSwitchUpdatedCb(ManualControlCommandHandle()); + BankUpdatedCb(StabilizationBankHandle()); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);