rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'next' into corvuscorax/OP-1022_AH_improvements_amorale

Browse Source
This commit is contained in:
Corvus Corax 2014-01-11 09:40:34 +01:00
commit 4c078f3dd8
13 changed files with 558 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -10,6 +10,7 @@ GPATH
GRTAGS GRTAGS
GSYMS GSYMS
GTAGS GTAGS
core
# flight # flight
/flight/*.pnproj /flight/*.pnproj
@ -56,6 +57,7 @@ openpilotgcs-build-desktop
/.cproject /.cproject
/.project /.project
/.metadata /.metadata
/.settings
# Ignore Eclipse temp folder, git plugin based? # Ignore Eclipse temp folder, git plugin based?
RemoteSystemsTempFiles RemoteSystemsTempFiles

81
flight/modules/ManualControl/manualcontrol.c
View File

@ -690,46 +690,53 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
return; return;
} }
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_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_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_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_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order // 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.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1]; stabilization.StabilizationMode.Pitch = stab_settings[1];
stabilization.StabilizationMode.Yaw = stab_settings[2]; // Other axes (yaw) cannot be Rattitude, so use Rate
// Should really do this for Attitude mode as well?
stabilization.Roll = if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? }
cmd->Roll * stabSettings.ManualRate.Roll : else {
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : stabilization.StabilizationMode.Yaw = stab_settings[2];
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : stabilization.Yaw =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
; (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
stabilization.Pitch = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
cmd->Pitch * stabSettings.ManualRate.Pitch : 0; // this is an invalid mode
(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_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
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization); StabilizationDesiredSet(&stabilization);

265
flight/modules/Stabilization/stabilization.c
View File

@ -71,8 +71,11 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) #define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define FAILSAFE_TIMEOUT_MS 30 #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 // Private variables
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
@ -88,12 +91,15 @@ bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES]; bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f; float vbar_decay = 0.991f;
struct pid pids[PID_MAX]; struct pid pids[PID_MAX];
static uint8_t rattitude_anti_windup;
// Private functions // Private functions
static void stabilizationTask(void *parameters); static void stabilizationTask(void *parameters);
static float bound(float val, float range); static float bound(float val, float range);
static void ZeroPids(void); static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev);
static float stab_log2f(float x);
static float stab_powf(float x, uint8_t p);
/** /**
* Module initialization * Module initialization
@ -175,7 +181,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif #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) { if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
continue; continue;
@ -246,9 +252,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */ #else /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory // Simpler algorithm for CC, less memory
float local_error[3] = { stabDesired.Roll - attitudeState.Roll, float local_error[3] = { stabDesired.Roll - attitudeState.Roll,
stabDesired.Pitch - attitudeState.Pitch, stabDesired.Pitch - attitudeState.Pitch,
stabDesired.Yaw - attitudeState.Yaw }; stabDesired.Yaw - attitudeState.Yaw };
// find shortest way // find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f); float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if (modulo < 0) { if (modulo < 0) {
@ -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[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (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 *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.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 // Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = 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 // Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@ -313,10 +319,127 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
break; 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: case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output // Store for debugging output
rateDesiredAxis[i] = attitudeDesiredAxis[i]; rateDesiredAxis[i] = stabDesiredAxis[i];
// Run a virtual flybar stabilization algorithm on this axis // Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); 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; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: 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) { if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0; 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); weak_leveling = bound(weak_leveling, weak_leveling_max);
// Compute desired rate as input biased towards leveling // 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] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); 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; 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 // While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i]; rateDesiredAxis[i] = stabDesiredAxis[i];
axis_lock_accum[i] = 0; axis_lock_accum[i] = 0;
} else { } else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change // 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); 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] = 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: case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO // 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]); cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters // Run the relay controller which also estimates the oscillation parameters
@ -392,7 +521,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f); actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f);
break; break;
default: default:
error = true; error = true;
@ -484,43 +613,112 @@ static float bound(float val, float range)
return val; 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) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
StabilizationSettingsGet(&settings); StabilizationSettingsGet(&settings);
// Set the roll rate PID constants // 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, settings.RollRatePID.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd, settings.RollRatePID.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit); settings.RollRatePID.ILimit);
// Set the pitch rate PID constants // Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp, pid_configure(&pids[PID_RATE_PITCH],
pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki, settings.PitchRatePID.Kp,
pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd, settings.PitchRatePID.Ki,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit); settings.PitchRatePID.Kd,
settings.PitchRatePID.ILimit);
// Set the yaw rate PID constants // Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp, pid_configure(&pids[PID_RATE_YAW],
pids[PID_RATE_YAW].i = settings.YawRatePID.Ki, settings.YawRatePID.Kp,
pids[PID_RATE_YAW].d = settings.YawRatePID.Kd, settings.YawRatePID.Ki,
pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit); settings.YawRatePID.Kd,
settings.YawRatePID.ILimit);
// Set the roll attitude PI constants // Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], settings.RollPI.Kp, pid_configure(&pids[PID_ROLL],
settings.RollPI.Ki, 0, settings.RollPI.Kp,
pids[PID_ROLL].iLim = settings.RollPI.ILimit); settings.RollPI.Ki,
0,
settings.RollPI.ILimit);
// Set the pitch attitude PI constants // Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp, pid_configure(&pids[PID_PITCH],
pids[PID_PITCH].i = settings.PitchPI.Ki, 0, settings.PitchPI.Kp,
settings.PitchPI.Ki,
0,
settings.PitchPI.ILimit); settings.PitchPI.ILimit);
// Set the yaw attitude PI constants // Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], settings.YawPI.Kp, pid_configure(&pids[PID_YAW],
settings.YawPI.Ki, 0, settings.YawPI.Kp,
settings.YawPI.Ki,
0,
settings.YawPI.ILimit); 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 // Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -554,6 +752,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// Compute time constant for vbar decay term based on a tau // Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau); vbar_decay = expf(-fakeDt / settings.VbarTau);
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
} }

61
ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp
View File

@ -151,7 +151,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
m_oplink->PairID4->setEnabled(false); m_oplink->PairID4->setEnabled(false);
m_oplink->Bind4->setEnabled(pairid4); m_oplink->Bind4->setEnabled(pairid4);
} else { } else {
qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; qDebug() << "ConfigPipXtremeWidget: Count not read PairID field.";
} }
UAVObjectField *pairRssiField = object->getField("PairSignalStrengths"); UAVObjectField *pairRssiField = object->getField("PairSignalStrengths");
if (pairRssiField) { if (pairRssiField) {
@ -164,38 +164,43 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt()));
m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt()));
} else { } else {
qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; qDebug() << "ConfigPipXtremeWidget: Count not read PairID field.";
} }
// Update the Description field // Update the Description field
// TODO use UAVObjectUtilManager::descriptionToStructure()
UAVObjectField *descField = object->getField("Description"); UAVObjectField *descField = object->getField("Description");
if (descField) { if (descField) {
/* if (descField->getValue(0) != QChar(255)) {
* This looks like a binary with a description at the end: /*
* 4 bytes: header: "OpFw". * This looks like a binary with a description at the end:
* 4 bytes: GIT commit tag (short version of SHA1). * 4 bytes: header: "OpFw".
* 4 bytes: Unix timestamp of compile time. * 4 bytes: GIT commit tag (short version of SHA1).
* 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. * 4 bytes: Unix timestamp of compile time.
* 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded. * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files.
* 20 bytes: SHA1 sum of the firmware. * 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded.
* 20 bytes: SHA1 sum of the uavo definitions. * 20 bytes: SHA1 sum of the firmware.
* 20 bytes: free for now. * 20 bytes: SHA1 sum of the uavo definitions.
*/ * 20 bytes: free for now.
char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; */
for (unsigned int i = 0; i < 26; ++i) { char buf[OPLinkStatus::DESCRIPTION_NUMELEM];
buf[i] = descField->getValue(i + 14).toChar().toLatin1(); for (unsigned int i = 0; i < 26; ++i) {
buf[i] = descField->getValue(i + 14).toChar().toLatin1();
}
buf[26] = '\0';
QString descstr(buf);
quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF;
for (int i = 1; i < 4; i++) {
gitDate = gitDate << 8;
gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF;
}
QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm");
m_oplink->FirmwareVersion->setText(descstr + " " + date);
} else {
m_oplink->FirmwareVersion->setText(tr("Unknown"));
} }
buf[26] = '\0';
QString descstr(buf);
quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF;
for (int i = 1; i < 4; i++) {
gitDate = gitDate << 8;
gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF;
}
QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm");
m_oplink->FirmwareVersion->setText(descstr + " " + date);
} else { } else {
qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; qDebug() << "ConfigPipXtremeWidget: Failed to read Description field.";
} }
// Update the serial number field // Update the serial number field
@ -211,7 +216,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0';
m_oplink->SerialNumber->setText(buf); m_oplink->SerialNumber->setText(buf);
} else { } else {
qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; qDebug() << "ConfigPipXtremeWidget: Failed to read CPUSerial field.";
} }
// Update the link state // Update the link state
@ -219,7 +224,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
if (linkField) { if (linkField) {
m_oplink->LinkState->setText(linkField->getValue().toString()); m_oplink->LinkState->setText(linkField->getValue().toString());
} else { } else {
qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; qDebug() << "ConfigPipXtremeWidget: Failed to read LinkState field.";
} }
} }

210
ground/openpilotgcs/src/plugins/config/stabilization.ui
View File

@ -627,8 +627,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>778</width> <width>798</width>
<height>659</height> <height>656</height>
</rect> </rect>
</property> </property>
<property name="sizePolicy"> <property name="sizePolicy">
@ -8867,9 +8867,9 @@ border-radius: 5;</string>
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-117</y> <y>0</y>
<width>778</width> <width>545</width>
<height>762</height> <height>711</height>
</rect> </rect>
</property> </property>
<property name="autoFillBackground"> <property name="autoFillBackground">
@ -18908,8 +18908,8 @@ border-radius: 5;</string>
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>792</width> <width>784</width>
<height>645</height> <height>673</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_9"> <layout class="QVBoxLayout" name="verticalLayout_9">
@ -21554,6 +21554,198 @@ border-radius: 5;</string>
</layout> </layout>
</widget> </widget>
</item> </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>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;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.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</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> <item>
<widget class="QGroupBox" name="groupBox_5"> <widget class="QGroupBox" name="groupBox_5">
<property name="sizePolicy"> <property name="sizePolicy">
@ -27021,8 +27213,8 @@ border-radius: 5;</string>
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>792</width> <width>407</width>
<height>645</height> <height>518</height>
</rect> </rect>
</property> </property>
<layout class="QGridLayout" name="gridLayout_23"> <layout class="QGridLayout" name="gridLayout_23">

10
ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
View File

@ -301,6 +301,14 @@ QByteArray UAVObjectUtilManager::getBoardDescription()
return ret; return ret;
} }
QString UAVObjectUtilManager::getBoardDescriptionString()
{
QByteArray arr = getBoardDescription();
int index = arr.indexOf(255);
return QString((index == -1) ? arr : arr.left(index));
}
// ****************************** // ******************************
// HomeLocation // HomeLocation
@ -474,5 +482,3 @@ bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescrip
} }
return false; return false;
} }
// ******************************

1
ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h
View File

@ -61,6 +61,7 @@ public:
QByteArray getBoardCPUSerial(); QByteArray getBoardCPUSerial();
quint32 getFirmwareCRC(); quint32 getFirmwareCRC();
QByteArray getBoardDescription(); QByteArray getBoardDescription();
QString getBoardDescriptionString();
deviceDescriptorStruct getBoardDescriptionStruct(); deviceDescriptorStruct getBoardDescriptionStruct();
static bool descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc); static bool descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc);
UAVObjectManager *getObjectManager(); UAVObjectManager *getObjectManager();

37
ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp
View File

@ -41,8 +41,7 @@ DeviceWidget::DeviceWidget(QWidget *parent) :
connect(myDevice->updateButton, SIGNAL(clicked()), this, SLOT(uploadFirmware())); connect(myDevice->updateButton, SIGNAL(clicked()), this, SLOT(uploadFirmware()));
connect(myDevice->pbLoad, SIGNAL(clicked()), this, SLOT(loadFirmware())); connect(myDevice->pbLoad, SIGNAL(clicked()), this, SLOT(loadFirmware()));
connect(myDevice->confirmCheckBox, SIGNAL(stateChanged(int)), this, SLOT(confirmCB(int))); connect(myDevice->confirmCheckBox, SIGNAL(stateChanged(int)), this, SLOT(confirmCB(int)));
QPixmap pix = QPixmap(QString(":uploader/images/view-refresh.svg")); myDevice->statusIcon->setPixmap(QPixmap(":uploader/images/view-refresh.svg"));
myDevice->statusIcon->setPixmap(pix);
myDevice->lblCertified->setText(""); myDevice->lblCertified->setText("");
} }
@ -79,12 +78,12 @@ void DeviceWidget::populate()
{ {
int id = m_dfu->devices[deviceID].ID; int id = m_dfu->devices[deviceID].ID;
myDevice->lbldevID->setText(QString("Device ID: ") + QString::number(id, 16)); myDevice->lbldevID->setText(tr("Device ID: ") + QString::number(id, 16));
// DeviceID tells us what sort of HW we have detected: // DeviceID tells us what sort of HW we have detected:
// display a nice icon: // display a nice icon:
myDevice->gVDevice->scene()->clear(); myDevice->gVDevice->scene()->clear();
myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id)); myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id));
myDevice->lblHWRev->setText(QString(tr("HW Revision: ")) + QString::number(id & 0x00FF, 16)); myDevice->lblHWRev->setText(tr("HW Revision: ") + QString::number(id & 0x00FF, 16));
switch (id) { switch (id) {
case 0x0101: case 0x0101:
@ -115,25 +114,23 @@ void DeviceWidget::populate()
bool r = m_dfu->devices[deviceID].Readable; bool r = m_dfu->devices[deviceID].Readable;
bool w = m_dfu->devices[deviceID].Writable; bool w = m_dfu->devices[deviceID].Writable;
myDevice->lblAccess->setText(QString("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-")); myDevice->lblAccess->setText(tr("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-"));
myDevice->lblMaxCode->setText(QString("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode)); myDevice->lblMaxCode->setText(tr("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode));
myDevice->lblCRC->setText(QString::number(m_dfu->devices[deviceID].FW_CRC)); myDevice->lblCRC->setText(QString::number(m_dfu->devices[deviceID].FW_CRC));
myDevice->lblBLVer->setText(QString("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version)); myDevice->lblBLVer->setText(tr("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version));
int size = ((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc; int size = ((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc;
m_dfu->enterDFU(deviceID); m_dfu->enterDFU(deviceID);
QByteArray desc = m_dfu->DownloadDescriptionAsBA(size); QByteArray desc = m_dfu->DownloadDescriptionAsBA(size);
if (!populateBoardStructuredDescription(desc)) { if (!populateBoardStructuredDescription(desc)) {
// TODO
// desc was not a structured description // desc was not a structured description
QString str = m_dfu->DownloadDescription(size); QString str = m_dfu->DownloadDescription(size);
myDevice->lblDescription->setText(QString("Firmware custom description: ") + str.left(str.indexOf(QChar(255)))); myDevice->lblDescription->setText((!str.isEmpty()) ? str : tr("Unknown"));
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
myDevice->lblBuildDate->setText("Warning: development firmware"); myDevice->lblBuildDate->setText(tr("Unknown"));
myDevice->lblGitTag->setText(""); myDevice->lblGitTag->setText(tr("Unknown"));
myDevice->lblBrdName->setText(""); myDevice->lblBrdName->setText(tr("Unknown"));
} }
status("Ready...", STATUSICON_INFO); status("Ready...", STATUSICON_INFO);
@ -179,13 +176,11 @@ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc)
myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4, "-").insert(7, "-")); myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4, "-").insert(7, "-"));
if (onBoardDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { if (onBoardDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) {
myDevice->lblDescription->setText(onBoardDescription.gitTag); myDevice->lblDescription->setText(onBoardDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build"));
} else { } else {
myDevice->lblDescription->setText(onBoardDescription.gitTag); myDevice->lblDescription->setText(onBoardDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
} }
@ -205,14 +200,12 @@ bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc)
if (LoadedDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { if (LoadedDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) {
myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->lblDescritpionL->setText(LoadedDescription.gitTag);
myDevice->description->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/application-certificate.svg"));
myDevice->lblCertifiedL->setPixmap(pix);
myDevice->lblCertifiedL->setToolTip(tr("Tagged officially released firmware build")); myDevice->lblCertifiedL->setToolTip(tr("Tagged officially released firmware build"));
} else { } else {
myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->lblDescritpionL->setText(LoadedDescription.gitTag);
myDevice->description->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertifiedL->setPixmap(pix);
myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build")); myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build"));
} }
myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescription.boardType << 8 | LoadedDescription.boardRevision)); myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescription.boardType << 8 | LoadedDescription.boardRevision));

6
ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp
View File

@ -342,8 +342,9 @@ QString DFUObject::DownloadDescription(int const & numberOfChars)
QByteArray arr; QByteArray arr;
StartDownloadT(&arr, numberOfChars, OP_DFU::Descript); StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
QString str(arr);
return str; int index = arr.indexOf(255);
return QString((index == -1) ? arr : arr.left(index));
} }
QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars) QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars)
@ -351,6 +352,7 @@ QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars)
QByteArray arr; QByteArray arr;
StartDownloadT(&arr, numberOfChars, OP_DFU::Descript); StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
return arr; return arr;
} }

24
ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp
View File

@ -104,24 +104,22 @@ void RunningDeviceWidget::populate()
deviceDescriptorStruct devDesc; deviceDescriptorStruct devDesc;
if (UAVObjectUtilManager::descriptionToStructure(description, devDesc)) { if (UAVObjectUtilManager::descriptionToStructure(description, devDesc)) {
if (devDesc.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { if (devDesc.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) {
myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build"));
} else { } else {
myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
} }
myDevice->lblGitCommitTag->setText("Git commit hash: " + devDesc.gitHash); myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + devDesc.gitHash);
myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-")); myDevice->lblFWDate->setText(tr("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-"));
} else { } else {
myDevice->lblFWTag->setText(QString("Firmware tag: ") + QString(description).left(QString(description).indexOf(QChar(255)))); QString desc = utilMngr->getBoardDescriptionString();
myDevice->lblGitCommitTag->setText("Git commit tag: Unknown"); myDevice->lblFWTag->setText(tr("Firmware tag: ") + (!desc.isEmpty() ? desc : tr("Unknown")));
myDevice->lblFWDate->setText(QString("Firmware date: Unknown")); myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + tr("Unknown"));
myDevice->lblFWDate->setText(tr("Firmware date: ") + tr("Unknown"));
} }
} }

6
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -22,17 +22,17 @@
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode --> <!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
<field name="Stabilization1Settings" units="" type="enum" <field name="Stabilization1Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw" 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" defaultvalue="Attitude,Attitude,AxisLock"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/> limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<field name="Stabilization2Settings" units="" type="enum" <field name="Stabilization2Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw" 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" defaultvalue="Attitude,Attitude,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/> limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<field name="Stabilization3Settings" units="" type="enum" <field name="Stabilization3Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw" 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" defaultvalue="Rate,Rate,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/> limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>

2
shared/uavobjectdefinition/stabilizationdesired.xml
View File

@ -6,7 +6,7 @@
<field name="Yaw" units="degrees" type="float" elements="1"/> <field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="Throttle" units="%" type="float" elements="1"/> <field name="Throttle" units="%" type="float" elements="1"/>
<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings --> <!-- 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"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -33,6 +33,8 @@
<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/> <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="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="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"/> <field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>