mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Merge branch 'corvuscorax/OP-1309_Stabilization-Refactoring' into corvuscorax/OP-1259_Cruise_Control_Tweaks
Conflicts: flight/modules/Stabilization/stabilization.c shared/uavobjectdefinition/stabilizationsettings.xml
This commit is contained in:
Corvus Corax
commit
c63540b2ec
@ -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
|
||||
55
flight/libraries/math/mathmisc.h
Normal file
55
flight/libraries/math/mathmisc.h
Normal file
@ -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 val<min(boundary1,boundary2)
|
||||
// returns max(boundary1,boundary2) if val>max(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 */
|
||||
@ -30,11 +30,9 @@
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "pid.h"
|
||||
#include <mathmisc.h>
|
||||
#include <pios_math.h>
|
||||
|
||||
// ! 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;
|
||||
}
|
||||
|
||||
@ -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,19 @@ 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_AUTOTUNE:
|
||||
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
|
||||
severity = SYSTEMALARMS_ALARM_ERROR;
|
||||
}
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
|
||||
if (coptercontrol) {
|
||||
severity = SYSTEMALARMS_ALARM_ERROR;
|
||||
}
|
||||
// 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_ERROR;
|
||||
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
|
||||
severity = SYSTEMALARMS_ALARM_ERROR;
|
||||
}
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
|
||||
if (coptercontrol) {
|
||||
severity = SYSTEMALARMS_ALARM_ERROR;
|
||||
@ -199,7 +184,7 @@ int32_t configuration_check()
|
||||
* @param[in] index Which stabilization mode to check
|
||||
* @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR
|
||||
*/
|
||||
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 ||
|
||||
@ -224,18 +209,45 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
|
||||
return SYSTEMALARMS_ALARM_ERROR;
|
||||
}
|
||||
|
||||
// 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_ERROR;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// 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_ERROR;
|
||||
}
|
||||
}
|
||||
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|
||||
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|
||||
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
|
||||
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
|
||||
)) {
|
||||
return SYSTEMALARMS_ALARM_ERROR;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -85,7 +85,6 @@ static void updatePathVelocity();
|
||||
static uint8_t updateFixedDesiredAttitude();
|
||||
static void updateFixedAttitude();
|
||||
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
|
||||
static float bound(float val, float min, float max);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
@ -277,9 +276,9 @@ static void updatePathVelocity()
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
default:
|
||||
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
|
||||
bound(progress.fractional_progress, 0, 1);
|
||||
boundf(progress.fractional_progress, 0, 1);
|
||||
altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
|
||||
bound(progress.fractional_progress, 0, 1);
|
||||
boundf(progress.fractional_progress, 0, 1);
|
||||
break;
|
||||
}
|
||||
// make sure groundspeed is not zero
|
||||
@ -427,15 +426,15 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
|
||||
// Desired ground speed
|
||||
groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
|
||||
indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias,
|
||||
fixedwingpathfollowerSettings.HorizontalVelMin,
|
||||
fixedwingpathfollowerSettings.HorizontalVelMax);
|
||||
indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias,
|
||||
fixedwingpathfollowerSettings.HorizontalVelMin,
|
||||
fixedwingpathfollowerSettings.HorizontalVelMax);
|
||||
|
||||
// Airspeed error
|
||||
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
|
||||
|
||||
// Vertical speed error
|
||||
descentspeedDesired = bound(
|
||||
descentspeedDesired = boundf(
|
||||
velocityDesired.Down,
|
||||
-fixedwingpathfollowerSettings.VerticalVelMax,
|
||||
fixedwingpathfollowerSettings.VerticalVelMax);
|
||||
@ -473,14 +472,14 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
*/
|
||||
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
|
||||
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
|
||||
powerIntegral = bound(powerIntegral + -descentspeedError * dT,
|
||||
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
|
||||
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
|
||||
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
|
||||
powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
|
||||
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
|
||||
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
|
||||
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
|
||||
} else { powerIntegral = 0; }
|
||||
|
||||
// Compute the cross feed from vertical speed to pitch, with saturation
|
||||
float speedErrorToPowerCommandComponent = bound(
|
||||
float speedErrorToPowerCommandComponent = boundf(
|
||||
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
|
||||
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
|
||||
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
|
||||
@ -497,9 +496,9 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
fixedwingpathfollowerStatus.Command.Power = powerCommand;
|
||||
|
||||
// set thrust
|
||||
stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
|
||||
fixedwingpathfollowerSettings.ThrustLimit.Min,
|
||||
fixedwingpathfollowerSettings.ThrustLimit.Max);
|
||||
stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
|
||||
fixedwingpathfollowerSettings.ThrustLimit.Min,
|
||||
fixedwingpathfollowerSettings.ThrustLimit.Max);
|
||||
|
||||
// Error condition: plane cannot hold altitude at current speed.
|
||||
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
|
||||
@ -529,16 +528,16 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
|
||||
if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
|
||||
// Integrate with saturation
|
||||
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
|
||||
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
|
||||
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
|
||||
airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
|
||||
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
|
||||
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
|
||||
}
|
||||
|
||||
// Compute the cross feed from vertical speed to pitch, with saturation
|
||||
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
|
||||
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
|
||||
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
|
||||
);
|
||||
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
|
||||
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
|
||||
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
|
||||
);
|
||||
|
||||
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
|
||||
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp
|
||||
@ -549,9 +548,9 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
|
||||
fixedwingpathfollowerStatus.Command.Speed = pitchCommand;
|
||||
|
||||
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
|
||||
fixedwingpathfollowerSettings.PitchLimit.Min,
|
||||
fixedwingpathfollowerSettings.PitchLimit.Max);
|
||||
stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
|
||||
fixedwingpathfollowerSettings.PitchLimit.Min,
|
||||
fixedwingpathfollowerSettings.PitchLimit.Max);
|
||||
|
||||
// Error condition: high speed dive
|
||||
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
|
||||
@ -606,9 +605,9 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
courseError -= 360.0f;
|
||||
}
|
||||
|
||||
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
|
||||
-fixedwingpathfollowerSettings.CoursePI.ILimit,
|
||||
fixedwingpathfollowerSettings.CoursePI.ILimit);
|
||||
courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
|
||||
-fixedwingpathfollowerSettings.CoursePI.ILimit,
|
||||
fixedwingpathfollowerSettings.CoursePI.ILimit);
|
||||
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
|
||||
courseIntegral);
|
||||
|
||||
@ -616,10 +615,10 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
|
||||
fixedwingpathfollowerStatus.Command.Course = courseCommand;
|
||||
|
||||
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral +
|
||||
courseCommand,
|
||||
fixedwingpathfollowerSettings.RollLimit.Min,
|
||||
fixedwingpathfollowerSettings.RollLimit.Max);
|
||||
stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral +
|
||||
courseCommand,
|
||||
fixedwingpathfollowerSettings.RollLimit.Min,
|
||||
fixedwingpathfollowerSettings.RollLimit.Max);
|
||||
|
||||
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
|
||||
|
||||
@ -633,7 +632,7 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
|
||||
@ -642,20 +641,6 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Bound input value between limits
|
||||
*/
|
||||
static float bound(float val, float min, float max)
|
||||
{
|
||||
if (val < min) {
|
||||
val = min;
|
||||
} else if (val > max) {
|
||||
val = max;
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
|
||||
|
||||
@ -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 <manualcontrolcommand.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <altitudeholddesired.h>
|
||||
#include <altitudeholdsettings.h>
|
||||
#include <positionstate.h>
|
||||
|
||||
#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
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -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,7 +81,7 @@ void pathPlannerHandler(bool newinit);
|
||||
*/
|
||||
#define assumptions1 \
|
||||
( \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
|
||||
@ -97,7 +90,7 @@ void pathPlannerHandler(bool newinit);
|
||||
|
||||
#define assumptions3 \
|
||||
( \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
|
||||
@ -106,7 +99,7 @@ void pathPlannerHandler(bool newinit);
|
||||
|
||||
#define assumptions5 \
|
||||
( \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
|
||||
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
|
||||
@ -119,9 +112,6 @@ 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_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
|
||||
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
|
||||
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
|
||||
|
||||
@ -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,
|
||||
@ -206,7 +196,6 @@ static void manualControlTask(void)
|
||||
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 +205,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;
|
||||
|
||||
@ -86,25 +86,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,13 +120,13 @@ 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
|
||||
|
||||
@ -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 <openpilot.h>
|
||||
|
||||
#include <callbackinfo.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <pid.h>
|
||||
#include <altitudeloop.h>
|
||||
#include <CoordinateConversions.h>
|
||||
#include <altitudeholdsettings.h>
|
||||
#include <altitudeholddesired.h> // object that will be updated by the module
|
||||
#include <altitudeholdstatus.h>
|
||||
#include <flightstatus.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <accelstate.h>
|
||||
#include <pios_constants.h>
|
||||
#include <velocitystate.h>
|
||||
#include <positionstate.h>
|
||||
// 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 */
|
||||
293
flight/modules/Stabilization/cruisecontrol.c
Normal file
293
flight/modules/Stabilization/cruisecontrol.c
Normal file
@ -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 <openpilot.h>
|
||||
#include <stabilization.h>
|
||||
#include <attitudestate.h>
|
||||
#include <sin_lookup.h>
|
||||
|
||||
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;
|
||||
}
|
||||
41
flight/modules/Stabilization/inc/altitudeloop.h
Normal file
41
flight/modules/Stabilization/inc/altitudeloop.h
Normal file
@ -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 */
|
||||
42
flight/modules/Stabilization/inc/cruisecontrol.h
Normal file
42
flight/modules/Stabilization/inc/cruisecontrol.h
Normal file
@ -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 <openpilot.h>
|
||||
#include <attitudestate.h>
|
||||
|
||||
void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand);
|
||||
float cruisecontrol_apply_factor(float raw);
|
||||
|
||||
#endif /* CRUISECONTROL_H */
|
||||
38
flight/modules/Stabilization/inc/innerloop.h
Normal file
38
flight/modules/Stabilization/inc/innerloop.h
Normal file
@ -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 */
|
||||
38
flight/modules/Stabilization/inc/outerloop.h
Normal file
38
flight/modules/Stabilization/inc/outerloop.h
Normal file
@ -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 */
|
||||
@ -33,10 +33,52 @@
|
||||
#ifndef STABILIZATION_H
|
||||
#define STABILIZATION_H
|
||||
|
||||
enum { ROLL, PITCH, YAW, MAX_AXES };
|
||||
#include <openpilot.h>
|
||||
#include <pid.h>
|
||||
#include <stabilizationsettings.h>
|
||||
#include <stabilizationbank.h>
|
||||
|
||||
|
||||
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
|
||||
|
||||
/**
|
||||
|
||||
289
flight/modules/Stabilization/innerloop.c
Normal file
289
flight/modules/Stabilization/innerloop.c
Normal file
@ -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 <openpilot.h>
|
||||
#include <pios_struct_helper.h>
|
||||
#include <pid.h>
|
||||
#include <callbackinfo.h>
|
||||
#include <ratedesired.h>
|
||||
#include <actuatordesired.h>
|
||||
#include <gyrostate.h>
|
||||
#include <airspeedstate.h>
|
||||
#include <stabilizationstatus.h>
|
||||
#include <flightstatus.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <stabilizationbank.h>
|
||||
|
||||
#include <stabilization.h>
|
||||
#include <relay_tuning.h>
|
||||
#include <virtualflybar.h>
|
||||
#include <cruisecontrol.h>
|
||||
|
||||
// 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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
288
flight/modules/Stabilization/outerloop.c
Normal file
288
flight/modules/Stabilization/outerloop.c
Normal file
@ -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 <openpilot.h>
|
||||
#include <pios_struct_helper.h>
|
||||
#include <pid.h>
|
||||
#include <callbackinfo.h>
|
||||
#include <ratedesired.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <attitudestate.h>
|
||||
#include <stabilizationstatus.h>
|
||||
#include <flightstatus.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <stabilizationbank.h>
|
||||
|
||||
|
||||
#include <stabilization.h>
|
||||
#include <cruisecontrol.h>
|
||||
#include <altitudeloop.h>
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -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.
|
||||
|
||||
@ -33,128 +33,55 @@
|
||||
|
||||
#include <openpilot.h>
|
||||
#include <pios_struct_helper.h>
|
||||
#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 <pid.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <stabilizationsettings.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <stabilizationstatus.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <stabilizationsettingsbank1.h>
|
||||
#include <stabilizationsettingsbank2.h>
|
||||
#include <stabilizationsettingsbank3.h>
|
||||
#include <relaytuning.h>
|
||||
#include <relaytuningsettings.h>
|
||||
#include <ratedesired.h>
|
||||
#include <sin_lookup.h>
|
||||
#include <stabilization.h>
|
||||
#include <innerloop.h>
|
||||
#include <outerloop.h>
|
||||
#include <altitudeloop.h>
|
||||
|
||||
// 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 float cruise_control_thrust_difference;
|
||||
static float cruise_control_max_angle;
|
||||
static float cruise_control_max_power_factor;
|
||||
static float cruise_control_power_trim;
|
||||
static float cruise_control_max_power_factor_angle;
|
||||
static float cruise_control_half_power_delay;
|
||||
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
|
||||
static uint8_t cruise_control_inverted_thrust_reversing;
|
||||
static uint8_t cruise_control_inverted_power_output;
|
||||
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 float CruiseControlAngleToFactor(float angle);
|
||||
static float CruiseControlFactorToThrust(float factor, float stick_thrust);
|
||||
static float CruiseControlLimitThrust(float thrust);
|
||||
static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
|
||||
static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
/**
|
||||
* Module initialization
|
||||
*/
|
||||
int32_t StabilizationStart()
|
||||
{
|
||||
// Initialize variables
|
||||
// Create object queue
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Listen for updates.
|
||||
// AttitudeStateConnectQueue(queue);
|
||||
GyroStateConnectQueue(queue);
|
||||
|
||||
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
|
||||
SettingsUpdatedCb(StabilizationSettingsHandle());
|
||||
|
||||
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
|
||||
StabilizationBankConnectCallback(BankUpdatedCb);
|
||||
|
||||
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
|
||||
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
|
||||
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
|
||||
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
|
||||
SettingsUpdatedCb(StabilizationSettingsHandle());
|
||||
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
|
||||
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
|
||||
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
|
||||
#endif
|
||||
@ -167,875 +94,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;
|
||||
|
||||
///////////////////////////////////////////////////////////////////////
|
||||
// Cruise Control
|
||||
// modify thrust according to 1/cos(bank angle)
|
||||
// to maintain same altitude with changing bank angle
|
||||
// but without manually adjusting thrust
|
||||
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
|
||||
///////////////////////////////////////////////////////////////////////
|
||||
|
||||
// Detect if the flight mode switch has changed. If it has, there
|
||||
// could be a time gap. E.g. enabled, then disabled for 30 seconds
|
||||
// then enabled again. Previous_angle will also be invalid because
|
||||
// of the time spent with Cruise Control off.
|
||||
static bool previous_time_valid; // initially false
|
||||
static uint8_t previous_flight_mode_switch_position = 250;
|
||||
if (flight_mode_switch_position != previous_flight_mode_switch_position) {
|
||||
previous_flight_mode_switch_position = flight_mode_switch_position;
|
||||
// Force calculations on this pass (usually every 8th pass),
|
||||
// but ignore rate calculations (uses time, previous_time, angle,
|
||||
// previous_angle)
|
||||
previous_time_valid = false;
|
||||
}
|
||||
|
||||
if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
|
||||
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
|
||||
&& cruise_control_max_power_factor > 0.0001f) {
|
||||
static float factor;
|
||||
static float previous_angle;
|
||||
static uint32_t previous_time;
|
||||
static uint8_t calc_count;
|
||||
uint32_t time;
|
||||
|
||||
// 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.
|
||||
|
||||
time = PIOS_DELAY_GetuS();
|
||||
|
||||
// Get roll and pitch angles, calculate combined angle, and begin
|
||||
// the general algorithm.
|
||||
// Example: 45 degrees roll plus 45 degrees pitch = 60 degrees
|
||||
// Do it every 8th iteration to save CPU.
|
||||
if ((time != previous_time && calc_count++ >= 8) || previous_time_valid == false) {
|
||||
float angle, angle_unmodified;
|
||||
|
||||
calc_count = 0;
|
||||
|
||||
// spherical right triangle
|
||||
// 0.0 <= angle <= 180.0
|
||||
angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll)
|
||||
* cos_lookup_deg(attitudeState.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
|
||||
// actuatorDesired.Thrust (negative stick values)
|
||||
//
|
||||
// When the aircraft is upright, thrust is always boosted
|
||||
// . for positive values of actuatorDesired.Thrust
|
||||
// When the aircraft is inverted, thrust is sometimes
|
||||
// . boosted or reversed (or combinations thereof) or zeroed
|
||||
// . for positive values of actuatorDesired.Thrust
|
||||
// It depends on the inverted power settings.
|
||||
// Of course, you can set MaxPowerFactor to 1.0 to
|
||||
// . effectively disable boost.
|
||||
if (actuatorDesired.Thrust > 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 (actuatorDesired.Thrust > 0.7f) {
|
||||
float thrust;
|
||||
|
||||
thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust);
|
||||
|
||||
// determine if we are in range of the transition
|
||||
|
||||
// given the thrust at max_angle and actuatorDesired.Thrust
|
||||
// (typically close to 1.0), change variable 'thrust' to
|
||||
// be the proportion of the largest thrust change possible
|
||||
// that occurs when going into inverted mode.
|
||||
// Example: 'thrust' is 0.8 A quad has min_thrust set
|
||||
// to 0.05 The difference is 0.75. The largest possible
|
||||
// difference with this setup is 0.9 - 0.05 = 0.85, so
|
||||
// the proportion is 0.75/0.85
|
||||
// That is nearly a full throttle stroke.
|
||||
// the 'thrust' variable is non-negative here
|
||||
switch (cruise_control_inverted_power_output) {
|
||||
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)) / cruise_control_thrust_difference;
|
||||
break;
|
||||
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
|
||||
// reversed but not boosted
|
||||
// : CP heli case, stroke is max to -stick
|
||||
// : thrust = (thrust - CruiseControlLimitThrust(-actuatorDesired.Thrust)) / cruise_control_thrust_difference;
|
||||
// else it is both unreversed and unboosted
|
||||
// : simply turn off boost, stroke is max to +stick
|
||||
// : thrust = (thrust - CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference;
|
||||
thrust = (thrust - CruiseControlLimitThrust(
|
||||
(cruise_control_inverted_thrust_reversing
|
||||
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED)
|
||||
? -actuatorDesired.Thrust
|
||||
: actuatorDesired.Thrust)) / cruise_control_thrust_difference;
|
||||
break;
|
||||
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
|
||||
// if boosted and reversed
|
||||
if (cruise_control_inverted_thrust_reversing
|
||||
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
|
||||
// CP heli case, stroke is max to min
|
||||
thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust)) / cruise_control_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 *= cruise_control_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 (cruise_control_max_angle - thrust <= angle
|
||||
&& angle <= cruise_control_max_angle + thrust) {
|
||||
// default to a little above max angle
|
||||
angle = cruise_control_max_angle + 0.01f;
|
||||
// if roll direction is downward
|
||||
// then thrust value is taken from below max angle
|
||||
// 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
|
||||
factor = CruiseControlAngleToFactor(angle);
|
||||
} else { // if thrust > 0 set factor from angle; else
|
||||
factor = 1.0f;
|
||||
}
|
||||
|
||||
if (angle >= cruise_control_max_angle) {
|
||||
switch (cruise_control_inverted_power_output) {
|
||||
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
|
||||
factor = 0.0f;
|
||||
break;
|
||||
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
|
||||
factor = 1.0f;
|
||||
break;
|
||||
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
|
||||
// no change, leave factor >= 1.0 alone
|
||||
break;
|
||||
}
|
||||
if (cruise_control_inverted_thrust_reversing
|
||||
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
|
||||
factor = -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
|
||||
|
||||
// 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
|
||||
actuatorDesired.Thrust = CruiseControlFactorToThrust(factor, actuatorDesired.Thrust);
|
||||
} // if Cruise Control is enabled on this flight switch position
|
||||
|
||||
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
||||
ActuatorDesiredSet(&actuatorDesired);
|
||||
} else {
|
||||
// Force all axes to reinitialize when engaged
|
||||
for (uint8_t i = 0; i < MAX_AXES; i++) {
|
||||
previous_mode[i] = 255;
|
||||
}
|
||||
}
|
||||
|
||||
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
|
||||
(lowThrottleZeroIntegral && throttleDesired < 0)) {
|
||||
// Force all axes to reinitialize when engaged
|
||||
for (uint8_t i = 0; i < MAX_AXES; i++) {
|
||||
previous_mode[i] = 255;
|
||||
}
|
||||
}
|
||||
|
||||
// Clear or set alarms. Done like this to prevent toggline each cycle
|
||||
// and hammering system alarms
|
||||
if (error) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
|
||||
StabilizationStatusData status;
|
||||
StabilizationDesiredStabilizationModeData mode;
|
||||
int t;
|
||||
|
||||
StabilizationDesiredStabilizationModeGet(&mode);
|
||||
for (t = 0; t < AXES; t++) {
|
||||
switch (cast_struct_to_array(mode, mode.Roll)[t]) {
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
StabilizationStatusSet(&status);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Clear the accumulators and derivatives for all the axes
|
||||
*/
|
||||
static void ZeroPids(void)
|
||||
static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
for (uint32_t i = 0; i < PID_MAX; i++) {
|
||||
pid_zero(&pids[i]);
|
||||
}
|
||||
uint8_t fm;
|
||||
|
||||
ManualControlCommandFlightModeSwitchPositionGet(&fm);
|
||||
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
axis_lock_accum[i] = 0.0f;
|
||||
if (fm == cur_flight_mode) {
|
||||
return;
|
||||
}
|
||||
cur_flight_mode = fm;
|
||||
SettingsBankUpdatedCb(NULL);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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);
|
||||
}
|
||||
|
||||
|
||||
static float CruiseControlAngleToFactor(float angle)
|
||||
{
|
||||
float factor;
|
||||
// avoid singularity
|
||||
if (angle > 89.999f && angle < 90.001f) {
|
||||
factor = cruise_control_max_power_factor;
|
||||
} else {
|
||||
// the simple bank angle boost calculation that Cruise Control revolves around
|
||||
factor = 1.0f / fabsf(cos_lookup_deg(angle));
|
||||
// factor in the power trim, no effect at 1.0, linear effect increases with factor
|
||||
factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
|
||||
// limit to user specified max power multiplier
|
||||
if (factor > cruise_control_max_power_factor) {
|
||||
factor = cruise_control_max_power_factor;
|
||||
}
|
||||
}
|
||||
return (factor);
|
||||
}
|
||||
|
||||
|
||||
// assumes 1.0 <= factor <= 100.0
|
||||
// a factor of less than 1.0 could make it return a value less than cruise_control_min_thrust
|
||||
// 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 float CruiseControlFactorToThrust(float factor, float thrust)
|
||||
{
|
||||
// don't touch if below min_thrust so we don't limit to min of min_thrust
|
||||
// e.g. multicopter motors always spin
|
||||
if (thrust > cruise_control_min_thrust) {
|
||||
thrust = CruiseControlLimitThrust(thrust * factor);
|
||||
}
|
||||
return (thrust);
|
||||
}
|
||||
|
||||
|
||||
static float CruiseControlLimitThrust(float thrust)
|
||||
{
|
||||
// limit to user specified absolute max thrust
|
||||
if (thrust > cruise_control_max_thrust) {
|
||||
thrust = cruise_control_max_thrust;
|
||||
} else if (thrust < cruise_control_min_thrust) {
|
||||
thrust = cruise_control_min_thrust;
|
||||
}
|
||||
return (thrust);
|
||||
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
|
||||
@ -1043,42 +284,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_thrust_difference = cruise_control_max_thrust - cruise_control_min_thrust;
|
||||
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;
|
||||
|
||||
cruise_control_max_angle = (float) settings.CruiseControlMaxAngle;
|
||||
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
|
||||
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
|
||||
cruise_control_half_power_delay = settings.CruiseControlPowerDelayComp / 2.0f;
|
||||
cruise_control_max_power_factor_angle = RAD2DEG(acosf(1.0f / settings.CruiseControlMaxPowerFactor));
|
||||
|
||||
cruise_control_inverted_thrust_reversing = settings.CruiseControlInvertedThrustReversing;
|
||||
cruise_control_inverted_power_output = settings.CruiseControlInvertedPowerOutput;
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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,
|
||||
@ -415,6 +419,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);
|
||||
@ -453,7 +462,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);
|
||||
@ -540,6 +554,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()) {
|
||||
|
||||
@ -101,7 +101,6 @@ static void updatePathVelocity();
|
||||
static void updateEndpointVelocity();
|
||||
static void updateFixedAttitude(float *attitude);
|
||||
static void updateVtolDesiredAttitude(bool yaw_attitude);
|
||||
static float bound(float val, float min, float max);
|
||||
static bool vtolpathfollower_enabled;
|
||||
static void accessoryUpdated(UAVObjEvent *ev);
|
||||
|
||||
@ -394,7 +393,7 @@ static void updatePathVelocity()
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
case PATHDESIRED_MODE_DRIVEENDPOINT:
|
||||
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
|
||||
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
|
||||
if (progress.fractional_progress > 1) {
|
||||
groundspeed = 0;
|
||||
}
|
||||
@ -403,7 +402,7 @@ static void updatePathVelocity()
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
default:
|
||||
groundspeed = pathDesired.StartingVelocity
|
||||
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
|
||||
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
|
||||
if (progress.fractional_progress > 1) {
|
||||
groundspeed = 0;
|
||||
}
|
||||
@ -427,14 +426,14 @@ static void updatePathVelocity()
|
||||
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
|
||||
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
|
||||
|
||||
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1);
|
||||
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
|
||||
|
||||
float downError = altitudeSetpoint - positionState.Down;
|
||||
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
||||
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
|
||||
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
// update pathstatus
|
||||
pathStatus.error = progress.error;
|
||||
@ -472,15 +471,15 @@ void updateEndpointVelocity()
|
||||
|
||||
// Compute desired north command
|
||||
northError = pathDesired.End.North - positionState.North;
|
||||
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
||||
northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
|
||||
|
||||
eastError = pathDesired.End.East - positionState.East;
|
||||
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
||||
eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral);
|
||||
|
||||
// Limit the maximum velocity
|
||||
@ -494,11 +493,11 @@ void updateEndpointVelocity()
|
||||
velocityDesired.East = eastCommand * scale;
|
||||
|
||||
downError = pathDesired.End.Down - positionState.Down;
|
||||
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
||||
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
|
||||
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
@ -595,18 +594,18 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
|
||||
|
||||
// Compute desired north command
|
||||
northError = velocityDesired.North - northVel;
|
||||
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
|
||||
northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral
|
||||
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd
|
||||
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
|
||||
// Compute desired east command
|
||||
eastError = velocityDesired.East - eastVel;
|
||||
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
|
||||
eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
|
||||
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral
|
||||
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd
|
||||
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
@ -615,22 +614,22 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
|
||||
downError = velocityDesired.Down - downVel;
|
||||
// Must flip this sign
|
||||
downError = -downError;
|
||||
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
|
||||
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
|
||||
vtolpathfollowerSettings.VerticalVelPID.ILimit);
|
||||
downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
|
||||
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
|
||||
vtolpathfollowerSettings.VerticalVelPID.ILimit);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
|
||||
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
|
||||
|
||||
stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1);
|
||||
stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1);
|
||||
|
||||
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
|
||||
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
||||
stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
|
||||
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
|
||||
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
|
||||
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
|
||||
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
|
||||
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
|
||||
// For now override thrust with manual control. Disable at your risk, quad goes to China.
|
||||
@ -692,19 +691,6 @@ static void updateNedAccel()
|
||||
NedAccelSet(&accelData);
|
||||
}
|
||||
|
||||
/**
|
||||
* Bound input value between limits
|
||||
*/
|
||||
static float bound(float val, float min, float max)
|
||||
{
|
||||
if (val < min) {
|
||||
val = min;
|
||||
} else if (val > max) {
|
||||
val = max;
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
#include <mathmisc.h>
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -40,6 +40,7 @@
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
#include <mathmisc.h>
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
#include <mathmisc.h>
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -40,6 +40,7 @@
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
#include <mathmisc.h>
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -40,6 +40,7 @@
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
#include <mathmisc.h>
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -40,6 +40,7 @@
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
#include <mathmisc.h>
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
@ -152,6 +152,9 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
|
||||
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", "Stabilization1Settings", ui->fmsSsPos1Thrust, "Thrust", 1, true);
|
||||
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Thrust, "Thrust", 1, true);
|
||||
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Thrust, "Thrust", 1, true);
|
||||
|
||||
addWidgetBinding("FlightModeSettings", "Arming", ui->armControl);
|
||||
addWidgetBinding("FlightModeSettings", "ArmedTimeout", ui->armTimeout, 0, 1000);
|
||||
@ -1324,32 +1327,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 +1356,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:
|
||||
|
||||
@ -17,7 +17,7 @@
|
||||
<item>
|
||||
<widget class="QTabWidget" name="tabWidget">
|
||||
<property name="currentIndex">
|
||||
<number>0</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="RCInput">
|
||||
<attribute name="title">
|
||||
@ -27,16 +27,7 @@
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -117,20 +108,11 @@
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>774</width>
|
||||
<height>748</height>
|
||||
<height>753</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout">
|
||||
<property name="leftMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="verticalSpacing">
|
||||
@ -161,16 +143,7 @@
|
||||
</property>
|
||||
<widget class="QWidget" name="advancedPage">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_3">
|
||||
<property name="leftMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -269,16 +242,7 @@
|
||||
</widget>
|
||||
<widget class="QWidget" name="wizard">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_5">
|
||||
<property name="leftMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -457,16 +421,7 @@
|
||||
<string>Flight Mode Switch Settings</string>
|
||||
</attribute>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_8">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -546,21 +501,12 @@
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>768</width>
|
||||
<height>742</height>
|
||||
<width>774</width>
|
||||
<height>753</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_7" rowstretch="1,0,0,0">
|
||||
<property name="leftMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<item row="3" column="0">
|
||||
@ -587,7 +533,7 @@
|
||||
<property name="title">
|
||||
<string>Stabilization Modes Configuration</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0,0">
|
||||
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0,0,0,0">
|
||||
<property name="leftMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
@ -626,6 +572,51 @@
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="10">
|
||||
<spacer name="horizontalSpacer_15">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="11">
|
||||
<widget class="QLabel" name="label_11">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>120</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</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;
|
||||
font: bold 12px;
|
||||
margin:1px;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Thrust</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="9">
|
||||
<widget class="QLabel" name="label_10">
|
||||
<property name="minimumSize">
|
||||
@ -684,7 +675,7 @@ margin:1px;</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="10">
|
||||
<item row="1" column="12">
|
||||
<spacer name="horizontalSpacer_11">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
@ -792,16 +783,7 @@ margin:1px;</string>
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_10">
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -843,16 +825,7 @@ margin:1px;</string>
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_14">
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -894,16 +867,7 @@ margin:1px;</string>
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_15">
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -936,6 +900,48 @@ margin:1px;</string>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="11">
|
||||
<widget class="QFrame" name="frame_7">
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::NoFrame</enum>
|
||||
</property>
|
||||
<property name="frameShadow">
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_16">
|
||||
<property name="margin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QComboBox" name="fmsSsPos1Thrust">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="fmsSsPos2Thrust">
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="fmsSsPos3Thrust">
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<spacer name="horizontalSpacer_14">
|
||||
<property name="orientation">
|
||||
@ -973,23 +979,14 @@ margin:1px;</string>
|
||||
<string>Flight Mode Switch Positions</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_4">
|
||||
<property name="leftMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>9</number>
|
||||
</property>
|
||||
<property name="horizontalSpacing">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item row="0" column="6">
|
||||
<widget class="QLabel" name="label_11">
|
||||
<widget class="QLabel" name="label_111">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>80</width>
|
||||
@ -1078,7 +1075,7 @@ channel value for each flight mode.</string>
|
||||
<item row="2" column="10">
|
||||
<widget class="QLabel" name="label_15">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
|
||||
<sizepolicy hsizetype="Ignored" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
@ -1090,7 +1087,7 @@ channel value for each flight mode.</string>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Avoid "Manual" for multirotors!</string>
|
||||
<string>Avoid "Manual" for multirotors! Never select "Altitude", "VelocityControl" or "CruiseControl" on a fixed wing!</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
@ -1116,22 +1113,6 @@ channel value for each flight mode.</string>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="5">
|
||||
<spacer name="horizontalSpacer_15">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="7">
|
||||
<spacer name="horizontalSpacer_16">
|
||||
<property name="orientation">
|
||||
@ -1164,35 +1145,6 @@ channel value for each flight mode.</string>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="4">
|
||||
<widget class="QLabel" name="label_12">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>105</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</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;
|
||||
font: bold 12px;
|
||||
margin:1px;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Cruise Control</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="8" rowspan="3">
|
||||
<widget class="Line" name="line">
|
||||
<property name="orientation">
|
||||
@ -1229,294 +1181,6 @@ margin:1px;</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="4" rowspan="2">
|
||||
<widget class="QFrame" name="frame_4">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>30</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>16777215</height>
|
||||
</size>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_4">
|
||||
<property name="spacing">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item alignment="Qt::AlignHCenter">
|
||||
<widget class="QCheckBox" name="cc_box_0">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Minimum">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #1.</p></body></html></string>
|
||||
</property>
|
||||
<property name="layoutDirection">
|
||||
<enum>Qt::RightToLeft</enum>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
|
||||
<string>index:0</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item alignment="Qt::AlignHCenter">
|
||||
<widget class="QCheckBox" name="cc_box_1">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #2.</p></body></html></string>
|
||||
</property>
|
||||
<property name="layoutDirection">
|
||||
<enum>Qt::RightToLeft</enum>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
|
||||
<string>index:1</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item alignment="Qt::AlignHCenter">
|
||||
<widget class="QCheckBox" name="cc_box_2">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #3.</p></body></html></string>
|
||||
</property>
|
||||
<property name="layoutDirection">
|
||||
<enum>Qt::RightToLeft</enum>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
|
||||
<string>index:2</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item alignment="Qt::AlignHCenter">
|
||||
<widget class="QCheckBox" name="cc_box_3">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #4.</p></body></html></string>
|
||||
</property>
|
||||
<property name="layoutDirection">
|
||||
<enum>Qt::RightToLeft</enum>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
|
||||
<string>index:3</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item alignment="Qt::AlignHCenter">
|
||||
<widget class="QCheckBox" name="cc_box_4">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #5.</p></body></html></string>
|
||||
</property>
|
||||
<property name="layoutDirection">
|
||||
<enum>Qt::RightToLeft</enum>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
|
||||
<string>index:4</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item alignment="Qt::AlignHCenter">
|
||||
<widget class="QCheckBox" name="cc_box_5">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #6.</p></body></html></string>
|
||||
</property>
|
||||
<property name="layoutDirection">
|
||||
<enum>Qt::RightToLeft</enum>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
|
||||
<string>index:5</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0" rowspan="2">
|
||||
<widget class="QFrame" name="frame">
|
||||
<property name="minimumSize">
|
||||
@ -1538,21 +1202,12 @@ margin:1px;</string>
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_8">
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="horizontalSpacing">
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="margin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item row="4" column="1">
|
||||
<widget class="QLabel" name="label_pos5">
|
||||
<property name="sizePolicy">
|
||||
@ -1770,16 +1425,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
|
||||
</size>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_6">
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item row="5" column="1">
|
||||
@ -1867,7 +1513,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="6" rowspan="2">
|
||||
<widget class="QFrame" name="frame_7">
|
||||
<widget class="QFrame" name="frame_8">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
@ -1881,16 +1527,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
|
||||
</size>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_9">
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -2075,16 +1712,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
|
||||
<string>Arming Settings</string>
|
||||
</attribute>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_12">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@ -2164,21 +1792,12 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>768</width>
|
||||
<height>742</height>
|
||||
<width>774</width>
|
||||
<height>753</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_2">
|
||||
<property name="leftMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<item>
|
||||
|
||||
@ -8444,7 +8444,7 @@ border-radius: 5;</string>
|
||||
<string notr="true"/>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Max rate attitude (deg/s)</string>
|
||||
<string>Max rate limit (all modes) (deg/s)</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
|
||||
@ -318,18 +318,21 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration()
|
||||
data.Stabilization1Settings[0] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
|
||||
data.Stabilization1Settings[1] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
|
||||
data.Stabilization1Settings[2] = FlightModeSettings::STABILIZATION1SETTINGS_AXISLOCK;
|
||||
data.Stabilization1Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL;
|
||||
data.Stabilization2Settings[0] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
|
||||
data.Stabilization2Settings[1] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
|
||||
data.Stabilization2Settings[2] = FlightModeSettings::STABILIZATION2SETTINGS_RATE;
|
||||
data.Stabilization2Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL;
|
||||
data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
|
||||
data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
|
||||
data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
|
||||
data.Stabilization3Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL;
|
||||
data2.FlightModeNumber = 3;
|
||||
data.FlightModePosition[0] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1;
|
||||
data.FlightModePosition[1] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2;
|
||||
data.FlightModePosition[2] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED3;
|
||||
data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD;
|
||||
data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_POSITIONHOLD;
|
||||
data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
|
||||
data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
|
||||
data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
|
||||
modeSettings->setData(data);
|
||||
addModifiedObject(modeSettings, tr("Writing flight mode settings 1/2"));
|
||||
|
||||
@ -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 \
|
||||
|
||||
@ -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
|
||||
|
||||
@ -1,14 +0,0 @@
|
||||
<xml>
|
||||
<object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
|
||||
<description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
|
||||
<field name="SetPoint" units="" type="float" elements="1"/>
|
||||
<field name="ControlMode" units="" type="enum" elements="1" options="Altitude,Velocity,Thrust" />
|
||||
<field name="Roll" units="deg" type="float" elements="1"/>
|
||||
<field name="Pitch" units="deg" type="float" elements="1"/>
|
||||
<field name="Yaw" units="deg/s" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -6,6 +6,8 @@
|
||||
<elementname>EventDispatcher</elementname>
|
||||
<elementname>StateEstimation</elementname>
|
||||
<elementname>AltitudeHold</elementname>
|
||||
<elementname>Stabilization0</elementname>
|
||||
<elementname>Stabilization1</elementname>
|
||||
<elementname>PathPlanner0</elementname>
|
||||
<elementname>PathPlanner1</elementname>
|
||||
<elementname>ManualControl</elementname>
|
||||
@ -16,6 +18,8 @@
|
||||
<elementname>EventDispatcher</elementname>
|
||||
<elementname>StateEstimation</elementname>
|
||||
<elementname>AltitudeHold</elementname>
|
||||
<elementname>Stabilization0</elementname>
|
||||
<elementname>Stabilization1</elementname>
|
||||
<elementname>PathPlanner0</elementname>
|
||||
<elementname>PathPlanner1</elementname>
|
||||
<elementname>ManualControl</elementname>
|
||||
@ -30,6 +34,8 @@
|
||||
<elementname>EventDispatcher</elementname>
|
||||
<elementname>StateEstimation</elementname>
|
||||
<elementname>AltitudeHold</elementname>
|
||||
<elementname>Stabilization0</elementname>
|
||||
<elementname>Stabilization1</elementname>
|
||||
<elementname>PathPlanner0</elementname>
|
||||
<elementname>PathPlanner1</elementname>
|
||||
<elementname>ManualControl</elementname>
|
||||
|
||||
@ -6,20 +6,38 @@
|
||||
|
||||
<!-- 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,Rattitude,RelayRate,RelayAttitude"
|
||||
defaultvalue="Attitude,Attitude,AxisLock"
|
||||
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
|
||||
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;"
|
||||
/>
|
||||
<field name="Stabilization2Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
|
||||
defaultvalue="Attitude,Attitude,Rate"
|
||||
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
|
||||
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;"
|
||||
/>
|
||||
<field name="Stabilization3Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw"
|
||||
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
|
||||
defaultvalue="Rate,Rate,Rate"
|
||||
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
|
||||
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;"
|
||||
/>
|
||||
|
||||
<!-- Note these options values should be identical to those defined in FlightMode -->
|
||||
<!-- Currently only some modes are enabled for UI using limits attribute per board. Update when more modes will be operational -->
|
||||
@ -27,32 +45,32 @@
|
||||
units=""
|
||||
type="enum"
|
||||
elements="6"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
|
||||
defaultvalue="Stabilized1,Stabilized2,Stabilized3,AltitudeHold,AltitudeVario,Manual"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"
|
||||
defaultvalue="Stabilized1,Stabilized2,Stabilized3,Manual,Manual,Manual"
|
||||
limits="\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
|
||||
\
|
||||
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
|
||||
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
|
||||
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
|
||||
|
||||
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
|
||||
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
|
||||
|
||||
<!-- Note these enumerated values should be the same as ManualControlSettings -->
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
|
||||
|
||||
<field name="ControlChain" units="bool" type="enum" options="false,true">
|
||||
<elementnames>
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
<field name="Roll" units="deg/s" type="float" elements="1"/>
|
||||
<field name="Pitch" units="deg/s" type="float" elements="1"/>
|
||||
<field name="Yaw" units="deg/s" type="float" elements="1"/>
|
||||
<field name="Thrust" units="%" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -5,9 +5,8 @@
|
||||
<field name="Pitch" units="degrees" type="float" elements="1"/>
|
||||
<field name="Yaw" units="degrees" type="float" elements="1"/>
|
||||
<field name="Thrust" 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,Rattitude,RelayRate,RelayAttitude"/>
|
||||
<field name="ThrustStabilizationMode" units="" type="enum" elements="1" options="None"/>
|
||||
<!-- These values should match those in FlightModeSettings.Stabilization{1,2,3}Settings -->
|
||||
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -22,6 +22,7 @@
|
||||
<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
|
||||
<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
|
||||
|
||||
<field name="AxisLockKp" units="" type="float" elements="1" defaultvalue="2.5"/>
|
||||
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="30"/>
|
||||
<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
|
||||
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
|
||||
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
|
||||
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
|
||||
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
|
||||
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
|
||||
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
|
||||
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
|
||||
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
|
||||
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
|
||||
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
|
||||
|
||||
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
|
||||
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
|
||||
|
||||
28
shared/uavobjectdefinition/stabilizationstatus.xml
Normal file
28
shared/uavobjectdefinition/stabilizationstatus.xml
Normal file
@ -0,0 +1,28 @@
|
||||
<xml>
|
||||
<object name="StabilizationStatus" singleinstance="true" settings="false" category="Control">
|
||||
<description>Contains status information to control submodules for stabilization.</description>
|
||||
|
||||
|
||||
<field name="OuterLoop" units="" type="enum" options="Direct,Attitude,Rattitude,Weakleveling,Altitude,VerticalVelocity">
|
||||
<elementnames>
|
||||
<elementname>Roll</elementname>
|
||||
<elementname>Pitch</elementname>
|
||||
<elementname>Yaw</elementname>
|
||||
<elementname>Thrust</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="InnerLoop" units="" type="enum" options="Direct,VirtualFlyBar,RelayTuning,AxisLock,Rate,CruiseControl">
|
||||
<elementnames>
|
||||
<elementname>Roll</elementname>
|
||||
<elementname>Pitch</elementname>
|
||||
<elementname>Yaw</elementname>
|
||||
<elementname>Thrust</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="5000"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
Loading…
Reference in New Issue
Block a user