diff --git a/flight/modules/AltitudeHold/inc/altitudehold.h b/flight/libraries/math/mathmisc.c similarity index 77% rename from flight/modules/AltitudeHold/inc/altitudehold.h rename to flight/libraries/math/mathmisc.c index 86ef46b2d..25ddc9559 100644 --- a/flight/modules/AltitudeHold/inc/altitudehold.h +++ b/flight/libraries/math/mathmisc.c @@ -1,9 +1,13 @@ /** ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Reuseable math functions + * @{ * - * @file examplemodperiodic.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Example module to be used as a template for actual modules. + * @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 * @@ -23,9 +27,6 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef EXAMPLEMODPERIODIC_H -#define EXAMPLEMODPERIODIC_H -int32_t ExampleModPeriodicInitialize(); -int32_t GuidanceInitialize(void); -#endif // EXAMPLEMODPERIODIC_H + +// 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 new file mode 100644 index 000000000..b4c8b75b8 --- /dev/null +++ b/flight/libraries/math/mathmisc.h @@ -0,0 +1,55 @@ +/** + ****************************************************************************** + * @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) +static inline float boundf(float val, float boundary1, float boundary2) +{ + if (boundary1 > 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; +} + +#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/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 3b075d96b..52134794b 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -46,7 +46,7 @@ ****************************/ // ! Check a stabilization mode switch position for safety -static int32_t check_stabilization_settings(int index, bool multirotor); +static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol); /** * Run a preflight check over the hardware configuration @@ -98,34 +98,28 @@ int32_t configuration_check() } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor, coptercontrol) : severity; break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor, coptercontrol) : severity; break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; + 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_CRITICAL; } break; - case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: - case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_CRITICAL; - } - // TODO: put check equivalent to TASK_MONITOR_IsRunning - // here as soon as available for delayed callbacks - break; - case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_CRITICAL; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_CRITICAL; - } - break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_CRITICAL; @@ -199,14 +193,8 @@ int32_t configuration_check() * @param[in] index Which stabilization mode to check * @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_CRITICAL */ -static int32_t check_stabilization_settings(int index, bool multirotor) +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_CRITICAL; - } - uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM]; // Get the different axis modes for this switch position @@ -220,22 +208,58 @@ static int32_t check_stabilization_settings(int index, bool multirotor) case 3: FlightModeSettingsStabilization3SettingsArrayGet(modes); break; + case 4: + FlightModeSettingsStabilization4SettingsArrayGet(modes); + break; + case 5: + FlightModeSettingsStabilization5SettingsArrayGet(modes); + break; + case 6: + FlightModeSettingsStabilization6SettingsArrayGet(modes); + break; default: return SYSTEMALARMS_ALARM_CRITICAL; } - // For multirotors verify that nothing is set to "none" + // For multirotors verify that roll/pitch/yaw are not set to "none" + // (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 < NELEMENTS(modes); i++) { - if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) { + for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) { + if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) { return SYSTEMALARMS_ALARM_CRITICAL; } } } + // coptercontrol cannot do altitude holding + if (coptercontrol) { + if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD + || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY + ) { + return SYSTEMALARMS_ALARM_CRITICAL; + } + } + + // 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_ALTITUDEHOLD + || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY + ) { + return SYSTEMALARMS_ALARM_CRITICAL; + } + } + 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_CRITICAL; + } + // 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/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/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 e61ad0da4..bea95ab7d 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 @@ -356,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); } @@ -427,15 +427,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 +473,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 +497,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 +529,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 +549,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 +606,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 +616,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 @@ -631,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_NONE; + 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); @@ -642,20 +643,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/ManualControl/altitudehandler.c b/flight/modules/ManualControl/altitudehandler.c deleted file mode 100644 index 0ca5a0d32..000000000 --- a/flight/modules/ManualControl/altitudehandler.c +++ /dev/null @@ -1,133 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup ManualControl - * @brief Interpretes the control input in ManualControlCommand - * @{ - * - * @file altitudehandler.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. - * - * @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 "inc/manualcontrol.h" -#include -#include -#include -#include -#include - -#if defined(REVOLUTION) -// Private constants - -// Private types - -// Private functions - -/** - * @brief Handler to control deprecated flight modes controlled by AltitudeHold module - * @input: ManualControlCommand - * @output: AltitudeHoldDesired - */ -void altitudeHandler(bool newinit) -{ - const float DEADBAND = 0.20f; - const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; - const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - - if (newinit) { - StabilizationBankInitialize(); - AltitudeHoldDesiredInitialize(); - AltitudeHoldSettingsInitialize(); - PositionStateInitialize(); - } - - - // this is the max speed in m/s at the extents of thrust - float thrustRate; - uint8_t thrustExp; - - static uint8_t flightMode; - static bool newaltitude = true; - - ManualControlCommandData cmd; - ManualControlCommandGet(&cmd); - - FlightStatusFlightModeGet(&flightMode); - - AltitudeHoldDesiredData altitudeHoldDesiredData; - AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - - AltitudeHoldSettingsThrustExpGet(&thrustExp); - AltitudeHoldSettingsThrustRateGet(&thrustRate); - - StabilizationBankData stabSettings; - StabilizationBankGet(&stabSettings); - - PositionStateData posState; - PositionStateGet(&posState); - - altitudeHoldDesiredData.Roll = cmd.Roll * stabSettings.RollMax; - altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax; - altitudeHoldDesiredData.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw; - - if (newinit) { - newaltitude = true; - } - - uint8_t cutOff; - AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff); - if (cutOff && cmd.Thrust < 0) { - // Cut thrust if desired - altitudeHoldDesiredData.SetPoint = cmd.Thrust; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST; - newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust > DEADBAND_HIGH) { - // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 - // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd.Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd.Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; - newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust < DEADBAND_LOW) { - altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd.Thrust < 0 ? 0 : cmd.Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd.Thrust) / DEADBAND_LOW) / 255 * thrustRate); - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; - newaltitude = true; - } else if (newaltitude == true) { - altitudeHoldDesiredData.SetPoint = posState.Down; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; - newaltitude = false; - } - - AltitudeHoldDesiredSet(&altitudeHoldDesiredData); -} - -#else /* if defined(REVOLUTION) */ -void altitudeHandler(__attribute__((unused)) bool newinit) -{ - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called -} -#endif // REVOLUTION - - -/** - * @} - * @} - */ diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index b5e2c9bf6..0d5c00d72 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -61,13 +61,6 @@ void manualHandler(bool newinit); */ void stabilizedHandler(bool newinit); -/** - * @brief Handler to control deprecated flight modes controlled by AltitudeHold module - * @input: ManualControlCommand - * @output: AltitudeHoldDesired - */ -void altitudeHandler(bool newinit); - /** * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands @@ -88,29 +81,114 @@ 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_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_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((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_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((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 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 \ @@ -119,15 +197,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_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ + ((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 a721b0a91..1bd29329f 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -85,16 +85,6 @@ static const controlHandler handler_AUTOTUNE = { }; #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES -// TODO: move the altitude handling into stabi -static const controlHandler handler_ALTITUDE = { - .controlChain = { - .Stabilization = true, - .PathFollower = false, - .PathPlanner = false, - }, - .handler = &altitudeHandler, -}; - static const controlHandler handler_PATHFOLLOWER = { .controlChain = { .Stabilization = true, @@ -123,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 && assumptions7 && assumptions_flightmode) /** * Module starting @@ -203,10 +193,12 @@ 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 - case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: case FLIGHTSTATUS_FLIGHTMODE_LAND: @@ -216,10 +208,6 @@ static void manualControlTask(void) case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: handler = &handler_PATHPLANNER; break; - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: - handler = &handler_ALTITUDE; - break; #endif case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: handler = &handler_AUTOTUNE; diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index d7912d936..2ab4222f9 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.Stabilization4Settings, settings.Stabilization4Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: + stab_settings = cast_struct_to_array(settings.Stabilization5Settings, settings.Stabilization5Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: + 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 AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); @@ -86,25 +95,25 @@ 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_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 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_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 @@ -120,19 +129,20 @@ 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_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 } stabilization.Thrust = cmd.Thrust; + stabilization.StabilizationMode.Thrust = stab_settings[3]; StabilizationDesiredSet(&stabilization); } diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/Stabilization/altitudeloop.c similarity index 50% rename from flight/modules/AltitudeHold/altitudehold.c rename to flight/modules/Stabilization/altitudeloop.c index 7bf98bfbf..0ca4e7f01 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/Stabilization/altitudeloop.c @@ -1,8 +1,8 @@ /** ****************************************************************************** * - * @file guidance.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @file altitudeloop.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint * and sets @ref AttitudeDesired. It only does this when the FlightMode field * of @ref ManualControlCommand is Auto. @@ -26,89 +26,122 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -/** - * Input object: ActiveWaypoint - * Input object: PositionState - * Input object: ManualControlCommand - * Output object: AttitudeDesired - * - * This module will periodically update the value of the AttitudeDesired object. - * - * The module executes in its own thread in this example. - * - * Modules have no API, all communication to other modules is done through UAVObjects. - * However modules may use the API exposed by shared libraries. - * See the OpenPilot wiki for more details. - * http://www.openpilot.org/OpenPilot_Application_Architecture - * - */ - #include #include -#include #include +#include #include #include -#include // object that will be updated by the module #include -#include -#include -#include -#include #include #include // Private constants -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL -#define STACK_SIZE_BYTES 1024 -#define DESIRED_UPDATE_RATE_MS 100 // milliseconds +#ifdef REVOLUTION + +#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 CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +#define STACK_SIZE_BYTES 512 // Private types // Private variables static DelayedCallbackInfo *altitudeHoldCBInfo; static AltitudeHoldSettingsData altitudeHoldSettings; static struct pid pid0, pid1; +static ThrustModeType thrustMode; +static PiOSDeltatimeConfig timeval; +static float thrustSetpoint = 0.0f; +static float thrustDemand = 0.0f; +static float startThrust = 0.5f; // Private functions static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); +static void VelocityStateUpdatedCb(UAVObjEvent *ev); /** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed + * Setup mode and setpoint */ -int32_t AltitudeHoldStart() +float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit) { - // Start main task - SettingsUpdatedCb(NULL); - PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo); + static bool newaltitude = true; - return 0; + if (reinit) { + startThrust = setpoint; + pid_zero(&pid0); + pid_zero(&pid1); + newaltitude = true; + } + + const float DEADBAND = 0.20f; + const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; + const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; + + // this is the max speed in m/s at the extents of thrust + float thrustRate; + uint8_t thrustExp; + + AltitudeHoldSettingsThrustExpGet(&thrustExp); + AltitudeHoldSettingsThrustRateGet(&thrustRate); + + PositionStateData posState; + PositionStateGet(&posState); + + if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) { + // Cut thrust if desired + thrustSetpoint = 0.0f; + thrustDemand = 0.0f; + thrustMode = DIRECT; + newaltitude = true; + } else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) { + // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 + // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 + thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); + thrustMode = ALTITUDEVARIO; + newaltitude = true; + } else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) { + thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate); + thrustMode = ALTITUDEVARIO; + newaltitude = true; + } else if (newaltitude == true) { + thrustSetpoint = posState.Down; + thrustMode = ALTITUDEHOLD; + newaltitude = false; + } + + return thrustDemand; } /** * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed */ -int32_t AltitudeHoldInitialize() +void stabilizationAltitudeloopInit() { AltitudeHoldSettingsInitialize(); - AltitudeHoldDesiredInitialize(); AltitudeHoldStatusInitialize(); + PositionStateInitialize(); + VelocityStateInitialize(); + PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); // Create object queue altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); + VelocityStateConnectCallback(&VelocityStateUpdatedCb); - return 0; + // Start main task + SettingsUpdatedCb(NULL); } -MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); /** @@ -116,44 +149,25 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); */ static void altitudeHoldTask(void) { - static float startThrust = 0.5f; - - // make sure we run only when we are supposed to run - FlightStatusData flightStatus; - - FlightStatusGet(&flightStatus); - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: - break; - default: - pid_zero(&pid0); - pid_zero(&pid1); - StabilizationDesiredThrustGet(&startThrust); - PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); - return; - - break; - } - AltitudeHoldStatusData altitudeHoldStatus; + AltitudeHoldStatusGet(&altitudeHoldStatus); // do the actual control loop(s) - AltitudeHoldDesiredData altitudeHoldDesired; - AltitudeHoldDesiredGet(&altitudeHoldDesired); float positionStateDown; PositionStateDownGet(&positionStateDown); float velocityStateDown; VelocityStateDownGet(&velocityStateDown); - switch (altitudeHoldDesired.ControlMode) { - case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE: + float dT; + dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + switch (thrustMode) { + case ALTITUDEHOLD: // altitude control loop - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT); break; - case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY: - altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint; + case ALTITUDEVARIO: + altitudeHoldStatus.VelocityDesired = thrustSetpoint; break; default: altitudeHoldStatus.VelocityDesired = 0; @@ -162,37 +176,16 @@ static void altitudeHoldTask(void) AltitudeHoldStatusSet(&altitudeHoldStatus); - float thrust; - switch (altitudeHoldDesired.ControlMode) { - case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST: - thrust = altitudeHoldDesired.SetPoint; + switch (thrustMode) { + case DIRECT: + thrustDemand = thrustSetpoint; break; default: // velocity control loop - thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT); - if (thrust >= 1.0f) { - thrust = 1.0f; - } - if (thrust <= 0.0f) { - thrust = 0.0f; - } break; } - - StabilizationDesiredData stab; - StabilizationDesiredGet(&stab); - stab.Roll = altitudeHoldDesired.Roll; - stab.Pitch = altitudeHoldDesired.Pitch; - stab.Yaw = altitudeHoldDesired.Yaw; - stab.Thrust = thrust; - stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - - StabilizationDesiredSet(&stab); - - PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) @@ -203,3 +196,11 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit); pid_zero(&pid1); } + +static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo); +} + + +#endif /* ifdef REVOLUTION */ diff --git a/flight/modules/Stabilization/cruisecontrol.c b/flight/modules/Stabilization/cruisecontrol.c new file mode 100644 index 000000000..2a816f60e --- /dev/null +++ b/flight/modules/Stabilization/cruisecontrol.c @@ -0,0 +1,293 @@ +/** + ****************************************************************************** + * @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 +#include + +static float cruisecontrol_factor = 1.0f; + + +static inline float CruiseControlLimitThrust(float thrust) +{ + // limit to user specified absolute max thrust + return boundf(thrust, stabSettings.cruiseControl.min_thrust, stabSettings.cruiseControl.max_thrust); +} + +// assumes 1.0 <= factor <= 100.0 +// a factor of less than 1.0 could make it return a value less than stabSettings.cruiseControl.min_thrust +// CP helis need to have min_thrust=-1 +// +// multicopters need to have min_thrust=0.05 or so +// values below that will not be subject to max / min limiting +// that means thrust can be less than min +// that means multicopter motors stop spinning at low stick +static inline float CruiseControlFactorToThrust(float factor, float thrust) +{ + // don't touch thrust if it's less than min_thrust + // without that test, quadcopter props will spin up + // to min thrust even at zero throttle stick + // if Cruise Control is enabled on this flight switch position + if (thrust > stabSettings.cruiseControl.min_thrust) { + return CruiseControlLimitThrust(thrust * factor); + } + return thrust; +} + +static float CruiseControlAngleToFactor(float angle) +{ + float factor; + + // avoid singularity + if (angle > 89.999f && angle < 90.001f) { + factor = stabSettings.settings.CruiseControlMaxPowerFactor; + } 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) * stabSettings.cruiseControl.power_trim + 1.0f; + // limit to user specified max power multiplier + if (factor > stabSettings.settings.CruiseControlMaxPowerFactor) { + factor = stabSettings.settings.CruiseControlMaxPowerFactor; + } + } + return factor; +} + + +void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand) +{ + static float previous_angle; + static uint32_t previous_time = 0; + static bool previous_time_valid = false; + + // For multiple, speedy flips this mainly strives to address the + // fact that (due to thrust delay) thrust didn't average straight + // down, but at an angle. For less speedy flips it acts like it + // used to. It can be turned off by setting power delay to 0. + + // 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. + + // Note that this code only handles the transition to/from inverted + // thrust. It doesn't handle the case where thrust is changed a + // lot in a small angle range when that range is close to 90 degrees. + // It doesn't handle the small constant "system delay" caused by the + // delay between reading sensors and actuators beginning to respond. + // It also assumes that the pilot is holding the throttle constant; + // when the pilot does change the throttle, the compensation is + // simply recalculated. + + // This implementation of future thrust isn't perfect. That would + // probably require an iterative procedure for solving a + // transcendental equation of the form linear(x) = 1/cos(x). It's + // shortcomings generally don't hurt anything and work better than + // without it. It is designed to work perfectly if the pilot is + // using full thrust during flips and it is only activated if 70% or + // greater thrust is used. + + uint32_t 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 || previous_time_valid == false) { + float angle, angle_unmodified; + + // spherical right triangle + // 0.0 <= angle <= 180.0 + angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitude->Roll) + * cos_lookup_deg(attitude->Pitch))); + + // 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; + + // rate can be negative. + rate = (angle - previous_angle) / ((float)(time - 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. + + // There is only one transition and the high power level for + // it is either: + // 1/fabsf(cos(angle)) * current thrust + // or max power factor * current thrust + // or full thrust + // You can cross the transition with angle either increasing + // or decreasing (rate positive or negative). + + // Thrust is never boosted for negative values of + // thrustDemand (negative stick values) + // + // When the aircraft is upright, thrust is always boosted + // . for positive values of thrustDemand + // When the aircraft is inverted, thrust is sometimes + // . boosted or reversed (or combinations thereof) or zeroed + // . for positive values of thrustDemand + // It depends on the inverted power settings. + // Of course, you can set MaxPowerFactor to 1.0 to + // . effectively disable boost. + if (thrustDemand > 0.0f) { + // to enable the future thrust calculations, make sure + // there is a large enough transition that the result + // will be roughly on vs. off; without that, it can + // 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 (thrustDemand > 0.7f) { + float thrust; + + thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand); + + // determine if we are in range of the transition + + // given the thrust at max_angle and thrustDemand + // (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. + // the 'thrust' variable is non-negative here + switch (stabSettings.settings.CruiseControlInvertedPowerOutput) { + case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO: + // normal multi-copter case, stroke is max to zero + // technically max to constant min_thrust + // can be used by CP + thrust = (thrust - CruiseControlLimitThrust(0.0f)) / stabSettings.cruiseControl.thrust_difference; + break; + case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL: + // reversed but not boosted + // : CP heli case, stroke is max to -stick + // : thrust = (thrust - CruiseControlLimitThrust(-thrustDemand)) / stabSettings.cruiseControl.thrust_difference; + // else it is both unreversed and unboosted + // : simply turn off boost, stroke is max to +stick + // : thrust = (thrust - CruiseControlLimitThrust(thrustDemand)) / stabSettings.cruiseControl.thrust_difference; + thrust = (thrust - CruiseControlLimitThrust( + (stabSettings.settings.CruiseControlInvertedThrustReversing + == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) + ? -thrustDemand + : thrustDemand)) / stabSettings.cruiseControl.thrust_difference; + break; + case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED: + // if boosted and reversed + if (stabSettings.settings.CruiseControlInvertedThrustReversing + == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) { + // CP heli case, stroke is max to min + thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand)) / stabSettings.cruiseControl.thrust_difference; + } + // else it is boosted and unreversed so the throttle doesn't change + else { + // CP heli case, no transition, so stroke is zero + thrust = 0.0f; + } + break; + } + + // 'thrust' is now the proportion of max stroke + // 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 *= stabSettings.cruiseControl.half_power_delay; + // 'thrust' is now the length of time for this stroke + // multiply that times angular rate to get the lead angle + thrust *= fabsf(rate); + // if the transition is within range we use it, + // else we just use the current calculated thrust + if ((float)stabSettings.settings.CruiseControlMaxAngle - thrust <= angle + && angle <= (float)stabSettings.settings.CruiseControlMaxAngle + thrust) { + // default to a little above max angle + angle = (float)stabSettings.settings.CruiseControlMaxAngle + 0.01f; + // if roll direction is downward + // then thrust value is taken from below max angle + // by the code that knows about the transition angle + if (rate < 0.0f) { + angle -= 0.02f; + } + } + } // if thrust > 0.7; else just use the angle we already calculated + cruisecontrol_factor = CruiseControlAngleToFactor(angle); + } else { // if thrust > 0 set factor from angle; else + cruisecontrol_factor = 1.0f; + } + + if (angle >= (float)stabSettings.settings.CruiseControlMaxAngle) { + switch (stabSettings.settings.CruiseControlInvertedPowerOutput) { + case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO: + cruisecontrol_factor = 0.0f; + break; + case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL: + cruisecontrol_factor = 1.0f; + break; + case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED: + // no change, leave factor >= 1.0 alone + break; + } + if (stabSettings.settings.CruiseControlInvertedThrustReversing + == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) { + cruisecontrol_factor = -cruisecontrol_factor; + } + } + } // 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_unmodified; + } // every 8th time +} + +float cruisecontrol_apply_factor(float raw) +{ + if (stabSettings.settings.CruiseControlMaxPowerFactor > 0.0001f) { + raw = CruiseControlFactorToThrust(cruisecontrol_factor, raw); + } + return raw; +} diff --git a/flight/modules/Stabilization/inc/altitudeloop.h b/flight/modules/Stabilization/inc/altitudeloop.h new file mode 100644 index 000000000..d4789c6eb --- /dev/null +++ b/flight/modules/Stabilization/inc/altitudeloop.h @@ -0,0 +1,41 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief altitudeloop mode + * @note This file implements the logic for a altitudeloop + * @{ + * + * @file altitudeloop.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 ALTITUDELOOP_H +#define ALTITUDELOOP_H + +typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType; + +void stabilizationAltitudeloopInit(); +float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit); + +#endif /* ALTITUDELOOP_H */ diff --git a/flight/modules/Stabilization/inc/cruisecontrol.h b/flight/modules/Stabilization/inc/cruisecontrol.h new file mode 100644 index 000000000..3caa3dba1 --- /dev/null +++ b/flight/modules/Stabilization/inc/cruisecontrol.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @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 + +#include +#include + +void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand); +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/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 396a91e89..8314ae97e 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -33,10 +33,52 @@ #ifndef STABILIZATION_H #define STABILIZATION_H -enum { ROLL, PITCH, YAW, MAX_AXES }; +#include +#include +#include +#include + int32_t StabilizationInitialize(); +typedef struct { + StabilizationSettingsData settings; + StabilizationBankData stabBank; + float gyro_alpha; + struct { + float min_thrust; + float max_thrust; + float thrust_difference; + float power_trim; + float half_power_delay; + float max_power_factor_angle; + } cruiseControl; + struct { + int8_t gyroupdates; + int8_t rateupdates; + } monitor; + float rattitude_mode_transition_stick_position; + struct pid innerPids[3], outerPids[3]; +} StabilizationData; + + +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 new file mode 100644 index 000000000..b04556cb6 --- /dev/null +++ b/flight/modules/Stabilization/innerloop.c @@ -0,0 +1,289 @@ +/** + ****************************************************************************** + * @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 innerloop.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 +#include + +// Private constants + +#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 + +// Private variables +static DelayedCallbackInfo *callbackHandle; +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; + +// Private functions +static void stabilizationInnerloopTask(); +static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); +#ifdef REVOLUTION +static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); +#endif + +void stabilizationInnerloopInit() +{ + RateDesiredInitialize(); + ActuatorDesiredInitialize(); + GyroStateInitialize(); + StabilizationStatusInitialize(); + FlightStatusInitialize(); + ManualControlCommandInitialize(); +#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); + + // schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared + PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER); +} + + +/** + * 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() +{ + // watchdog and error handling + { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); +#endif + 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 < -(2 * OUTERLOOP_SKIPCOUNT)) { + // warning if rate loop skipped more than 2 execution + warn = true; + } + if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) { + // error if rate loop skipped more than 4 executions + error = true; + } + // check if gyro keeps updating + if (stabSettings.monitor.gyroupdates < 1) { + // critical if gyro didn't update at all! + crit = true; + } + if (stabSettings.monitor.gyroupdates > 1) { + // warning if we missed a gyro update + warn = true; + } + if (stabSettings.monitor.gyroupdates > 3) { + // error if we missed 3 gyro updates + error = true; + } + stabSettings.monitor.gyroupdates = 0; + + if (crit) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL); + } else if (error) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR); + } else if (warn) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + } + } + + + 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) { + stabSettings.innerPids[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: + rate[t] = boundf(rate[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; + 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(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(&stabSettings.innerPids[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); + } + + actuator.UpdateTime = dT * 1000; + + 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; + } + } + { + 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; + } + } + } + PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER); +} + + +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); + stabSettings.monitor.gyroupdates++; +} + +#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/outerloop.c b/flight/modules/Stabilization/outerloop.c new file mode 100644 index 000000000..05ff97189 --- /dev/null +++ b/flight/modules/Stabilization/outerloop.c @@ -0,0 +1,288 @@ +/** + ****************************************************************************** + * @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 +#include +#include + +// Private constants + +#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 + +// Private variables +static DelayedCallbackInfo *callbackHandle; +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); + +void stabilizationOuterloopInit() +{ + RateDesiredInitialize(); + StabilizationDesiredInitialize(); + AttitudeStateInitialize(); + StabilizationStatusInitialize(); + 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); +} + + +/** + * 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() +{ + AttitudeStateData attitudeState; + RateDesiredData rateDesired; + StabilizationDesiredData stabilizationDesired; + StabilizationStatusOuterLoopData enabled; + + AttitudeStateGet(&attitudeState); + StabilizationDesiredGet(&stabilizationDesired); + RateDesiredGet(&rateDesired); + StabilizationStatusOuterLoopGet(&enabled); + float *stabilizationDesiredAxis = &stabilizationDesired.Roll; + float *rateDesiredAxis = &rateDesired.Roll; + int t; + 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]); + previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t]; + + if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) { + if (reinit) { + stabSettings.outerPids[t].iAccumulator = 0; + } + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { + case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: + rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); + break; + case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE: + { + float stickinput[3]; + 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]; + // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together + rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT), + -cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t], + cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] + ); + // 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 = fabsf(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 STABILIZATIONSTATUS_OUTERLOOP_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(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); + + // 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; + } + } else { + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { +#ifdef REVOLUTION + case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: + rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit); + break; + case STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY: + rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit); + break; +#endif /* REVOLUTION */ + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: + default: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; + break; + } + } + } + + 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, rateDesired.Thrust); + stabSettings.monitor.rateupdates = 0; +} + + +static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + // to reduce CPU utilisation, outer loop is not executed every state update + static uint8_t cpusafer = 0; + + 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/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 6be3280ee..15b8c3caf 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -33,121 +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 +#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 - -#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 float bound(float val, float range); -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()); + BankUpdatedCb(StabilizationBankHandle()); - - // 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); #endif @@ -160,650 +95,189 @@ 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(); +#ifdef REVOLUTION + stabilizationAltitudeloopInit(); +#endif + 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] = - bound(stabDesiredAxis[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); - - 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] = bound(rateDesiredAxis[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); - - 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 = bound(stabDesiredAxis[i], 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 = bound(rateDesiredAxisAttitude, - 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] = bound(actuatorDesiredAxis[i], 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 = bound(weak_leveling, 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); - - 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] = bound(axis_lock_accum[i], 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]); - - 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_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]); - - // 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); - - 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] = bound(rateDesiredAxis[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); - - break; - - case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 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_CRITICAL); - } 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); } - -/** - * 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) { 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 @@ -811,37 +285,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; + stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f; + stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f; + stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust; - memcpy( - cruise_control_flight_mode_switch_pos_enable, - settings.CruiseControlFlightModeSwitchPosEnable, - sizeof(cruise_control_flight_mode_switch_pos_enable)); + stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f; + stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f; + stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor)); } /** diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index 211b7b864..cc1a284f1 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -37,12 +37,9 @@ #include "stabilizationsettings.h" // ! Private variables -static float vbar_integral[MAX_AXES]; +static float vbar_integral[3]; 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) { @@ -64,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; @@ -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/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5b71b252d..211512988 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -155,6 +155,10 @@ static stateFilter cfmFilter; static stateFilter ekf13iFilter; static stateFilter ekf13Filter; +// this is a hack to provide a computational shortcut for faster gyro state progression +static float gyroRaw[3]; +static float gyroDelta[3]; + // preconfigured filter chains selectable via revoSettings.FusionAlgorithm static const filterPipeline *cfQueue = &(filterPipeline) { .filter = &magFilter, @@ -416,6 +420,11 @@ static void StateEstimationCb(void) // fetch sensors, check values, and load into state struct FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z); + if (IS_SET(states.updated, SENSORUPDATES_gyro)) { + gyroRaw[0] = states.gyro[0]; + gyroRaw[1] = states.gyro[1]; + gyroRaw[2] = states.gyro[2]; + } FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); @@ -454,7 +463,12 @@ static void StateEstimationCb(void) case RUNSTATE_SAVE: // the final output of filters is saved in state variables - EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z); + // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut + if (IS_SET(states.updated, SENSORUPDATES_gyro)) { + gyroDelta[0] = states.gyro[0] - gyroRaw[0]; + gyroDelta[1] = states.gyro[1] - gyroRaw[1]; + gyroDelta[2] = states.gyro[2] - gyroRaw[2]; + } EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z); EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z); EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down); @@ -541,6 +555,14 @@ static void sensorUpdatedCb(UAVObjEvent *ev) if (ev->obj == GyroSensorHandle()) { updatedSensors |= SENSORUPDATES_gyro; + // shortcut - update GyroState right away + GyroSensorData s; + GyroStateData t; + GyroSensorGet(&s); + t.x = s.x + gyroDelta[0]; + t.y = s.y + gyroDelta[1]; + t.z = s.z + gyroDelta[2]; + GyroStateSet(&t); } if (ev->obj == AccelSensorHandle()) { diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index ad58066e3..b4aa1a8d7 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); } @@ -516,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); } @@ -595,18 +595,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 +615,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. @@ -647,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); } @@ -692,19 +693,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/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/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/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index d589d20ed..5c954526a 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/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/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 5b89372a1..bf9254e7c 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -33,7 +33,7 @@ MODULES += Sensors MODULES += StateEstimation # use instead of Attitude MODULES += Altitude/revolution MODULES += Airspeed -MODULES += AltitudeHold +#MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization MODULES += VtolPathFollower MODULES += ManualControl diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 72f51cbec..44c43d080 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 @@ -105,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus UAVOBJSRCFILENAMES += altitudefiltersettings -UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive 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/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index c60e9a453..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 diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 2e5df1ac3..2b2a4ad99 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -33,7 +33,7 @@ MODULES += Sensors MODULES += StateEstimation # use instead of Attitude MODULES += Altitude/revolution MODULES += Airspeed -MODULES += AltitudeHold +#MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization MODULES += VtolPathFollower MODULES += ManualControl diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 72f51cbec..44c43d080 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 @@ -105,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus UAVOBJSRCFILENAMES += altitudefiltersettings -UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive 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 beda566a8..bc05d3b69 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -42,7 +42,7 @@ MODULES += FirmwareIAP MODULES += StateEstimation #MODULES += Sensors/simulated/Sensors MODULES += Airspeed -MODULES += AltitudeHold +#MODULES += AltitudeHold # now integrated in Stabilization #MODULES += OveroSync # Paths @@ -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/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 7044cada6..724adc0a6 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 @@ -107,7 +108,6 @@ UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += revosettings -UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance 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/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 914709810..40039bb47 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -15,7 +15,7 @@ id="svg3604" version="1.1" inkscape:version="0.48.4 r9939" - sodipodi:docname="system-health-mod.svg" + sodipodi:docname="system-health.svg" inkscape:export-filename="C:\NoBackup\OpenPilot\mainboard-health.png" inkscape:export-xdpi="269.53" inkscape:export-ydpi="269.53" @@ -27,19 +27,20 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="10.554213" - inkscape:cx="47.1292" - inkscape:cy="39.787519" - inkscape:current-layer="svg3604" + inkscape:zoom="7.4629556" + inkscape:cx="32.393349" + inkscape:cy="37.450437" + inkscape:current-layer="foreground" id="namedview3608" - showgrid="false" - inkscape:window-width="1920" - inkscape:window-height="1025" - inkscape:window-x="-2" - inkscape:window-y="-3" + showgrid="true" + inkscape:window-width="1366" + inkscape:window-height="712" + inkscape:window-x="-4" + inkscape:window-y="-4" inkscape:window-maximized="1" showguides="true" - inkscape:guide-bbox="true"> + inkscape:guide-bbox="true" + inkscape:snap-global="false"> + + @@ -521,17 +529,6 @@ inkscape:vp_z="1 : 0.5 : 1" inkscape:persp3d-origin="0.5 : 0.33333333 : 1" id="perspective3185" /> - - - - + + + + + + + + @@ -643,2845 +693,2249 @@ 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="547.97754" + y="411.38937" + ry="1" /> + + width="19.129522" + height="10.639811" + x="504.99179" + y="387.5629" + ry="0.24124773" + inkscape:label="#rect4550" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-7" + width="37.799622" + height="13.255064" + x="553.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="527.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="500.06686" + y="360.27713" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-7-4" + width="24.646557" + height="13.221018" + x="527.58087" + y="395.29047" + ry="0" /> - - - - - - + width="10.470912" + height="8.5405388" + x="532.78839" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8" /> - + width="10.470912" + height="8.5405388" + x="544.5553" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-7" /> + + width="10.470912" + height="8.5405388" + x="568.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="579.8559" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-2" /> + SYSTEM HEALTH + Sensors + Auto + Misc. + Link + Power + Syst. - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + inkscape:label="#g3426" + transform="translate(14,0)"> + id="path6233-9-9-86-0" + 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" /> + id="path6233-9-9-4-5-6" + 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" /> - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + 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" /> + transform="scale(0.83127393,1.2029729)">PATH Boot Fault + id="tspan4291" + x="93.860985" + y="8.3444462">PLAN Battery + id="tspan3458" + x="10.759087" + y="8.3200321">ATTITUDE Sensors + id="tspan4287" + x="41.377663" + y="8.3434696">STAB Airspeed + id="tspan5866" + x="16.329399" + y="20.115072">GPS + SENSORS + AIRSPEED + BARO + CPU + EVENT + MEM. + STACK + I2C + CONF. + BOOT + TELEM. + BATT. + TIME + + + + + + + + + + + + + + + + + + + + + + + + INPUT + + + OUTPUT 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/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index d3348787c..a4530978b 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -146,12 +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); @@ -1324,32 +1339,26 @@ void ConfigInputWidget::updatePositionSlider() default: case 6: ui->fmsModePos6->setEnabled(true); - ui->cc_box_5->setEnabled(true); ui->pidBankSs1_5->setEnabled(true); // pass through case 5: ui->fmsModePos5->setEnabled(true); - ui->cc_box_4->setEnabled(true); ui->pidBankSs1_4->setEnabled(true); // pass through case 4: ui->fmsModePos4->setEnabled(true); - ui->cc_box_3->setEnabled(true); ui->pidBankSs1_3->setEnabled(true); // pass through case 3: ui->fmsModePos3->setEnabled(true); - ui->cc_box_2->setEnabled(true); ui->pidBankSs1_2->setEnabled(true); // pass through case 2: ui->fmsModePos2->setEnabled(true); - ui->cc_box_1->setEnabled(true); ui->pidBankSs1_1->setEnabled(true); // pass through case 1: ui->fmsModePos1->setEnabled(true); - ui->cc_box_0->setEnabled(true); ui->pidBankSs1_0->setEnabled(true); // pass through case 0: @@ -1359,32 +1368,26 @@ void ConfigInputWidget::updatePositionSlider() switch (manualSettingsDataPriv.FlightModeNumber) { case 0: ui->fmsModePos1->setEnabled(false); - ui->cc_box_0->setEnabled(false); ui->pidBankSs1_0->setEnabled(false); // pass through case 1: ui->fmsModePos2->setEnabled(false); - ui->cc_box_1->setEnabled(false); ui->pidBankSs1_1->setEnabled(false); // pass through case 2: ui->fmsModePos3->setEnabled(false); - ui->cc_box_2->setEnabled(false); ui->pidBankSs1_2->setEnabled(false); // pass through case 3: ui->fmsModePos4->setEnabled(false); - ui->cc_box_3->setEnabled(false); ui->pidBankSs1_3->setEnabled(false); // pass through case 4: ui->fmsModePos5->setEnabled(false); - ui->cc_box_4->setEnabled(false); ui->pidBankSs1_4->setEnabled(false); // pass through case 5: ui->fmsModePos6->setEnabled(false); - ui->cc_box_5->setEnabled(false); ui->pidBankSs1_5->setEnabled(false); // pass through case 6: diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index b6a1f765e..48fee6df8 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -546,8 +546,8 @@ 0 0 - 768 - 742 + 774 + 748 @@ -587,7 +587,7 @@ Stabilization Modes Configuration - + 9 @@ -626,6 +626,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; +margin:1px; +font:bold; + + + Thrust + + + Qt::AlignCenter + + + @@ -644,8 +689,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; Yaw @@ -673,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 @@ -684,7 +729,7 @@ margin:1px; - + Qt::Horizontal @@ -718,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 @@ -750,6 +795,18 @@ margin:1px; QFrame::Raised + + 1 + + + 1 + + + 1 + + + 1 + @@ -780,6 +837,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 + + + @@ -831,6 +918,27 @@ margin:1px; + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + @@ -882,6 +990,27 @@ margin:1px; + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + @@ -933,6 +1062,99 @@ margin:1px; + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + 0 + 0 + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + @@ -989,7 +1211,7 @@ margin:1px; 6 - + 80 @@ -1006,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 @@ -1021,7 +1243,7 @@ margin:1px; - 132 + 138 20 @@ -1035,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 @@ -1048,12 +1270,6 @@ margin:1px; - - - 151 - 16777215 - - Number of positions your FlightMode switch has. @@ -1078,19 +1294,20 @@ channel value for each flight mode. - + 0 0 + 10 75 true - Avoid "Manual" for multirotors! + <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 @@ -1116,22 +1333,6 @@ channel value for each flight mode. - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 10 - 20 - - - - @@ -1164,35 +1365,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 - - - @@ -1218,8 +1390,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 @@ -1229,294 +1401,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 - - - - - - - @@ -1867,7 +1751,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread - + 75 @@ -2164,8 +2048,8 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread 0 0 - 768 - 742 + 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 468b4b527..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 @@ -8444,7 +8444,7 @@ border-radius: 5; - Max rate attitude (deg/s) + Max rate limit (all modes) (deg/s) Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -13377,8 +13377,6 @@ border-radius: 5; 5 - - 0.000100000000000 @@ -15943,7 +15941,6 @@ border-radius: 5; element:Kp haslimits:no scale:1 - buttongroup:5,20 @@ -16302,8 +16299,8 @@ border-radius: 5; 0 0 - 798 - 705 + 829 + 691 @@ -16737,7 +16734,7 @@ border-radius: 5; 0 - 16 + 20 @@ -17266,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 @@ -17287,7 +17286,7 @@ border-radius: 5; 0 - 16 + 20 @@ -17355,7 +17354,6 @@ border-radius: 5; 26 26 26 - @@ -17817,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 @@ -18092,7 +18092,7 @@ border-radius: 5; 0 - 16 + 20 @@ -18621,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 @@ -18882,21 +18884,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 162 + 20 @@ -19421,12 +19417,13 @@ 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)); color: rgb(255, 255, 255); -border-radius: 5; +border-radius: 5; +margin:1px; +font:bold; Weak Leveling Kp @@ -19439,21 +19436,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 179 + 20 @@ -19946,7 +19937,6 @@ border-radius: 5; 39 39 39 - @@ -19983,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 @@ -19996,21 +19988,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 132 + 20 @@ -20539,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 @@ -20552,21 +20540,15 @@ border-radius: 5; - + 0 0 - 144 - 16 - - - - - 175 - 16777215 + 176 + 20 @@ -21095,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 @@ -21131,7 +21115,7 @@ border-radius: 5; - 5 + 25 22 @@ -21981,7 +21965,6 @@ border-radius: 5; - @@ -22046,7 +22029,7 @@ border-radius: 5; 0 - 16 + 20 @@ -22575,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 @@ -22596,7 +22581,7 @@ border-radius: 5; 0 - 16 + 20 @@ -23125,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 @@ -23211,7 +23198,7 @@ border-radius: 5; 90 - 16 + 20 @@ -23740,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 @@ -23923,8 +23912,8 @@ border-radius: 5; 0 0 - 798 - 705 + 665 + 435 @@ -24186,6 +24175,12 @@ border-radius: 5; + + + 75 + true + + 9 @@ -24200,30 +24195,23 @@ border-radius: 5; 9 - + + + + 75 + 0 + + + + Qt::StrongFocus + - <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. -</p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - 0 - - - -1.000000000000000 - - - 1.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 +24219,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 +24268,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 +24280,7 @@ border-radius: 5; 0 - 180.000000000000000 + 255.000000000000000 105.000000000000000 @@ -24278,7 +24299,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 +24311,13 @@ border-radius: 5; 2 - 0.000000000000000 + 1.000000000000000 50.000000000000000 - 0.250000000000000 + 0.100000000000000 3.000000000000000 @@ -24310,11 +24331,10 @@ border-radius: 5; buttongroup:16 - - + 0 @@ -24323,7 +24343,7 @@ border-radius: 5; - 140 + 155 16 @@ -24339,75 +24359,13 @@ color: rgb(255, 255, 255); border-radius: 5; - PowerTrim + PowerDelayComp Qt::AlignCenter - - - - <html><head/><body><p>This needs to be 0 for all copters except CP helis that are using idle up.</p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - 0 - - - - objname:StabilizationSettings - fieldname:CruiseControlNeutralThrust - 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 - - - - @@ -24451,7 +24409,7 @@ border-radius: 5; - 140 + 90 16 @@ -24484,7 +24442,7 @@ border-radius: 5; - 140 + 92 16 @@ -24510,7 +24468,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 @@ -24521,6 +24479,12 @@ border-radius: 5; 0 + + -100.000000000000000 + + + 49.000000000000000 + 5.000000000000000 @@ -24535,43 +24499,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; - - - NeutralThrust - - - 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 @@ -24582,6 +24513,9 @@ border-radius: 5; 0 + + 51.000000000000000 + 90.000000000000000 @@ -24606,7 +24540,7 @@ border-radius: 5; - 140 + 97 16 @@ -24629,8 +24563,24 @@ border-radius: 5; - - + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 90 + 11 + + + + + + 0 @@ -24639,7 +24589,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 @@ -24662,21 +24673,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 + + + @@ -24770,8 +24818,8 @@ border-radius: 5; 0 0 - 798 - 705 + 478 + 518 @@ -26166,7 +26214,6 @@ border-radius: 5; Qt::StrongFocus - 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 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); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index 57a6b7723..22af09bd1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -37,47 +37,64 @@ void AutoUpdatePage::enableButtons(bool enable = false) void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant value) { + QString msg; + switch (status) { case uploader::WAITING_DISCONNECT: - getWizard()->setWindowFlags(getWizard()->windowFlags() & ~Qt::WindowStaysOnTopHint); disableButtons(); - 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 (should use UploaderGadgetWidget::BOARD_EVENT_TIMEOUT) + ui->levellinProgressBar->setMaximum(20); + ui->levellinProgressBar->setValue(value.toInt()); break; case uploader::WAITING_CONNECT: - getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); - getWizard()->setWindowIcon(qApp->windowIcon()); + // Note: + // 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("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 (should use UploaderGadgetWidget::BOARD_EVENT_TIMEOUT) + 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); - getWizard()->setWindowIcon(qApp->windowIcon()); + // getWizard()->setWindowFlags(getWizard()->windowFlags() | Qt::WindowStaysOnTopHint); + // getWizard()->setWindowIcon(qApp->windowIcon()); enableButtons(true); - getWizard()->show(); - ui->statusLabel->setText("Something went wrong, you will have to manually upgrade the board using the uploader plugin"); + QString msg = value.toString(); + if (msg.isEmpty()) { + msg = tr("Something went wrong, you will have to manually upgrade the board using the uploader plugin."); + } + ui->statusLabel->setText(QString("%1").arg(msg)); break; } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 9221d7393..bdb18b3b2 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -318,19 +318,34 @@ 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::STABILIZATION2SETTINGS_MANUAL; data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; + 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_ALTITUDEHOLD; - data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_POSITIONHOLD; - 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/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..14d136def 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.cpp @@ -28,21 +28,26 @@ #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 +#include + +TelemetryPlugin::TelemetryPlugin() : firmwareWarningMessageBox(0) {} TelemetryPlugin::~TelemetryPlugin() @@ -81,166 +86,83 @@ 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(); -///*! -// 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())); -// } -// } + disconnect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); -// 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))); -// } + if (firmwareWarningMessageBox) { + delete firmwareWarningMessageBox; + } +} + +void TelemetryPlugin::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 versionFormat = "%1 (%2-%3)"; + 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()); + firmwareWarningMessageBox->setWindowModality(Qt::NonModal); + 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.")); + // should we want to re-introduce the checkbox + // 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); + firmwareWarningMessageBox->show(); + } +} diff --git a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h index 300c08ad7..6ed3f56d7 100644 --- a/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h +++ b/ground/openpilotgcs/src/plugins/telemetry/telemetryplugin.h @@ -29,6 +29,7 @@ #include +class QMessageBox; 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; + QMessageBox *firmwareWarningMessageBox; }; #endif // TELEMETRYPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 9c6dc8d67..6d74e9fa6 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -35,7 +35,6 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ $$UAVOBJECT_SYNTHETICS/attitudestate.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ - $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \ $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ @@ -62,6 +61,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 \ @@ -136,7 +136,6 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ - $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \ @@ -163,6 +162,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/ground/openpilotgcs/src/plugins/uploader/uploader.ui b/ground/openpilotgcs/src/plugins/uploader/uploader.ui index 1143f5e1f..cc143f976 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.ui +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.ui @@ -47,7 +47,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -88,10 +97,10 @@ 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 + Erase Settings @@ -192,6 +201,12 @@ through serial or USB) + + + 70 + 0 + + 75 @@ -277,7 +292,7 @@ Rescue is possible in USB mode only. 0 - false + true 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/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index d6f12e464..cf14ee25e 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -25,18 +25,111 @@ * 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 #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) : + QProgressDialog(labelText, tr("Cancel"), 0, timeout, parent, flags), bar(new QProgressBar(this)) +{ + setWindowTitle(title); + setAutoReset(false); + // open immediately... + setMinimumDuration(0); + // setup progress bar + bar->setRange(0, timeout); + bar->setFormat(tr("Timing out in %1 seconds").arg(timeout)); + setBar(bar); +} + +void TimedDialog::perform() +{ + setValue(value() + 1); + int remaining = bar->maximum() - bar->value(); + if (remaining > 0) { + bar->setFormat(tr("Timing out in %1 seconds").arg(remaining)); + } else { + bar->setFormat(tr("Timed out after %1 seconds").arg(bar->maximum())); + } +} + +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::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())); + 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(); + } +} + +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(); @@ -44,35 +137,31 @@ 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(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); - connect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); - 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(onPhisicalHWConnect())); + 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); @@ -82,11 +171,9 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) // And check whether by any chance we are not already connected if (telMngr->isConnected()) { onAutopilotConnect(); - versionMatchCheck(); } } - bool sortPorts(const QSerialPortInfo &s1, const QSerialPortInfo &s2) { return s1.portName() < s2.portName(); @@ -141,7 +228,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; } @@ -151,14 +238,12 @@ void UploaderGadgetWidget::bootButtonsSetEnable(bool enabled) m_config->bootButton->setEnabled(enabled); m_config->safeBootButton->setEnabled(enabled); - - m_config->eraseBootButton->setEnabled( - enabled && - // this feature is supported only on BL revision >= 4 - ((dfu != NULL) && (dfu->devices[0].BL_Version >= 4))); + // this feature is supported only on BL revision >= 4 + bool isBL4 = ((dfu != NULL) && (dfu->devices[0].BL_Version >= 4)); + m_config->eraseBootButton->setEnabled(isBL4 && enabled); } -void UploaderGadgetWidget::onPhisicalHWConnect() +void UploaderGadgetWidget::onPhysicalHWConnect() { bootButtonsSetEnable(false); m_config->rescueButton->setEnabled(false); @@ -190,7 +275,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")); } /** @@ -210,6 +295,14 @@ void UploaderGadgetWidget::onAutopilotDisconnect() } } +static void sleep(int ms) +{ + QEventLoop eventLoop; + QTimer::singleShot(ms, &eventLoop, SLOT(quit())); + + eventLoop.exec(); +} + /** Tell the mainboard to go to bootloader: - Send the relevant IAP commands @@ -221,7 +314,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: @@ -235,39 +328,37 @@ 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))); m_config->haltButton->setEnabled(true); break; } - QTimer::singleShot(600, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); + 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))); m_config->haltButton->setEnabled(true); break; } - QTimer::singleShot(600, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); + 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: @@ -286,14 +377,12 @@ void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) QString dli = cm->getCurrentDevice().getConName(); QString dlj = cm->getCurrentDevice().getConName(); cm->disconnectDevice(); - QTimer::singleShot(200, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); + sleep(200); // Tell connections to stop their polling threads: otherwise it will mess up DFU cm->suspendPolling(); - QTimer::singleShot(200, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); + 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 { @@ -319,7 +408,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; } @@ -330,15 +419,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(); - QTimer::singleShot(500, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); + 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; @@ -358,7 +446,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 @@ -380,20 +468,14 @@ 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 { - QMessageBox mbox(this); - mbox.setText(QString(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(); } } @@ -418,13 +500,7 @@ void UploaderGadgetWidget::systemReset() log("Board Reset initiated."); goToBootloader(); } else { - QMessageBox mbox(this); - mbox.setText(QString(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(); } } @@ -440,20 +516,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 @@ -494,7 +567,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. @@ -531,32 +604,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; } - timer.start(500); - loop.exec(); } - 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()); @@ -581,6 +647,7 @@ bool UploaderGadgetWidget::autoUpdate() emit autoUpdateSignal(FAILURE, QVariant()); return false; } + QString filename; emit autoUpdateSignal(LOADING_FW, QVariant()); switch (dfu->devices[0].ID) { @@ -617,9 +684,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(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)) { emit autoUpdateSignal(FAILURE, QVariant()); @@ -630,7 +698,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) { @@ -642,7 +710,17 @@ 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); } @@ -658,62 +736,62 @@ void UploaderGadgetWidget::systemRescue() cm->disconnectDevice(); // stop the polling thread: otherwise it will mess up DFU cm->suspendPolling(); + // Delete all previous tabs: while (m_config->systemElements->count()) { QWidget *qw = m_config->systemElements->widget(0); m_config->systemElements->removeTab(0); delete qw; } + // Existing DFU objects will have a handle over USB and will // disturb everything for the rescue process: if (dfu) { delete dfu; dfu = NULL; } + // Avoid users pressing Rescue twice. m_config->rescueButton->setEnabled(false); - // Now we're good to go: + // Now we're good to go clearLog(); log("**********************************************************"); log("** Follow those instructions to attempt a system rescue **"); log("**********************************************************"); log("You will be prompted to first connect USB, then system power"); + + // Check if boards are connected and, if yes, prompt user to disconnect them all if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { - if (QMessageBox::warning(this, tr("OpenPilot Uploader"), tr("Please disconnect your OpenPilot board"), QMessageBox::Ok, QMessageBox::Cancel) == QMessageBox::Cancel) { + 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) { + 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); return; } } - // Now we're good to go: - clearLog(); - log("**********************************************************"); - log("** Follow those instructions to attempt a system rescue **"); - log("**********************************************************"); - log("You will be prompted to first connect USB, then system power"); - m_progress = new QProgressDialog(tr("Please connect your OpenPilot board (USB only!)"), tr("Cancel"), 0, 20); - QProgressBar *bar = new QProgressBar(m_progress); - bar->setFormat("Timeout"); - m_progress->setBar(bar); - m_progress->setMinimumDuration(0); - m_progress->setRange(0, 20); - connect(m_progress, SIGNAL(canceled()), this, SLOT(cancel())); - m_timer = new QTimer(this); - connect(m_timer, SIGNAL(timeout()), this, SLOT(perform())); - m_timer->start(1000); - connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &m_eventloop, SLOT(quit())); - m_eventloop.exec(); - if (!m_timer->isActive()) { - m_progress->close(); - m_timer->stop(); - QMessageBox::warning(this, tr("OpenPilot Uploader"), tr("No board connection was detected!")); + + // 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) { + 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); return; } - m_progress->close(); - m_timer->stop(); - log("... Detecting First Board..."); - repaint(); + + log("Detecting first board..."); dfu = new DFUObject(DFU_DEBUG, false, QString()); dfu->AbortOperation(); if (!dfu->enterDFU(0)) { @@ -732,7 +810,8 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; } - log(QString("Found ") + QString::number(dfu->numberOfDevices) + QString(" device(s).")); + log(QString("Found %1 device(s).").arg(dfu->numberOfDevices)); + if (dfu->numberOfDevices > 5) { log("Inconsistent number of devices, aborting!"); delete dfu; @@ -747,36 +826,15 @@ 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); bootButtonsSetEnable(true); m_config->rescueButton->setEnabled(false); - currentStep = IAP_STATE_BOOTLOADER; // So that we can boot from the GUI afterwards. -} -void UploaderGadgetWidget::perform() -{ - if (m_progress->value() == 19) { - m_timer->stop(); - m_eventloop.exit(); - } - m_progress->setValue(m_progress->value() + 1); -} -void UploaderGadgetWidget::performAuto() -{ - ++autoUpdateConnectTimeout; - emit autoUpdateSignal(WAITING_CONNECT, autoUpdateConnectTimeout * 5); - if (autoUpdateConnectTimeout == 20) { - m_timer->stop(); - m_eventloop.exit(); - } -} -void UploaderGadgetWidget::cancel() -{ - m_timer->stop(); - m_eventloop.exit(); + // So that we can boot from the GUI afterwards. + currentStep = IAP_STATE_BOOTLOADER; } void UploaderGadgetWidget::uploadStarted() @@ -823,6 +881,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(); } @@ -831,14 +890,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); @@ -846,37 +904,57 @@ 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->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(); + m_config->autoUpdateProgressBar->setFormat(tr("Timing out in %1 seconds").arg(remaining)); 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->autoUpdateLabel->setText("Board was updated successfully, press OK to finish."); + m_config->autoUpdateProgressBar->setValue(m_config->autoUpdateProgressBar->maximum()); + 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: - m_config->autoUpdateLabel->setText("Something went wrong, you will have to manually upgrade the board. Press OK to continue."); + msg = value.toString(); + if (msg.isEmpty()) { + 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)); finishAutoUpdate(); break; } @@ -887,9 +965,8 @@ void UploaderGadgetWidget::autoUpdateStatus(uploader::AutoUpdateStep status, QVa */ void UploaderGadgetWidget::log(QString str) { - qDebug() << str; + qDebug() << "UploaderGadgetWidget -" << str; m_config->textBrowser->append(str); - m_config->textBrowser->repaint(); } void UploaderGadgetWidget::clearLog() @@ -908,92 +985,45 @@ 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. - - @param errorString The error string to display. - - @param errorNumber Not used - - */ -void UploaderGadgetWidget::error(QString errorString, int errorNumber) -{ - Q_UNUSED(errorNumber); - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText(errorString); - msgBox.exec(); - m_config->boardStatus->setText(errorString); -} - -/** - 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::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)); } + +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 14d9ac098..135513cdd 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -29,79 +29,111 @@ #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 + +public: + TimedDialog(const QString &title, const QString &labelText, int timeout, QWidget *parent = 0, Qt::WindowFlags flags = 0); + +private slots: + void perform(); + +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 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); + +private slots: + void perform(); + void deviceEvent(); + +private: + QEventLoop eventLoop; + QTimer timer; + // timeout in ms + int timeout; + // elapsed time in seconds + int elapsed; + int targetDeviceCount; + int result; +}; class UPLOADER_EXPORT UploaderGadgetWidget : public QWidget { Q_OBJECT - public: UploaderGadgetWidget(QWidget *parent = 0); ~UploaderGadgetWidget(); + + static const int BOARD_EVENT_TIMEOUT; + static const int AUTOUPDATE_CLOSE_TIMEOUT; + void log(QString str); bool autoUpdateCapable(); + public slots: void onAutopilotConnect(); void onAutopilotDisconnect(); void populate(); void openHelp(); bool autoUpdate(); - void autoUpdateProgress(int); + void autoUpdateDisconnectProgress(int); + void autoUpdateConnectProgress(int); + void autoUpdateFlashProgress(int); + signals: void autoUpdateSignal(uploader::AutoUpdateStep, QVariant); + private: Ui_UploaderWidget *m_config; DFUObject *dfu; IAPStep currentStep; 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; + int confirmEraseSettingsMessageBox(); + int cannotHaltMessageBox(); + int cannotResetMessageBox(); + private slots: - void onPhisicalHWConnect(); - void versionMatchCheck(); - void error(QString errorString, int errorNumber); - void info(QString infoString, int infoNumber); + void onPhysicalHWConnect(); void goToBootloader(UAVObject * = NULL, bool = false); void systemHalt(); void systemReset(); @@ -111,9 +143,6 @@ private slots: void commonSystemBoot(bool safeboot = false, bool erase = false); void systemRescue(); void getSerialPorts(); - void perform(); - void performAuto(); - void cancel(); void uploadStarted(); void uploadEnded(bool succeed); void downloadStarted(); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp index e58a1eee6..615c61561 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp @@ -26,10 +26,11 @@ */ #include "uploaderplugin.h" #include "uploadergadgetfactory.h" -#include -#include + #include +#include + UploaderPlugin::UploaderPlugin() { // Do nothing @@ -44,8 +45,10 @@ bool UploaderPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); + mf = new UploaderGadgetFactory(this); addAutoReleasedObject(mf); + return true; } diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h index 2fb6a19f9..245482736 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h @@ -44,6 +44,7 @@ public: void extensionsInitialized(); bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); + private: UploaderGadgetFactory *mf; }; 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 diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml deleted file mode 100644 index 39babfe63..000000000 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - Holds the desired altitude (from manual control) as well as the desired attitude to pass through - - - - - - - - - - - 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 diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index b9b6aa211..af68ab276 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -6,20 +6,71 @@ + elementnames="Roll,Pitch,Yaw,Thrust" + options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl" + defaultvalue="Attitude,Attitude,AxisLock,Manual" + limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\ + %0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\ + %0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;" + /> + elementnames="Roll,Pitch,Yaw,Thrust" + options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl" + defaultvalue="Attitude,Attitude,Rate,Manual" + limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\ + %0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\ + %0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;" + /> + elementnames="Roll,Pitch,Yaw,Thrust" + options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl" + defaultvalue="Rate,Rate,Rate,Manual" + limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \ + %NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\ + %0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\ + %0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;" + /> + + + @@ -27,32 +78,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,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI" + defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6" 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..494cf474e 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + 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 @@ + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 70bdfe9c0..963dd1440 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..230ceed95 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -22,6 +22,7 @@ + @@ -30,15 +31,17 @@ - - + + - - + + + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index f7924c0be..48a912615 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index 381f1a7c6..4d583c96d 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 14c44d626..a2346b984 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml new file mode 100644 index 000000000..0a3788b76 --- /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 + + + + + + + + +