mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Merge remote-tracking branch 'origin/next' into filnet/OP-1122_gcs_waypoint_editor_stabilization
Conflicts: ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
This commit is contained in:
Philippe Renon
commit
ff8a001a51
@ -690,46 +690,52 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
|
||||
return;
|
||||
}
|
||||
|
||||
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
||||
stabilization.StabilizationMode.Roll = stab_settings[0];
|
||||
stabilization.StabilizationMode.Pitch = stab_settings[1];
|
||||
stabilization.StabilizationMode.Yaw = stab_settings[2];
|
||||
|
||||
stabilization.Roll =
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? 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.ManualRate.Roll :
|
||||
(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_RELAYRATE) ?
|
||||
cmd->Roll * stabSettings.ManualRate.Roll :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
|
||||
;
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll :
|
||||
(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_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.ManualRate.Pitch :
|
||||
(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_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
|
||||
(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
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch :
|
||||
(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
|
||||
|
||||
stabilization.Yaw =
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
|
||||
cmd->Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
|
||||
(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
|
||||
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
||||
stabilization.StabilizationMode.Roll = stab_settings[0];
|
||||
stabilization.StabilizationMode.Pitch = stab_settings[1];
|
||||
// Other axes (yaw) cannot be Rattitude, so use Rate
|
||||
// Should really do this for Attitude mode as well?
|
||||
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
|
||||
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
|
||||
} else {
|
||||
stabilization.StabilizationMode.Yaw = stab_settings[2];
|
||||
stabilization.Yaw =
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
|
||||
(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_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
|
||||
0; // this is an invalid mode
|
||||
}
|
||||
|
||||
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
|
||||
StabilizationDesiredSet(&stabilization);
|
||||
|
||||
@ -71,8 +71,11 @@
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
|
||||
#define FAILSAFE_TIMEOUT_MS 30
|
||||
|
||||
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
|
||||
|
||||
// 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
|
||||
// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain
|
||||
// - two independant rate PIDs because it does rate and attitude simultaneously
|
||||
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX };
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
@ -88,12 +91,15 @@ bool lowThrottleZeroIntegral;
|
||||
bool lowThrottleZeroAxis[MAX_AXES];
|
||||
float vbar_decay = 0.991f;
|
||||
struct pid pids[PID_MAX];
|
||||
static uint8_t rattitude_anti_windup;
|
||||
|
||||
// Private functions
|
||||
static void stabilizationTask(void *parameters);
|
||||
static float bound(float val, float range);
|
||||
static void ZeroPids(void);
|
||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||
static float stab_log2f(float x);
|
||||
static float stab_powf(float x, uint8_t p);
|
||||
|
||||
/**
|
||||
* Module initialization
|
||||
@ -175,7 +181,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
|
||||
#endif
|
||||
|
||||
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
|
||||
// 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;
|
||||
@ -263,7 +269,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
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 *attitudeDesiredAxis = &stabDesired.Roll;
|
||||
float *stabDesiredAxis = &stabDesired.Roll;
|
||||
float *actuatorDesiredAxis = &actuatorDesired.Roll;
|
||||
float *rateDesiredAxis = &rateDesired.Roll;
|
||||
|
||||
@ -288,7 +294,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
// Store to rate desired variable for storing to UAVO
|
||||
rateDesiredAxis[i] =
|
||||
bound(attitudeDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
|
||||
bound(stabDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
|
||||
|
||||
// Compute the inner loop
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
@ -313,10 +319,127 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
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;
|
||||
pids[PID_RATEA_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(settings.ManualRate, settings.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(settings.RollMax, settings.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(settings.MaximumRate,
|
||||
settings.MaximumRate.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));
|
||||
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
|
||||
+ magnitude * rateDesiredAxisRate;
|
||||
|
||||
// Compute the inner loop for both Rate mode and Attitude mode
|
||||
// actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates
|
||||
actuatorDesiredAxis[i] =
|
||||
(1.0f - magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT)
|
||||
+ magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||
|
||||
// settings.RattitudeAntiWindup controls the iAccumulator zeroing
|
||||
// - so both iAccs don't wind up too far;
|
||||
// - nor do both iAccs get held too close to zero at mid stick
|
||||
|
||||
// I suspect that there would be windup without it
|
||||
// - since rate and attitude fight each other here
|
||||
// - rate trying to pull it over the top and attitude trying to pull it back down
|
||||
|
||||
// Wind-up increases linearly with cycles for a fixed error.
|
||||
// We must never increase the iAcc or we risk oscillation.
|
||||
|
||||
// Use the powf() function to make two anti-windup curves
|
||||
// - one for zeroing rate close to center stick
|
||||
// - the other for zeroing attitude close to max stick
|
||||
|
||||
// the bigger the dT the more anti windup needed
|
||||
// the bigger the PID[].i the more anti windup needed
|
||||
// more anti windup is achieved with a lower powf() power
|
||||
// a doubling of e.g. PID[].i should cause the runtime anti windup factor
|
||||
// to get twice as far away from 1.0 (towards zero)
|
||||
// e.g. from .90 to .80
|
||||
|
||||
// magic numbers
|
||||
// these generate the inverted parabola like curves that go through [0,1] and [1,0]
|
||||
// the higher the power, the closer the curve is to a straight line
|
||||
// from [0,1] to [1,1] to [1,0] and the less anti windup is applied
|
||||
|
||||
// the UAVO RattitudeAntiWindup varies from 0 to 31
|
||||
// 0 turns off anti windup
|
||||
// 1 gives very little anti-windup because the power given to powf() is 31
|
||||
// 31 gives a lot of anti-windup because the power given to powf() is 1
|
||||
// the 32.1 is what does this
|
||||
// the 7.966 and 17.668 cancel the default PID value and dT given to log2f()
|
||||
// if these are non-default, tweaking is thus done so the user doesn't have to readjust
|
||||
// the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22
|
||||
// these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits
|
||||
// used in the mantissa of the float
|
||||
// i.e. 1.0-(0.5^22) almost underflows
|
||||
|
||||
// This may only be useful for aircraft with large Ki values and limits
|
||||
if (dT > 0.0f && rattitude_anti_windup > 0.0f) {
|
||||
float factor;
|
||||
|
||||
// At magnitudes close to one, the Attitude accumulators gets zeroed
|
||||
if (pids[PID_ROLL + i].i > 0.0f) {
|
||||
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL + i].i))) - rattitude_anti_windup);
|
||||
pids[PID_ROLL + i].iAccumulator *= factor;
|
||||
}
|
||||
if (pids[PID_RATEA_ROLL + i].i > 0.0f) {
|
||||
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL + i].i))) - rattitude_anti_windup);
|
||||
pids[PID_RATEA_ROLL + i].iAccumulator *= factor;
|
||||
}
|
||||
|
||||
// At magnitudes close to zero, the Rate accumulator gets zeroed
|
||||
if (pids[PID_RATE_ROLL + i].i > 0.0f) {
|
||||
factor = 1.0f - stab_powf(1.0f - magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL + i].i))) - rattitude_anti_windup);
|
||||
pids[PID_RATE_ROLL + i].iAccumulator *= factor;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||
|
||||
// Store for debugging output
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i];
|
||||
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);
|
||||
@ -324,6 +447,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
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;
|
||||
@ -333,7 +462,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
weak_leveling = bound(weak_leveling, weak_leveling_max);
|
||||
|
||||
// Compute desired rate as input biased towards leveling
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_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);
|
||||
|
||||
@ -345,13 +474,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
if (fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) {
|
||||
if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
|
||||
// While getting strong commands act like rate mode
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i];
|
||||
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] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
|
||||
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);
|
||||
}
|
||||
@ -366,7 +495,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
|
||||
// Store to rate desired variable for storing to UAVO
|
||||
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i],
|
||||
rateDesiredAxis[i] = bound(stabDesiredAxis[i],
|
||||
cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
|
||||
|
||||
// Run the relay controller which also estimates the oscillation parameters
|
||||
@ -392,7 +521,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
|
||||
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f);
|
||||
actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f);
|
||||
break;
|
||||
default:
|
||||
error = true;
|
||||
@ -484,43 +613,109 @@ static float bound(float val, float range)
|
||||
return val;
|
||||
}
|
||||
|
||||
|
||||
// x small (0.0 < x < .01) so interpolation of fractional part is reasonable
|
||||
static float stab_log2f(float x)
|
||||
{
|
||||
union {
|
||||
volatile float f;
|
||||
volatile uint32_t i;
|
||||
volatile unsigned char c[4];
|
||||
} __attribute__((packed)) u1, u2;
|
||||
|
||||
u2.f = u1.f = x;
|
||||
u1.i <<= 1;
|
||||
u2.i &= 0xff800000;
|
||||
|
||||
// get and unbias the exponent, add in a linear interpolation of the fractional part
|
||||
return (float)(u1.c[3] - 127) + (x / u2.f) - 1.0f;
|
||||
}
|
||||
|
||||
|
||||
// 0<=x<=1, 0<=p<=31
|
||||
static float stab_powf(float x, uint8_t p)
|
||||
{
|
||||
float retval = 1.0f;
|
||||
|
||||
while (p) {
|
||||
if (p & 1) {
|
||||
retval *= x;
|
||||
}
|
||||
x *= x;
|
||||
p >>= 1;
|
||||
}
|
||||
|
||||
return retval;
|
||||
}
|
||||
|
||||
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
StabilizationSettingsGet(&settings);
|
||||
|
||||
// Set the roll rate PID constants
|
||||
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.Kp,
|
||||
pid_configure(&pids[PID_RATE_ROLL],
|
||||
settings.RollRatePID.Kp,
|
||||
settings.RollRatePID.Ki,
|
||||
pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd,
|
||||
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit);
|
||||
settings.RollRatePID.Kd,
|
||||
settings.RollRatePID.ILimit);
|
||||
|
||||
// Set the pitch rate PID constants
|
||||
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp,
|
||||
pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki,
|
||||
pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd,
|
||||
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit);
|
||||
pid_configure(&pids[PID_RATE_PITCH],
|
||||
settings.PitchRatePID.Kp,
|
||||
settings.PitchRatePID.Ki,
|
||||
settings.PitchRatePID.Kd,
|
||||
settings.PitchRatePID.ILimit);
|
||||
|
||||
// Set the yaw rate PID constants
|
||||
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp,
|
||||
pids[PID_RATE_YAW].i = settings.YawRatePID.Ki,
|
||||
pids[PID_RATE_YAW].d = settings.YawRatePID.Kd,
|
||||
pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit);
|
||||
pid_configure(&pids[PID_RATE_YAW],
|
||||
settings.YawRatePID.Kp,
|
||||
settings.YawRatePID.Ki,
|
||||
settings.YawRatePID.Kd,
|
||||
settings.YawRatePID.ILimit);
|
||||
|
||||
// Set the roll attitude PI constants
|
||||
pid_configure(&pids[PID_ROLL], settings.RollPI.Kp,
|
||||
settings.RollPI.Ki, 0,
|
||||
pids[PID_ROLL].iLim = settings.RollPI.ILimit);
|
||||
pid_configure(&pids[PID_ROLL],
|
||||
settings.RollPI.Kp,
|
||||
settings.RollPI.Ki,
|
||||
0,
|
||||
settings.RollPI.ILimit);
|
||||
|
||||
// Set the pitch attitude PI constants
|
||||
pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp,
|
||||
pids[PID_PITCH].i = settings.PitchPI.Ki, 0,
|
||||
pid_configure(&pids[PID_PITCH],
|
||||
settings.PitchPI.Kp,
|
||||
settings.PitchPI.Ki,
|
||||
0,
|
||||
settings.PitchPI.ILimit);
|
||||
|
||||
// Set the yaw attitude PI constants
|
||||
pid_configure(&pids[PID_YAW], settings.YawPI.Kp,
|
||||
settings.YawPI.Ki, 0,
|
||||
pid_configure(&pids[PID_YAW],
|
||||
settings.YawPI.Kp,
|
||||
settings.YawPI.Ki,
|
||||
0,
|
||||
settings.YawPI.ILimit);
|
||||
|
||||
// Set the Rattitude roll rate PID constants
|
||||
pid_configure(&pids[PID_RATEA_ROLL],
|
||||
settings.RollRatePID.Kp,
|
||||
settings.RollRatePID.Ki,
|
||||
settings.RollRatePID.Kd,
|
||||
settings.RollRatePID.ILimit);
|
||||
|
||||
// Set the Rattitude pitch rate PID constants
|
||||
pid_configure(&pids[PID_RATEA_PITCH],
|
||||
settings.PitchRatePID.Kp,
|
||||
settings.PitchRatePID.Ki,
|
||||
settings.PitchRatePID.Kd,
|
||||
settings.PitchRatePID.ILimit);
|
||||
|
||||
// Set the Rattitude yaw rate PID constants
|
||||
pid_configure(&pids[PID_RATEA_YAW],
|
||||
settings.YawRatePID.Kp,
|
||||
settings.YawRatePID.Ki,
|
||||
settings.YawRatePID.Kd,
|
||||
settings.YawRatePID.ILimit);
|
||||
|
||||
// Set up the derivative term
|
||||
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
|
||||
|
||||
@ -554,6 +749,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
|
||||
// Compute time constant for vbar decay term based on a tau
|
||||
vbar_decay = expf(-fakeDt / settings.VbarTau);
|
||||
|
||||
// Rattitude flight mode anti-windup factor
|
||||
rattitude_anti_windup = settings.RattitudeAntiWindup;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -199,8 +199,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
|
||||
} else {
|
||||
m_oplink->FirmwareVersion->setText(tr("Unknown"));
|
||||
}
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
qDebug() << "ConfigPipXtremeWidget: Failed to read Description field.";
|
||||
}
|
||||
|
||||
|
||||
@ -627,8 +627,8 @@
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>778</width>
|
||||
<height>659</height>
|
||||
<width>798</width>
|
||||
<height>656</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="sizePolicy">
|
||||
@ -8867,9 +8867,9 @@ border-radius: 5;</string>
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>-117</y>
|
||||
<width>778</width>
|
||||
<height>762</height>
|
||||
<y>0</y>
|
||||
<width>545</width>
|
||||
<height>711</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="autoFillBackground">
|
||||
@ -18908,8 +18908,8 @@ border-radius: 5;</string>
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>792</width>
|
||||
<height>645</height>
|
||||
<width>784</width>
|
||||
<height>673</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_9">
|
||||
@ -21554,6 +21554,198 @@ border-radius: 5;</string>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QGroupBox" name="gridGroupBox">
|
||||
<property name="title">
|
||||
<string>Rattitude</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_24">
|
||||
<property name="leftMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<item row="0" column="0">
|
||||
<spacer name="horizontalSpacer_54">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QPushButton" name="pushButton_10">
|
||||
<property name="text">
|
||||
<string>Default</string>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>button:default</string>
|
||||
<string>buttongroup:15</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0" colspan="2">
|
||||
<widget class="QGroupBox" name="gridGroupBox">
|
||||
<property name="styleSheet">
|
||||
<string notr="true">QGroupBox{border: 0px;}</string>
|
||||
</property>
|
||||
<property name="flat">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_26">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item row="1" column="0">
|
||||
<spacer name="horizontalSpacer_55">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>90</width>
|
||||
<height>11</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QDoubleSpinBox" name="HorizonAntiWindup">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>5</width>
|
||||
<height>22</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>175</width>
|
||||
<height>22</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Higher values will keep larger Ki terms and limits from winding up at partial stick. Consider increasing this if you have high Ki values and limits and a sudden stick motion from one aircraft bank angle to another causes the aircraft to rotate and then slowly change rotation.</p></body></html></string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
<property name="decimals">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<double>0.000000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>31.000000000000000</double>
|
||||
</property>
|
||||
<property name="value">
|
||||
<double>10.000000000000000</double>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:RattitudeAntiWindup</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
<string>buttongroup:15</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>144</width>
|
||||
<height>16</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>175</width>
|
||||
<height>16777215</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Anti-Windup</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<spacer name="horizontalSpacer_56">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>300</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QGroupBox" name="groupBox_5">
|
||||
<property name="sizePolicy">
|
||||
@ -27021,8 +27213,8 @@ border-radius: 5;</string>
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>792</width>
|
||||
<height>645</height>
|
||||
<width>407</width>
|
||||
<height>518</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_23">
|
||||
|
||||
@ -304,6 +304,7 @@ QString UAVObjectUtilManager::getBoardDescriptionString()
|
||||
QByteArray arr = getBoardDescription();
|
||||
|
||||
int index = arr.indexOf(255);
|
||||
|
||||
return QString((index == -1) ? arr : arr.left(index));
|
||||
}
|
||||
|
||||
|
||||
@ -22,17 +22,17 @@
|
||||
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
|
||||
<field name="Stabilization1Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
|
||||
defaultvalue="Attitude,Attitude,AxisLock"
|
||||
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
|
||||
<field name="Stabilization2Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
|
||||
defaultvalue="Attitude,Attitude,Rate"
|
||||
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
|
||||
<field name="Stabilization3Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
|
||||
defaultvalue="Rate,Rate,Rate"
|
||||
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
<field name="Yaw" units="degrees" type="float" elements="1"/>
|
||||
<field name="Throttle" units="%" type="float" elements="1"/>
|
||||
<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings -->
|
||||
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"/>
|
||||
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -33,6 +33,8 @@
|
||||
<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>
|
||||
<field name="MaxWeakLevelingRate" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
|
||||
|
||||
<field name="RattitudeAntiWindup" units="" type="uint8" elements="1" defaultvalue="10"/>
|
||||
|
||||
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
|
||||
|
||||
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user