mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-19 04:52:12 +01:00
Merged in TheOtherCliff/librepilot/theothercliff/LP-76_Port_Autotune_from_dRonin (pull request #198)
Theothercliff/lp 76_port_autotune_from_dronin
This commit is contained in:
Alessio Morale
commit
c9188fc5a2
@ -164,6 +164,14 @@ int32_t configuration_check()
|
||||
ADDSEVERITY(!coptercontrol);
|
||||
ADDSEVERITY(navCapableFusion);
|
||||
break;
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
|
||||
ADDSEVERITY(!gps_assisted);
|
||||
// it would be fun to try autotune on a fixed wing
|
||||
// but that should only be attempted by devs at first
|
||||
ADDSEVERITY(multirotor);
|
||||
break;
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
default:
|
||||
// Uncovered modes are automatically an error
|
||||
ADDSEVERITY(false);
|
||||
|
||||
1328
flight/modules/AutoTune/autotune.c
Normal file
1328
flight/modules/AutoTune/autotune.c
Normal file
@ -0,0 +1,1328 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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 AutoTune/autotune.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* dRonin, http://dRonin.org/, Copyright (C) 2015-2016
|
||||
* Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief Automatic PID tuning 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.h>
|
||||
#include <flightstatus.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <manualcontrolsettings.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <gyrostate.h>
|
||||
#include <actuatordesired.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <stabilizationsettings.h>
|
||||
#include <systemidentsettings.h>
|
||||
#include <systemidentstate.h>
|
||||
#include <pios_board_info.h>
|
||||
#include <systemsettings.h>
|
||||
#include <taskinfo.h>
|
||||
#include <stabilization.h>
|
||||
#include <hwsettings.h>
|
||||
#include <stabilizationsettingsbank1.h>
|
||||
#include <stabilizationsettingsbank2.h>
|
||||
#include <stabilizationsettingsbank3.h>
|
||||
#include <accessorydesired.h>
|
||||
|
||||
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
#define powapprox fastpow
|
||||
#define expapprox fastexp
|
||||
#else
|
||||
#define powapprox powf
|
||||
#define expapprox expf
|
||||
#endif /* defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
|
||||
// Private constants
|
||||
#undef STACK_SIZE_BYTES
|
||||
// Pull Request version tested on Nano. 120 bytes of stack left when configured with 1340
|
||||
#define STACK_SIZE_BYTES 1340
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
|
||||
|
||||
#define AF_NUMX 13
|
||||
#define AF_NUMP 43
|
||||
|
||||
#if !defined(AT_QUEUE_NUMELEM)
|
||||
#define AT_QUEUE_NUMELEM 18
|
||||
#endif
|
||||
|
||||
#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */
|
||||
#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */
|
||||
#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */
|
||||
#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */
|
||||
|
||||
#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */
|
||||
#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */
|
||||
#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */
|
||||
#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */
|
||||
#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */
|
||||
|
||||
// CheckSettings() returned error bits
|
||||
#define ROLL_BETA_LOW 1
|
||||
#define PITCH_BETA_LOW 2
|
||||
#define YAW_BETA_LOW 4
|
||||
#define TAU_TOO_LONG 8
|
||||
#define TAU_TOO_SHORT 16
|
||||
|
||||
// smooth-quick modes
|
||||
#define SMOOTH_QUICK_DISABLED 0
|
||||
#define SMOOTH_QUICK_ACCESSORY_BASE 10
|
||||
#define SMOOTH_QUICK_TOGGLE_BASE 20
|
||||
|
||||
|
||||
// Private types
|
||||
enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING };
|
||||
|
||||
struct at_queued_data {
|
||||
float y[3]; /* Gyro measurements */
|
||||
float u[3]; /* Actuator desired */
|
||||
float throttle; /* Throttle desired */
|
||||
|
||||
uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */
|
||||
};
|
||||
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
static bool moduleEnabled;
|
||||
static xQueueHandle atQueue;
|
||||
static volatile uint32_t atPointsSpilled;
|
||||
static uint32_t throttleAccumulator;
|
||||
static uint8_t rollMax, pitchMax;
|
||||
static StabilizationBankManualRateData manualRate;
|
||||
static float gX[AF_NUMX] = { 0 };
|
||||
static float gP[AF_NUMP] = { 0 };
|
||||
static SystemIdentSettingsData systemIdentSettings;
|
||||
static SystemIdentStateData systemIdentState;
|
||||
static int8_t accessoryToUse;
|
||||
static int8_t flightModeSwitchTogglePosition;
|
||||
|
||||
|
||||
// Private functions
|
||||
static void AutoTuneTask(void *parameters);
|
||||
static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in);
|
||||
static void AfInit(float X[AF_NUMX], float P[AF_NUMP]);
|
||||
static uint8_t CheckSettingsRaw();
|
||||
static uint8_t CheckSettings();
|
||||
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise);
|
||||
static void ComputeStabilizationAndSetPids();
|
||||
static void ProportionPidsSmoothToQuick(float min, float val, float max);
|
||||
static void AtNewGyroData(UAVObjEvent *ev);
|
||||
static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle);
|
||||
static void UpdateStabilizationDesired(bool doingIdent);
|
||||
static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
|
||||
static void InitSystemIdent(bool loadDefaults);
|
||||
static void InitSmoothQuick(bool loadToggle);
|
||||
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AutoTuneInitialize(void)
|
||||
{
|
||||
// Create a queue, connect to manual control command and flightstatus
|
||||
#ifdef MODULE_AutoTune_BUILTIN
|
||||
moduleEnabled = true;
|
||||
#else
|
||||
HwSettingsOptionalModulesData optionalModules;
|
||||
HwSettingsOptionalModulesGet(&optionalModules);
|
||||
if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) {
|
||||
// even though the AutoTune module is automatically enabled
|
||||
// (below, when the flight mode switch is configured to use autotune)
|
||||
// there are use cases where the user may even want it enabled without being on the FMS
|
||||
// that allows PIDs to be adjusted in flight
|
||||
moduleEnabled = true;
|
||||
} else {
|
||||
// if the user did not enable the autotune module
|
||||
// do it for them if they have autotune on their flight mode switch
|
||||
FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
|
||||
moduleEnabled = false;
|
||||
FlightModeSettingsInitialize();
|
||||
FlightModeSettingsFlightModePositionGet(fms);
|
||||
for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) {
|
||||
if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
|
||||
moduleEnabled = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif /* ifdef MODULE_AutoTune_BUILTIN */
|
||||
|
||||
if (moduleEnabled) {
|
||||
SystemIdentSettingsInitialize();
|
||||
SystemIdentStateInitialize();
|
||||
atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data));
|
||||
if (!atQueue) {
|
||||
moduleEnabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AutoTuneStart(void)
|
||||
{
|
||||
// Start main task if it is enabled
|
||||
if (moduleEnabled) {
|
||||
// taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
|
||||
// TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
|
||||
// PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
|
||||
GyroStateConnectCallback(AtNewGyroData);
|
||||
xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart);
|
||||
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
enum AUTOTUNE_STATE state = AT_INIT;
|
||||
uint32_t lastUpdateTime = 0; // initialization is only for compiler warning
|
||||
float noise[3] = { 0 };
|
||||
uint32_t lastTime = 0.0f;
|
||||
uint32_t measureTime = 0;
|
||||
uint32_t updateCounter = 0;
|
||||
bool saveSiNeeded = false;
|
||||
bool savePidNeeded = false;
|
||||
|
||||
// get max attitude / max rate
|
||||
// for use in generating Attitude mode commands from this module
|
||||
// note that the values could change when they change flight mode (and the associated bank)
|
||||
StabilizationBankRollMaxGet(&rollMax);
|
||||
StabilizationBankPitchMaxGet(&pitchMax);
|
||||
StabilizationBankManualRateGet(&manualRate);
|
||||
// correctly set accessoryToUse and flightModeSwitchTogglePosition
|
||||
// based on what is in SystemIdent
|
||||
// so that the user can use the PID smooth->quick slider in following flights
|
||||
InitSystemIdent(false);
|
||||
InitSmoothQuick(true);
|
||||
|
||||
while (1) {
|
||||
uint32_t diffTime;
|
||||
bool doingIdent = false;
|
||||
bool canSleep = true;
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
// I have never seen this module misbehave in a way that requires a watchdog, so not bothering making one
|
||||
// PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
|
||||
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
|
||||
if (saveSiNeeded) {
|
||||
saveSiNeeded = false;
|
||||
// Save SystemIdentSettings to permanent settings
|
||||
UAVObjSave(SystemIdentSettingsHandle(), 0);
|
||||
}
|
||||
if (savePidNeeded) {
|
||||
savePidNeeded = false;
|
||||
// Save PIDs to permanent settings
|
||||
switch (systemIdentSettings.DestinationPidBank) {
|
||||
case 1:
|
||||
UAVObjSave(StabilizationSettingsBank1Handle(), 0);
|
||||
break;
|
||||
case 2:
|
||||
UAVObjSave(StabilizationSettingsBank2Handle(), 0);
|
||||
break;
|
||||
case 3:
|
||||
UAVObjSave(StabilizationSettingsBank3Handle(), 0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// if using flight mode switch "quick toggle 3x" to "try smooth -> quick PIDs" is enabled
|
||||
// and user toggled into and back out of AutoTune three times in the last two seconds
|
||||
// and the autotune data gathering is complete
|
||||
// and the autotune data gathered is good
|
||||
// note: CheckFlightModeSwitchForPidRequest(mode) only returns true if current mode is not autotune
|
||||
if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)
|
||||
&& systemIdentSettings.Complete && !CheckSettings()) {
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
|
||||
// if user toggled while armed set PID's to next in sequence
|
||||
// 2,3,4,0,1 (5 positions) or 1,2,0 (3 positions) or 3,4,5,6,0,1,2 (7 positions)
|
||||
// if you assume that smoothest is -100 and quickest is +100
|
||||
// this corresponds to 0,+50,+100,-100,-50 (for 5 position toggle)
|
||||
++flightModeSwitchTogglePosition;
|
||||
if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) {
|
||||
flightModeSwitchTogglePosition = 0;
|
||||
}
|
||||
} else {
|
||||
// if they did it disarmed, then set PID's back to AutoTune default
|
||||
flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
|
||||
}
|
||||
ProportionPidsSmoothToQuick(0.0f,
|
||||
(float)flightModeSwitchTogglePosition,
|
||||
(float)(systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE));
|
||||
savePidNeeded = true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////
|
||||
// if configured to use a slider for smooth-quick and the autotune module is running
|
||||
// (note that the module can be automatically or manually enabled)
|
||||
// then the smooth-quick slider is always active (when not actually in autotune mode)
|
||||
//
|
||||
// when the slider is active it will immediately change the PIDs
|
||||
// and it will schedule the PIDs to be written to permanent storage
|
||||
//
|
||||
// if the FC is disarmed, the perm write will happen on next loop
|
||||
// but if the FC is armed, the perm write will only occur when the FC goes disarmed
|
||||
//////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// we don't want it saving to permanent storage many times
|
||||
// while the user is moving the knob once, so wait till the knob stops moving
|
||||
static uint8_t savePidDelay;
|
||||
// any time we are not in AutoTune mode:
|
||||
// - the user may be using the accessory0-3 knob/slider to request PID changes
|
||||
// - the state machine needs to be reset
|
||||
// - the local version of Attitude mode gets skipped
|
||||
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
|
||||
static bool savePidActive = false;
|
||||
// if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range
|
||||
// and FC is not currently running autotune
|
||||
// and accessory0-3 changed by at least 1/160 of full range (2)
|
||||
// (don't bother checking to see if the requested accessory# is configured properly
|
||||
// if it isn't, the value will be 0 which is the center of [-1,1] anyway)
|
||||
if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) {
|
||||
static AccessoryDesiredData accessoryValueOld = { 0.0f };
|
||||
AccessoryDesiredData accessoryValue;
|
||||
AccessoryDesiredInstGet(accessoryToUse, &accessoryValue);
|
||||
// if the accessory changed more than 1/80
|
||||
// (this test is intended to remove one unit jitter)
|
||||
// some old PPM receivers use a low resolution chip which only allows about 180 steps
|
||||
// what we are doing here does not need any higher precision than that
|
||||
if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) {
|
||||
accessoryValueOld = accessoryValue;
|
||||
// this copies the PIDs to memory and makes them active
|
||||
// but does not write them to permanent storage
|
||||
ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f);
|
||||
// don't save PID to perm storage the first time
|
||||
// that usually means at power up
|
||||
//
|
||||
// that keeps it from writing the same value at each boot
|
||||
// but means that it won't be permanent if they move the slider before FC power on
|
||||
// (note that the PIDs will be changed, just not saved permanently)
|
||||
if (savePidActive) {
|
||||
// this schedules the first possible write of the PIDs to occur a fraction of a second or so from now
|
||||
// and moves the scheduled time if it is already scheduled
|
||||
savePidDelay = SMOOTH_QUICK_FLUSH_TICKS;
|
||||
} else {
|
||||
savePidActive = true;
|
||||
}
|
||||
} else if (savePidDelay && --savePidDelay == 0) {
|
||||
// this flags that the PIDs can be written to permanent storage right now
|
||||
// but they will only be written when the FC is disarmed
|
||||
// so this means immediate (after NOT_AT_MODE_DELAY_MS) or wait till FC is disarmed
|
||||
savePidNeeded = true;
|
||||
}
|
||||
} else {
|
||||
savePidDelay = 0;
|
||||
}
|
||||
state = AT_INIT;
|
||||
vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS);
|
||||
continue;
|
||||
} else {
|
||||
savePidDelay = 0;
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
case AT_INIT:
|
||||
// beware that control comes here every time the user toggles the flight mode switch into AutoTune
|
||||
// and it isn't appropriate to reset the main state here
|
||||
// init must wait until after a delay has passed:
|
||||
// - to make sure they intended to stay in this mode
|
||||
// - to wait for the stab bank to get populated with the new bank info
|
||||
// This is a race. It is possible that flightStatus.FlightMode has been changed,
|
||||
// but the stab bank hasn't been changed yet.
|
||||
state = AT_INIT_DELAY;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
break;
|
||||
|
||||
case AT_INIT_DELAY:
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
// after a small delay, get the stab bank values and SystemIdentSettings in case they changed
|
||||
// this is a very small delay (100ms), so "quick 3x fms toggle" gets in here
|
||||
if (diffTime > INIT_TIME_DELAY_MS) {
|
||||
// do these here so the user has at most a 1/10th second
|
||||
// with controls that use the previous bank's rates
|
||||
StabilizationBankRollMaxGet(&rollMax);
|
||||
StabilizationBankPitchMaxGet(&pitchMax);
|
||||
StabilizationBankManualRateGet(&manualRate);
|
||||
// load SystemIdentSettings so that they can change it
|
||||
// and do smooth-quick on changed values
|
||||
InitSystemIdent(false);
|
||||
InitSmoothQuick(false);
|
||||
// wait for FC to arm in case they are doing this without a flight mode switch
|
||||
// that causes the 2+ second delay that follows to happen after arming
|
||||
// which gives them a chance to take off before the shakes start
|
||||
// the FC must be armed and if we check here it also allows switchless setup to use autotune
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
|
||||
state = AT_INIT_DELAY2;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_INIT_DELAY2:
|
||||
// delay for 2 seconds before actually starting the SystemIdent flight mode and AutoTune.
|
||||
// that allows the user to get his fingers on the sticks
|
||||
// and avoids starting the AutoTune if the user is toggling the flight mode switch
|
||||
// to select other PIDs on the "simulated Smooth Quick slider".
|
||||
// or simply "passing through" this flight mode to get to another flight mode
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
// after 2 seconds start systemident flight mode
|
||||
if (diffTime > SYSTEMIDENT_TIME_DELAY_MS) {
|
||||
doingIdent = true;
|
||||
// after an additional .5 seconds start capturing data
|
||||
if (diffTime > INIT_TIME_DELAY2_MS) {
|
||||
// Reset save status
|
||||
// save SI data even if partial or bad, aids in diagnostics
|
||||
saveSiNeeded = true;
|
||||
// don't save PIDs until data gathering is complete
|
||||
// and the complete data has been sanity checked
|
||||
savePidNeeded = false;
|
||||
InitSystemIdent(true);
|
||||
InitSmoothQuick(true);
|
||||
AfInit(gX, gP);
|
||||
UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f);
|
||||
measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
|
||||
state = AT_START;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_START:
|
||||
lastTime = PIOS_DELAY_GetRaw();
|
||||
doingIdent = true;
|
||||
/* Drain the queue of all current data */
|
||||
xQueueReset(atQueue);
|
||||
/* And reset the point spill counter */
|
||||
updateCounter = 0;
|
||||
atPointsSpilled = 0;
|
||||
throttleAccumulator = 0;
|
||||
state = AT_RUN;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
break;
|
||||
|
||||
case AT_RUN:
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
doingIdent = true;
|
||||
canSleep = false;
|
||||
// 4 gyro samples per cycle
|
||||
// 2ms cycle time
|
||||
// that is 500 gyro samples per second if it sleeps each time
|
||||
// actually less than 500 because it cycle time is processing time + 2ms
|
||||
for (int i = 0; i < MAX_PTS_PER_CYCLE; i++) {
|
||||
struct at_queued_data pt;
|
||||
/* Grab an autotune point */
|
||||
if (xQueueReceive(atQueue, &pt, 0) != pdTRUE) {
|
||||
/* We've drained the buffer fully */
|
||||
canSleep = true;
|
||||
break;
|
||||
}
|
||||
/* calculate time between successive points */
|
||||
float dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.raw_time) * 1.0e-6f;
|
||||
/* This is for the first point, but
|
||||
* also if we have extended drops */
|
||||
if (dT_s > 0.010f) {
|
||||
dT_s = 0.010f;
|
||||
}
|
||||
lastTime = pt.raw_time;
|
||||
AfPredict(gX, gP, pt.u, pt.y, dT_s, pt.throttle);
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz
|
||||
noise[j] = NOISE_ALPHA * noise[j] + (1 - NOISE_ALPHA) * (pt.y[j] - gX[j]) * (pt.y[j] - gX[j]);
|
||||
}
|
||||
// This will work up to 8kHz with an 89% throttle position before overflow
|
||||
throttleAccumulator += 10000 * pt.throttle;
|
||||
// Update uavo every 256 cycles to avoid
|
||||
// telemetry spam
|
||||
if (((updateCounter++) & 0xff) == 0) {
|
||||
float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
|
||||
UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle);
|
||||
}
|
||||
}
|
||||
if (diffTime > measureTime) { // Move on to next state
|
||||
// permanent flag that AT is complete and PIDs can be calculated
|
||||
state = AT_FINISHED;
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_FINISHED:
|
||||
// data is automatically considered bad if FC was disarmed at the time AT completed
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
|
||||
// always calculate and save PIDs if disabling sanity checks
|
||||
if (!CheckSettings()) {
|
||||
ComputeStabilizationAndSetPids();
|
||||
savePidNeeded = true;
|
||||
// mark these results as good in the permanent settings so they can be used next flight too
|
||||
systemIdentSettings.Complete = true;
|
||||
// mark these results as good in the log settings so they can be viewed in playback
|
||||
systemIdentState.Complete = true;
|
||||
}
|
||||
// always raise an alarm if sanity checks failed
|
||||
// even if disabling sanity checks
|
||||
// that way user can still see that they failed
|
||||
uint8_t failureBits = CheckSettingsRaw();
|
||||
if (failureBits) {
|
||||
// raise a warning that includes failureBits to indicate what failed
|
||||
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING,
|
||||
SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits);
|
||||
}
|
||||
}
|
||||
float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
|
||||
UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hoverThrottle);
|
||||
SystemIdentSettingsSet(&systemIdentSettings);
|
||||
state = AT_WAITING;
|
||||
break;
|
||||
|
||||
case AT_WAITING:
|
||||
default:
|
||||
// after tuning, wait here till user switches to another flight mode
|
||||
// or disarms
|
||||
break;
|
||||
}
|
||||
|
||||
// fly in Attitude mode or in SystemIdent mode
|
||||
UpdateStabilizationDesired(doingIdent);
|
||||
|
||||
if (canSleep) {
|
||||
vTaskDelay(YIELD_MS / portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// gyro sensor callback
|
||||
// get gyro data and actuatordesired into a packet
|
||||
// and put it in the queue for later processing
|
||||
static void AtNewGyroData(UAVObjEvent *ev)
|
||||
{
|
||||
static struct at_queued_data q_item;
|
||||
static bool last_sample_unpushed = false;
|
||||
GyroStateData gyro;
|
||||
ActuatorDesiredData actuators;
|
||||
|
||||
if (!ev || !ev->obj || ev->instId != 0 || ev->event != EV_UPDATED) {
|
||||
return;
|
||||
}
|
||||
|
||||
// object will at times change asynchronously so must copy data here, with locking
|
||||
// and do it as soon as possible
|
||||
GyroStateGet(&gyro);
|
||||
ActuatorDesiredGet(&actuators);
|
||||
|
||||
if (last_sample_unpushed) {
|
||||
/* Last time we were unable to queue up the gyro data.
|
||||
* Try again, last chance! */
|
||||
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
|
||||
atPointsSpilled++;
|
||||
}
|
||||
}
|
||||
|
||||
q_item.raw_time = PIOS_DELAY_GetRaw();
|
||||
q_item.y[0] = gyro.x;
|
||||
q_item.y[1] = gyro.y;
|
||||
q_item.y[2] = gyro.z;
|
||||
q_item.u[0] = actuators.Roll;
|
||||
q_item.u[1] = actuators.Pitch;
|
||||
q_item.u[2] = actuators.Yaw;
|
||||
q_item.throttle = actuators.Thrust;
|
||||
|
||||
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
|
||||
last_sample_unpushed = true;
|
||||
} else {
|
||||
last_sample_unpushed = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// check for the user quickly toggling the flight mode switch
|
||||
// into and out of AutoTune, 3 times
|
||||
// that is a signal that the user wants to try the next PID settings
|
||||
// on the scale from smooth to quick
|
||||
// when it exceeds the quickest setting, it starts back at the smoothest setting
|
||||
static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode)
|
||||
{
|
||||
static uint32_t lastUpdateTime;
|
||||
static uint8_t flightModePrev;
|
||||
static uint8_t counter;
|
||||
uint32_t updateTime;
|
||||
|
||||
// only count transitions into and out of autotune
|
||||
if ((flightModePrev == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ^ (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE)) {
|
||||
flightModePrev = flightMode;
|
||||
updateTime = xTaskGetTickCount();
|
||||
// if it has been over 2 seconds, reset the counter
|
||||
if (updateTime - lastUpdateTime > 2000) {
|
||||
counter = 0;
|
||||
}
|
||||
// if the counter is reset, start a new time period
|
||||
if (counter == 0) {
|
||||
lastUpdateTime = updateTime;
|
||||
}
|
||||
// if flight mode has toggled into autotune 3 times but is currently not autotune
|
||||
if (++counter >= 5 && flightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
|
||||
counter = 0;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// read SystemIdent uavos, update the local structures
|
||||
// and set some flags based on the values
|
||||
// it is used two ways:
|
||||
// - on startup it reads settings so the user can reuse an old tune with smooth-quick
|
||||
// - at tune time, it inits the state in preparation for tuning
|
||||
static void InitSystemIdent(bool loadDefaults)
|
||||
{
|
||||
SystemIdentSettingsGet(&systemIdentSettings);
|
||||
|
||||
if (loadDefaults) {
|
||||
// get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau)
|
||||
// so that if they are changed there (mainly for future code changes), they will be changed here too
|
||||
SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0);
|
||||
SystemIdentStateGet(&systemIdentState);
|
||||
// Tau, Beta, and the Complete flag get default values
|
||||
// in preparation for running AutoTune
|
||||
systemIdentSettings.Tau = systemIdentState.Tau;
|
||||
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
|
||||
systemIdentSettings.Complete = systemIdentState.Complete;
|
||||
} else {
|
||||
// Tau, Beta, and the Complete flag get stored values
|
||||
// so the user can fly another battery to select and test PIDs with the slider/knob
|
||||
systemIdentState.Tau = systemIdentSettings.Tau;
|
||||
memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData));
|
||||
systemIdentState.Complete = systemIdentSettings.Complete;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void InitSmoothQuick(bool loadToggle)
|
||||
{
|
||||
uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource;
|
||||
|
||||
switch (SmoothQuickSource) {
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
|
||||
// disable PID changing with flight mode switch
|
||||
// ignore loadToggle if user is also switching to use knob as source
|
||||
flightModeSwitchTogglePosition = -1;
|
||||
accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
|
||||
systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
|
||||
break;
|
||||
|
||||
case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points
|
||||
case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points
|
||||
case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points
|
||||
// disable PID changing with accessory0-3
|
||||
accessoryToUse = -1;
|
||||
// don't allow init of current toggle position in the middle of 3x fms toggle
|
||||
if (loadToggle) {
|
||||
// first test PID is in the middle of the smooth -> quick range
|
||||
flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
|
||||
}
|
||||
systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
|
||||
break;
|
||||
|
||||
case SMOOTH_QUICK_DISABLED:
|
||||
default:
|
||||
// disable PID changing with flight mode switch
|
||||
// ignore loadToggle since user is disabling toggle
|
||||
flightModeSwitchTogglePosition = -1;
|
||||
// disable PID changing with accessory0-3
|
||||
accessoryToUse = -1;
|
||||
systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// update the gain and delay with current calculated value
|
||||
// these are stored in the settings for use with next battery
|
||||
// and also in the state for logging purposes
|
||||
static void UpdateSystemIdentState(const float *X, const float *noise,
|
||||
float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle)
|
||||
{
|
||||
systemIdentState.Beta.Roll = X[6];
|
||||
systemIdentState.Beta.Pitch = X[7];
|
||||
systemIdentState.Beta.Yaw = X[8];
|
||||
systemIdentState.Bias.Roll = X[10];
|
||||
systemIdentState.Bias.Pitch = X[11];
|
||||
systemIdentState.Bias.Yaw = X[12];
|
||||
systemIdentState.Tau = X[9];
|
||||
// 'settings' beta and tau have same value as 'state' versions
|
||||
// the state version produces a GCS log
|
||||
// the settings version is remembered after power off/on
|
||||
systemIdentSettings.Tau = systemIdentState.Tau;
|
||||
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
|
||||
if (noise) {
|
||||
systemIdentState.Noise.Roll = noise[0];
|
||||
systemIdentState.Noise.Pitch = noise[1];
|
||||
systemIdentState.Noise.Yaw = noise[2];
|
||||
}
|
||||
systemIdentState.Period = dT_s * 1000.0f;
|
||||
systemIdentState.NumAfPredicts = predicts;
|
||||
systemIdentState.NumSpilledPts = spills;
|
||||
systemIdentState.HoverThrottle = hover_throttle;
|
||||
|
||||
SystemIdentStateSet(&systemIdentState);
|
||||
}
|
||||
|
||||
|
||||
// when running AutoTune mode, this bypasses manualcontrol.c / stabilizedhandler.c
|
||||
// to control exactly when the multicopter should be in Attitude mode vs. SystemIdent mode
|
||||
static void UpdateStabilizationDesired(bool doingIdent)
|
||||
{
|
||||
StabilizationDesiredData stabDesired;
|
||||
ManualControlCommandData manualControlCommand;
|
||||
|
||||
ManualControlCommandGet(&manualControlCommand);
|
||||
|
||||
stabDesired.Roll = manualControlCommand.Roll * rollMax;
|
||||
stabDesired.Pitch = manualControlCommand.Pitch * pitchMax;
|
||||
stabDesired.Yaw = manualControlCommand.Yaw * manualRate.Yaw;
|
||||
stabDesired.Thrust = manualControlCommand.Thrust;
|
||||
|
||||
if (doingIdent) {
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
|
||||
} else {
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
}
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
}
|
||||
|
||||
|
||||
// check the completed autotune state (mainly gain and delay)
|
||||
// to see if it is reasonable
|
||||
// return a bit mask of errors detected
|
||||
static uint8_t CheckSettingsRaw()
|
||||
{
|
||||
uint8_t retVal = 0;
|
||||
|
||||
// Check the axis gains
|
||||
// Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values.
|
||||
if (systemIdentState.Beta.Roll < 6) {
|
||||
retVal |= ROLL_BETA_LOW;
|
||||
}
|
||||
if (systemIdentState.Beta.Pitch < 6) {
|
||||
retVal |= PITCH_BETA_LOW;
|
||||
}
|
||||
// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default
|
||||
#if 0
|
||||
// if (systemIdentState.Beta.Yaw < 4) {
|
||||
if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) {
|
||||
retVal |= YAW_BETA_LOW;
|
||||
}
|
||||
#endif
|
||||
// Check the response speed
|
||||
// Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values.
|
||||
if (expf(systemIdentState.Tau) > 0.1f) {
|
||||
retVal |= TAU_TOO_LONG;
|
||||
}
|
||||
// Extreme values: Your estimated response speed (tau) is faster than normal. This will result in large PID values.
|
||||
else if (expf(systemIdentState.Tau) < 0.008f) {
|
||||
retVal |= TAU_TOO_SHORT;
|
||||
}
|
||||
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
// check the completed autotune state (mainly gain and delay)
|
||||
// to see if it is reasonable
|
||||
// override bad yaw values if configured that way
|
||||
// return a bit mask of errors detected
|
||||
static uint8_t CheckSettings()
|
||||
{
|
||||
uint8_t retVal = CheckSettingsRaw();
|
||||
|
||||
if (systemIdentSettings.DisableSanityChecks) {
|
||||
retVal = 0;
|
||||
}
|
||||
// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default
|
||||
#if 0
|
||||
// if not calculating yaw, or if calculating yaw but ignoring errors
|
||||
else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) {
|
||||
// clear the yaw error bit
|
||||
retVal &= ~YAW_BETA_LOW;
|
||||
}
|
||||
#endif
|
||||
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
// given Tau(delay) and Beta(gain) from the tune (and user selection of smooth to quick) calculate the PIDs
|
||||
// this code came from dRonin GCS and uses double precision math
|
||||
// most of the doubles could be replaced with floats
|
||||
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate)
|
||||
{
|
||||
_Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)");
|
||||
StabilizationBankData stabSettingsBank;
|
||||
|
||||
switch (systemIdentSettings.DestinationPidBank) {
|
||||
case 1:
|
||||
StabilizationSettingsBank1Get((void *)&stabSettingsBank);
|
||||
break;
|
||||
case 2:
|
||||
StabilizationSettingsBank2Get((void *)&stabSettingsBank);
|
||||
break;
|
||||
case 3:
|
||||
StabilizationSettingsBank3Get((void *)&stabSettingsBank);
|
||||
break;
|
||||
}
|
||||
|
||||
// These three parameters define the desired response properties
|
||||
// - rate scale in the fraction of the natural speed of the system
|
||||
// to strive for.
|
||||
// - damp is the amount of damping in the system. higher values
|
||||
// make oscillations less likely
|
||||
// - ghf is the amount of high frequency gain and limits the influence
|
||||
// of noise
|
||||
const double ghf = (double)noiseRate / 1000.0d;
|
||||
const double damp = (double)dampRate / 100.0d;
|
||||
|
||||
double tau = exp(systemIdentState.Tau);
|
||||
double exp_beta_roll_times_ghf = exp(systemIdentState.Beta.Roll) * ghf;
|
||||
double exp_beta_pitch_times_ghf = exp(systemIdentState.Beta.Pitch) * ghf;
|
||||
|
||||
double wn = 1.0d / tau;
|
||||
double tau_d = 0.0d;
|
||||
for (int i = 0; i < 30; i++) {
|
||||
double tau_d_roll = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_roll_times_ghf);
|
||||
double tau_d_pitch = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_pitch_times_ghf);
|
||||
// Select the slowest filter property
|
||||
tau_d = (tau_d_roll > tau_d_pitch) ? tau_d_roll : tau_d_pitch;
|
||||
wn = (tau + tau_d) / (tau * tau_d) / (2.0d * damp + 2.0d);
|
||||
}
|
||||
|
||||
// Set the real pole position. The first pole is quite slow, which
|
||||
// prevents the integral being too snappy and driving too much
|
||||
// overshoot.
|
||||
const double a = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn) / 20.0d;
|
||||
const double b = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn - a);
|
||||
|
||||
// Calculate the gain for the outer loop by approximating the
|
||||
// inner loop as a single order lpf. Set the outer loop to be
|
||||
// critically damped;
|
||||
const double zeta_o = 1.3d;
|
||||
const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d / wn);
|
||||
const double ki_o = 0.75d * kp_o / (2.0d * M_PI_D * tau * 10.0d);
|
||||
|
||||
float kpMax = 0.0f;
|
||||
double betaMinLn = 1000.0d;
|
||||
StabilizationBankRollRatePIDData *rollPitchPid = NULL; // satisfy compiler warning only
|
||||
|
||||
for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) {
|
||||
double betaLn = SystemIdentStateBetaToArray(systemIdentState.Beta)[i];
|
||||
double beta = exp(betaLn);
|
||||
double ki;
|
||||
double kp;
|
||||
double kd;
|
||||
|
||||
switch (i) {
|
||||
case 0: // Roll
|
||||
case 1: // Pitch
|
||||
ki = a * b * wn * wn * tau * tau_d / beta;
|
||||
kp = tau * tau_d * ((a + b) * wn * wn + 2.0d * a * b * damp * wn) / beta - ki * tau_d;
|
||||
kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0d * damp * wn) - 1.0d) / beta - kp * tau_d;
|
||||
if (betaMinLn > betaLn) {
|
||||
betaMinLn = betaLn;
|
||||
// RollRatePID PitchRatePID YawRatePID
|
||||
// form an array of structures
|
||||
// point to one
|
||||
rollPitchPid = &(&stabSettingsBank.RollRatePID)[i];
|
||||
}
|
||||
break;
|
||||
case 2: // Yaw
|
||||
// yaw uses a mixture of yaw and the slowest axis (pitch) for it's beta and thus PID calculation
|
||||
// calculate the ratio to use when converting from the slowest axis (pitch) to the yaw axis
|
||||
// as (e^(betaMinLn-betaYawLn))^0.6
|
||||
// which is (e^betaMinLn / e^betaYawLn)^0.6
|
||||
// which is (betaMin / betaYaw)^0.6
|
||||
// which is betaMin^0.6 / betaYaw^0.6
|
||||
// now given that kp for each axis can be written as kpaxis = xp / betaaxis
|
||||
// for xp that is constant across all axes
|
||||
// then kpmin (probably kppitch) was xp / betamin (probably betapitch)
|
||||
// which we multiply by betaMin^0.6 / betaYaw^0.6 to get the new Yaw kp
|
||||
// so the new kpyaw is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6)
|
||||
// which is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6)
|
||||
// which is (xp * betaMin^0.6) / (betaMin * betaYaw^0.6)
|
||||
// which is xp / (betaMin * betaYaw^0.6 / betaMin^0.6)
|
||||
// which is xp / (betaMin^0.4 * betaYaw^0.6)
|
||||
// hence the new effective betaYaw for Yaw P is (betaMin^0.4)*(betaYaw^0.6)
|
||||
beta = exp(0.6d * (betaMinLn - (double)systemIdentState.Beta.Yaw));
|
||||
kp = (double)rollPitchPid->Kp * beta;
|
||||
ki = 0.8d * (double)rollPitchPid->Ki * beta;
|
||||
kd = 0.8d * (double)rollPitchPid->Kd * beta;
|
||||
break;
|
||||
}
|
||||
|
||||
if (i < 2) {
|
||||
if (kpMax < (float)kp) {
|
||||
kpMax = (float)kp;
|
||||
}
|
||||
} else {
|
||||
// use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value
|
||||
// use largest roll/pitch kp because it is the axis most slowed by rotational inertia
|
||||
// and yaw is also slowed maximally by rotational inertia
|
||||
// note that kp, ki, kd are all proportional in beta
|
||||
// so reducing them all proportionally is the same as changing beta
|
||||
float min = 0.0f;
|
||||
float max = 0.0f;
|
||||
switch (systemIdentSettings.CalculateYaw) {
|
||||
case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUELIMITTORATIO:
|
||||
max = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMax;
|
||||
min = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMin;
|
||||
break;
|
||||
case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUEIGNORELIMIT:
|
||||
default:
|
||||
max = 1000.0f;
|
||||
min = 0.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
float ratio = 1.0f;
|
||||
if (min > 0.0f && (float)kp < min) {
|
||||
ratio = (float)kp / min;
|
||||
} else if (max > 0.0f && (float)kp > max) {
|
||||
ratio = (float)kp / max;
|
||||
}
|
||||
kp /= (double)ratio;
|
||||
ki /= (double)ratio;
|
||||
kd /= (double)ratio;
|
||||
}
|
||||
|
||||
switch (i) {
|
||||
case 0: // Roll
|
||||
stabSettingsBank.RollRatePID.Kp = kp;
|
||||
stabSettingsBank.RollRatePID.Ki = ki;
|
||||
stabSettingsBank.RollRatePID.Kd = kd;
|
||||
stabSettingsBank.RollPI.Kp = kp_o;
|
||||
stabSettingsBank.RollPI.Ki = ki_o;
|
||||
break;
|
||||
case 1: // Pitch
|
||||
stabSettingsBank.PitchRatePID.Kp = kp;
|
||||
stabSettingsBank.PitchRatePID.Ki = ki;
|
||||
stabSettingsBank.PitchRatePID.Kd = kd;
|
||||
stabSettingsBank.PitchPI.Kp = kp_o;
|
||||
stabSettingsBank.PitchPI.Ki = ki_o;
|
||||
break;
|
||||
case 2: // Yaw
|
||||
stabSettingsBank.YawRatePID.Kp = kp;
|
||||
stabSettingsBank.YawRatePID.Ki = ki;
|
||||
stabSettingsBank.YawRatePID.Kd = kd;
|
||||
#if 0
|
||||
// if we ever choose to use these
|
||||
// (e.g. mag yaw attitude)
|
||||
// here they are
|
||||
stabSettingsBank.YawPI.Kp = kp_o;
|
||||
stabSettingsBank.YawPI.Ki = ki_o;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Librepilot might do something more with this some time
|
||||
// stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d);
|
||||
// SystemIdentSettingsDerivativeCutoffSet(&systemIdentSettings.DerivativeCutoff);
|
||||
|
||||
// Save PIDs to UAVO RAM (not permanently yet)
|
||||
switch (systemIdentSettings.DestinationPidBank) {
|
||||
case 1:
|
||||
StabilizationSettingsBank1Set((void *)&stabSettingsBank);
|
||||
break;
|
||||
case 2:
|
||||
StabilizationSettingsBank2Set((void *)&stabSettingsBank);
|
||||
break;
|
||||
case 3:
|
||||
StabilizationSettingsBank3Set((void *)&stabSettingsBank);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// calculate PIDs using default smooth-quick settings
|
||||
static void ComputeStabilizationAndSetPids()
|
||||
{
|
||||
ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentSettings.DampRate, systemIdentSettings.NoiseRate);
|
||||
}
|
||||
|
||||
|
||||
// scale the damp and the noise to generate PIDs according to how a slider or other user specified ratio is set
|
||||
//
|
||||
// when val is half way between min and max, it generates the default PIDs
|
||||
// when val is min, it generates the smoothest configured PIDs
|
||||
// when val is max, it generates the quickest configured PIDs
|
||||
//
|
||||
// when val is between min and (min+max)/2, it scales val over the range [min, (min+max)/2] to generate PIDs between smoothest and default
|
||||
// when val is between (min+max)/2 and max, it scales val over the range [(min+max)/2, max] to generate PIDs between default and quickest
|
||||
//
|
||||
// this is done piecewise because we are not guaranteed that default-min == max-default
|
||||
// but we are given that [smoothDamp,smoothNoise] [defaultDamp,defaultNoise] [quickDamp,quickNoise] are all good parameterizations
|
||||
// this code guarantees that we will get those exact parameterizations at (val =) min, (max+min)/2, and max
|
||||
static void ProportionPidsSmoothToQuick(float min, float val, float max)
|
||||
{
|
||||
float ratio, damp, noise;
|
||||
|
||||
// translate from range [min, max] to range [0, max-min]
|
||||
// that takes care of min < 0 case too
|
||||
val -= min;
|
||||
max -= min;
|
||||
ratio = val / max;
|
||||
|
||||
if (ratio <= 0.5f) {
|
||||
// scale ratio in [0,0.5] to produce PIDs in [smoothest,default]
|
||||
ratio *= 2.0f;
|
||||
damp = (systemIdentSettings.DampMax * (1.0f - ratio)) + (systemIdentSettings.DampRate * ratio);
|
||||
noise = (systemIdentSettings.NoiseMin * (1.0f - ratio)) + (systemIdentSettings.NoiseRate * ratio);
|
||||
} else {
|
||||
// scale ratio in [0.5,1.0] to produce PIDs in [default,quickest]
|
||||
ratio = (ratio - 0.5f) * 2.0f;
|
||||
damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio);
|
||||
noise = (systemIdentSettings.NoiseRate * (1.0f - ratio)) + (systemIdentSettings.NoiseMax * ratio);
|
||||
}
|
||||
|
||||
ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Prediction step for EKF on control inputs to quad that
|
||||
* learns the system properties
|
||||
* @param X the current state estimate which is updated in place
|
||||
* @param P the current covariance matrix, updated in place
|
||||
* @param[in] the current control inputs (roll, pitch, yaw)
|
||||
* @param[in] the gyro measurements
|
||||
*/
|
||||
__attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in)
|
||||
{
|
||||
const float Ts = dT_s;
|
||||
const float Tsq = Ts * Ts;
|
||||
const float Tsq3 = Tsq * Ts;
|
||||
const float Tsq4 = Tsq * Tsq;
|
||||
|
||||
// for convenience and clarity code below uses the named versions of
|
||||
// the state variables
|
||||
float w1 = X[0]; // roll rate estimate
|
||||
float w2 = X[1]; // pitch rate estimate
|
||||
float w3 = X[2]; // yaw rate estimate
|
||||
float u1 = X[3]; // scaled roll torque
|
||||
float u2 = X[4]; // scaled pitch torque
|
||||
float u3 = X[5]; // scaled yaw torque
|
||||
const float e_b1 = expapprox(X[6]); // roll torque scale
|
||||
const float b1 = X[6];
|
||||
const float e_b2 = expapprox(X[7]); // pitch torque scale
|
||||
const float b2 = X[7];
|
||||
const float e_b3 = expapprox(X[8]); // yaw torque scale
|
||||
const float b3 = X[8];
|
||||
const float e_tau = expapprox(X[9]); // time response of the motors
|
||||
const float tau = X[9];
|
||||
const float bias1 = X[10]; // bias in the roll torque
|
||||
const float bias2 = X[11]; // bias in the pitch torque
|
||||
const float bias3 = X[12]; // bias in the yaw torque
|
||||
|
||||
// inputs to the system (roll, pitch, yaw)
|
||||
const float u1_in = 4 * t_in * u_in[0];
|
||||
const float u2_in = 4 * t_in * u_in[1];
|
||||
const float u3_in = 4 * t_in * u_in[2];
|
||||
|
||||
// measurements from gyro
|
||||
const float gyro_x = gyro[0];
|
||||
const float gyro_y = gyro[1];
|
||||
const float gyro_z = gyro[2];
|
||||
|
||||
// update named variables because we want to use predicted
|
||||
// values below
|
||||
w1 = X[0] = w1 - Ts * bias1 * e_b1 + Ts * u1 * e_b1;
|
||||
w2 = X[1] = w2 - Ts * bias2 * e_b2 + Ts * u2 * e_b2;
|
||||
w3 = X[2] = w3 - Ts * bias3 * e_b3 + Ts * u3 * e_b3;
|
||||
u1 = X[3] = (Ts * u1_in) / (Ts + e_tau) + (u1 * e_tau) / (Ts + e_tau);
|
||||
u2 = X[4] = (Ts * u2_in) / (Ts + e_tau) + (u2 * e_tau) / (Ts + e_tau);
|
||||
u3 = X[5] = (Ts * u3_in) / (Ts + e_tau) + (u3 * e_tau) / (Ts + e_tau);
|
||||
// X[6] to X[12] unchanged
|
||||
|
||||
/**** filter parameters ****/
|
||||
const float q_w = 1e-3f;
|
||||
const float q_ud = 1e-3f;
|
||||
const float q_B = 1e-6f;
|
||||
const float q_tau = 1e-6f;
|
||||
const float q_bias = 1e-19f;
|
||||
const float s_a = 150.0f; // expected gyro measurment noise
|
||||
|
||||
const float Q[AF_NUMX] = { q_w, q_w, q_w, q_ud, q_ud, q_ud, q_B, q_B, q_B, q_tau, q_bias, q_bias, q_bias };
|
||||
|
||||
float D[AF_NUMP];
|
||||
for (uint32_t i = 0; i < AF_NUMP; i++) {
|
||||
D[i] = P[i];
|
||||
}
|
||||
|
||||
const float e_tau2 = e_tau * e_tau;
|
||||
const float e_tau3 = e_tau * e_tau2;
|
||||
const float e_tau4 = e_tau2 * e_tau2;
|
||||
const float Ts_e_tau2 = (Ts + e_tau) * (Ts + e_tau);
|
||||
const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2;
|
||||
|
||||
// covariance propagation - D is stored copy of covariance
|
||||
P[0] = D[0] + Q[0] + 2 * Ts * e_b1 * (D[3] - D[28] - D[9] * bias1 + D[9] * u1)
|
||||
+ Tsq * (e_b1 * e_b1) * (D[4] - 2 * D[29] + D[32] - 2 * D[10] * bias1 + 2 * D[30] * bias1 + 2 * D[10] * u1 - 2 * D[30] * u1
|
||||
+ D[11] * (bias1 * bias1) + D[11] * (u1 * u1) - 2 * D[11] * bias1 * u1);
|
||||
P[1] = D[1] + Q[1] + 2 * Ts * e_b2 * (D[5] - D[33] - D[12] * bias2 + D[12] * u2)
|
||||
+ Tsq * (e_b2 * e_b2) * (D[6] - 2 * D[34] + D[37] - 2 * D[13] * bias2 + 2 * D[35] * bias2 + 2 * D[13] * u2 - 2 * D[35] * u2
|
||||
+ D[14] * (bias2 * bias2) + D[14] * (u2 * u2) - 2 * D[14] * bias2 * u2);
|
||||
P[2] = D[2] + Q[2] + 2 * Ts * e_b3 * (D[7] - D[38] - D[15] * bias3 + D[15] * u3)
|
||||
+ Tsq * (e_b3 * e_b3) * (D[8] - 2 * D[39] + D[42] - 2 * D[16] * bias3 + 2 * D[40] * bias3 + 2 * D[16] * u3 - 2 * D[40] * u3
|
||||
+ D[17] * (bias3 * bias3) + D[17] * (u3 * u3) - 2 * D[17] * bias3 * u3);
|
||||
P[3] = (D[3] * (e_tau2 + Ts * e_tau) + Ts * e_b1 * e_tau2 * (D[4] - D[29]) + Tsq * e_b1 * e_tau * (D[4] - D[29])
|
||||
+ D[18] * Ts * e_tau * (u1 - u1_in) + D[10] * e_b1 * (u1 * (Ts * e_tau2 + Tsq * e_tau) - bias1 * (Ts * e_tau2 + Tsq * e_tau))
|
||||
+ D[21] * Tsq * e_b1 * e_tau * (u1 - u1_in) + D[31] * Tsq * e_b1 * e_tau * (u1_in - u1)
|
||||
+ D[24] * Tsq * e_b1 * e_tau * (u1 * (u1 - bias1) + u1_in * (bias1 - u1))) / Ts_e_tau2;
|
||||
P[4] = (Q[3] * Tsq4 + e_tau4 * (D[4] + Q[3]) + 2 * Ts * e_tau3 * (D[4] + 2 * Q[3]) + 4 * Q[3] * Tsq3 * e_tau
|
||||
+ Tsq * e_tau2 * (D[4] + 6 * Q[3] + u1 * (D[27] * u1 + 2 * D[21]) + u1_in * (D[27] * u1_in - 2 * D[21]))
|
||||
+ 2 * D[21] * Ts * e_tau3 * (u1 - u1_in) - 2 * D[27] * Tsq * u1 * u1_in * e_tau2) / Ts_e_tau4;
|
||||
P[5] = (D[5] * (e_tau2 + Ts * e_tau) + Ts * e_b2 * e_tau2 * (D[6] - D[34])
|
||||
+ Tsq * e_b2 * e_tau * (D[6] - D[34]) + D[19] * Ts * e_tau * (u2 - u2_in)
|
||||
+ D[13] * e_b2 * (u2 * (Ts * e_tau2 + Tsq * e_tau) - bias2 * (Ts * e_tau2 + Tsq * e_tau))
|
||||
+ D[22] * Tsq * e_b2 * e_tau * (u2 - u2_in) + D[36] * Tsq * e_b2 * e_tau * (u2_in - u2)
|
||||
+ D[25] * Tsq * e_b2 * e_tau * (u2 * (u2 - bias2) + u2_in * (bias2 - u2))) / Ts_e_tau2;
|
||||
P[6] = (Q[4] * Tsq4 + e_tau4 * (D[6] + Q[4]) + 2 * Ts * e_tau3 * (D[6] + 2 * Q[4]) + 4 * Q[4] * Tsq3 * e_tau
|
||||
+ Tsq * e_tau2 * (D[6] + 6 * Q[4] + u2 * (D[27] * u2 + 2 * D[22]) + u2_in * (D[27] * u2_in - 2 * D[22]))
|
||||
+ 2 * D[22] * Ts * e_tau3 * (u2 - u2_in) - 2 * D[27] * Tsq * u2 * u2_in * e_tau2) / Ts_e_tau4;
|
||||
P[7] = (D[7] * (e_tau2 + Ts * e_tau) + Ts * e_b3 * e_tau2 * (D[8] - D[39])
|
||||
+ Tsq * e_b3 * e_tau * (D[8] - D[39]) + D[20] * Ts * e_tau * (u3 - u3_in)
|
||||
+ D[16] * e_b3 * (u3 * (Ts * e_tau2 + Tsq * e_tau) - bias3 * (Ts * e_tau2 + Tsq * e_tau))
|
||||
+ D[23] * Tsq * e_b3 * e_tau * (u3 - u3_in) + D[41] * Tsq * e_b3 * e_tau * (u3_in - u3)
|
||||
+ D[26] * Tsq * e_b3 * e_tau * (u3 * (u3 - bias3) + u3_in * (bias3 - u3))) / Ts_e_tau2;
|
||||
P[8] = (Q[5] * Tsq4 + e_tau4 * (D[8] + Q[5]) + 2 * Ts * e_tau3 * (D[8] + 2 * Q[5]) + 4 * Q[5] * Tsq3 * e_tau
|
||||
+ Tsq * e_tau2 * (D[8] + 6 * Q[5] + u3 * (D[27] * u3 + 2 * D[23]) + u3_in * (D[27] * u3_in - 2 * D[23]))
|
||||
+ 2 * D[23] * Ts * e_tau3 * (u3 - u3_in) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2) / Ts_e_tau4;
|
||||
P[9] = D[9] - Ts * e_b1 * (D[30] - D[10] + D[11] * (bias1 - u1));
|
||||
P[10] = (D[10] * (Ts + e_tau) + D[24] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2);
|
||||
P[11] = D[11] + Q[6];
|
||||
P[12] = D[12] - Ts * e_b2 * (D[35] - D[13] + D[14] * (bias2 - u2));
|
||||
P[13] = (D[13] * (Ts + e_tau) + D[25] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2);
|
||||
P[14] = D[14] + Q[7];
|
||||
P[15] = D[15] - Ts * e_b3 * (D[40] - D[16] + D[17] * (bias3 - u3));
|
||||
P[16] = (D[16] * (Ts + e_tau) + D[26] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2);
|
||||
P[17] = D[17] + Q[8];
|
||||
P[18] = D[18] - Ts * e_b1 * (D[31] - D[21] + D[24] * (bias1 - u1));
|
||||
P[19] = D[19] - Ts * e_b2 * (D[36] - D[22] + D[25] * (bias2 - u2));
|
||||
P[20] = D[20] - Ts * e_b3 * (D[41] - D[23] + D[26] * (bias3 - u3));
|
||||
P[21] = (D[21] * (Ts + e_tau) + D[27] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2);
|
||||
P[22] = (D[22] * (Ts + e_tau) + D[27] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2);
|
||||
P[23] = (D[23] * (Ts + e_tau) + D[27] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2);
|
||||
P[24] = D[24];
|
||||
P[25] = D[25];
|
||||
P[26] = D[26];
|
||||
P[27] = D[27] + Q[9];
|
||||
P[28] = D[28] - Ts * e_b1 * (D[32] - D[29] + D[30] * (bias1 - u1));
|
||||
P[29] = (D[29] * (Ts + e_tau) + D[31] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2);
|
||||
P[30] = D[30];
|
||||
P[31] = D[31];
|
||||
P[32] = D[32] + Q[10];
|
||||
P[33] = D[33] - Ts * e_b2 * (D[37] - D[34] + D[35] * (bias2 - u2));
|
||||
P[34] = (D[34] * (Ts + e_tau) + D[36] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2);
|
||||
P[35] = D[35];
|
||||
P[36] = D[36];
|
||||
P[37] = D[37] + Q[11];
|
||||
P[38] = D[38] - Ts * e_b3 * (D[42] - D[39] + D[40] * (bias3 - u3));
|
||||
P[39] = (D[39] * (Ts + e_tau) + D[41] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2);
|
||||
P[40] = D[40];
|
||||
P[41] = D[41];
|
||||
P[42] = D[42] + Q[12];
|
||||
|
||||
/********* this is the update part of the equation ***********/
|
||||
float S[3] = { P[0] + s_a, P[1] + s_a, P[2] + s_a };
|
||||
X[0] = w1 + P[0] * ((gyro_x - w1) / S[0]);
|
||||
X[1] = w2 + P[1] * ((gyro_y - w2) / S[1]);
|
||||
X[2] = w3 + P[2] * ((gyro_z - w3) / S[2]);
|
||||
X[3] = u1 + P[3] * ((gyro_x - w1) / S[0]);
|
||||
X[4] = u2 + P[5] * ((gyro_y - w2) / S[1]);
|
||||
X[5] = u3 + P[7] * ((gyro_z - w3) / S[2]);
|
||||
X[6] = b1 + P[9] * ((gyro_x - w1) / S[0]);
|
||||
X[7] = b2 + P[12] * ((gyro_y - w2) / S[1]);
|
||||
X[8] = b3 + P[15] * ((gyro_z - w3) / S[2]);
|
||||
X[9] = tau + P[18] * ((gyro_x - w1) / S[0]) + P[19] * ((gyro_y - w2) / S[1]) + P[20] * ((gyro_z - w3) / S[2]);
|
||||
X[10] = bias1 + P[28] * ((gyro_x - w1) / S[0]);
|
||||
X[11] = bias2 + P[33] * ((gyro_y - w2) / S[1]);
|
||||
X[12] = bias3 + P[38] * ((gyro_z - w3) / S[2]);
|
||||
|
||||
// update the duplicate cache
|
||||
for (uint32_t i = 0; i < AF_NUMP; i++) {
|
||||
D[i] = P[i];
|
||||
}
|
||||
|
||||
// This is an approximation that removes some cross axis uncertainty but
|
||||
// substantially reduces the number of calculations
|
||||
P[0] = -D[0] * (D[0] / S[0] - 1);
|
||||
P[1] = -D[1] * (D[1] / S[1] - 1);
|
||||
P[2] = -D[2] * (D[2] / S[2] - 1);
|
||||
P[3] = -D[3] * (D[0] / S[0] - 1);
|
||||
P[4] = D[4] - D[3] * D[3] / S[0];
|
||||
P[5] = -D[5] * (D[1] / S[1] - 1);
|
||||
P[6] = D[6] - D[5] * D[5] / S[1];
|
||||
P[7] = -D[7] * (D[2] / S[2] - 1);
|
||||
P[8] = D[8] - D[7] * D[7] / S[2];
|
||||
P[9] = -D[9] * (D[0] / S[0] - 1);
|
||||
P[10] = D[10] - D[3] * (D[9] / S[0]);
|
||||
P[11] = D[11] - D[9] * (D[9] / S[0]);
|
||||
P[12] = -D[12] * (D[1] / S[1] - 1);
|
||||
P[13] = D[13] - D[5] * (D[12] / S[1]);
|
||||
P[14] = D[14] - D[12] * (D[12] / S[1]);
|
||||
P[15] = -D[15] * (D[2] / S[2] - 1);
|
||||
P[16] = D[16] - D[7] * (D[15] / S[2]);
|
||||
P[17] = D[17] - D[15] * (D[15] / S[2]);
|
||||
P[18] = -D[18] * (D[0] / S[0] - 1);
|
||||
P[19] = -D[19] * (D[1] / S[1] - 1);
|
||||
P[20] = -D[20] * (D[2] / S[2] - 1);
|
||||
P[21] = D[21] - D[3] * (D[18] / S[0]);
|
||||
P[22] = D[22] - D[5] * (D[19] / S[1]);
|
||||
P[23] = D[23] - D[7] * (D[20] / S[2]);
|
||||
P[24] = D[24] - D[9] * (D[18] / S[0]);
|
||||
P[25] = D[25] - D[12] * (D[19] / S[1]);
|
||||
P[26] = D[26] - D[15] * (D[20] / S[2]);
|
||||
P[27] = D[27] - D[18] * (D[18] / S[0]) - D[19] * (D[19] / S[1]) - D[20] * (D[20] / S[2]);
|
||||
P[28] = -D[28] * (D[0] / S[0] - 1);
|
||||
P[29] = D[29] - D[3] * (D[28] / S[0]);
|
||||
P[30] = D[30] - D[9] * (D[28] / S[0]);
|
||||
P[31] = D[31] - D[18] * (D[28] / S[0]);
|
||||
P[32] = D[32] - D[28] * (D[28] / S[0]);
|
||||
P[33] = -D[33] * (D[1] / S[1] - 1);
|
||||
P[34] = D[34] - D[5] * (D[33] / S[1]);
|
||||
P[35] = D[35] - D[12] * (D[33] / S[1]);
|
||||
P[36] = D[36] - D[19] * (D[33] / S[1]);
|
||||
P[37] = D[37] - D[33] * (D[33] / S[1]);
|
||||
P[38] = -D[38] * (D[2] / S[2] - 1);
|
||||
P[39] = D[39] - D[7] * (D[38] / S[2]);
|
||||
P[40] = D[40] - D[15] * (D[38] / S[2]);
|
||||
P[41] = D[41] - D[20] * (D[38] / S[2]);
|
||||
P[42] = D[42] - D[38] * (D[38] / S[2]);
|
||||
|
||||
// apply limits to some of the state variables
|
||||
if (X[9] > -1.5f) {
|
||||
X[9] = -1.5f;
|
||||
} else if (X[9] < -5.5f) { /* 4ms */
|
||||
X[9] = -5.5f;
|
||||
}
|
||||
if (X[10] > 0.5f) {
|
||||
X[10] = 0.5f;
|
||||
} else if (X[10] < -0.5f) {
|
||||
X[10] = -0.5f;
|
||||
}
|
||||
if (X[11] > 0.5f) {
|
||||
X[11] = 0.5f;
|
||||
} else if (X[11] < -0.5f) {
|
||||
X[11] = -0.5f;
|
||||
}
|
||||
if (X[12] > 0.5f) {
|
||||
X[12] = 0.5f;
|
||||
} else if (X[12] < -0.5f) {
|
||||
X[12] = -0.5f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Initialize the state variable and covariance matrix
|
||||
* for the system identification EKF
|
||||
*/
|
||||
static void AfInit(float X[AF_NUMX], float P[AF_NUMP])
|
||||
{
|
||||
static const float qInit[AF_NUMX] = {
|
||||
1.0f, 1.0f, 1.0f,
|
||||
1.0f, 1.0f, 1.0f,
|
||||
0.05f, 0.05f, 0.005f,
|
||||
0.05f,
|
||||
0.05f, 0.05f, 0.05f
|
||||
};
|
||||
|
||||
// X[0] = X[1] = X[2] = 0.0f; // assume no rotation
|
||||
// X[3] = X[4] = X[5] = 0.0f; // and no net torque
|
||||
// X[6] = X[7] = 10.0f; // roll and pitch medium amount of strength
|
||||
// X[8] = 7.0f; // yaw strength
|
||||
// X[9] = -4.0f; // and 50 (18?) ms time scale
|
||||
// X[10] = X[11] = X[12] = 0.0f; // zero bias
|
||||
|
||||
memset(X, 0, AF_NUMX * sizeof(X[0]));
|
||||
// get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau)
|
||||
// so that if they are changed there (mainly for future code changes), they will be changed here too
|
||||
memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta));
|
||||
X[9] = systemIdentState.Tau;
|
||||
|
||||
// P initialization
|
||||
memset(P, 0, AF_NUMP * sizeof(P[0]));
|
||||
P[0] = qInit[0];
|
||||
P[1] = qInit[1];
|
||||
P[2] = qInit[2];
|
||||
P[4] = qInit[3];
|
||||
P[6] = qInit[4];
|
||||
P[8] = qInit[5];
|
||||
P[11] = qInit[6];
|
||||
P[14] = qInit[7];
|
||||
P[17] = qInit[8];
|
||||
P[27] = qInit[9];
|
||||
P[32] = qInit[10];
|
||||
P[37] = qInit[11];
|
||||
P[42] = qInit[12];
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -1,337 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup Autotuning module
|
||||
* @brief Reads from @ref ManualControlCommand and fakes a rate mode while
|
||||
* toggling the channels to relay mode
|
||||
* @{
|
||||
*
|
||||
* @file autotune.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
||||
*
|
||||
* @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
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input objects: None, takes sensor data via pios
|
||||
* Output objects: @ref AttitudeRaw @ref AttitudeActual
|
||||
*
|
||||
* This module computes an attitude estimate from the sensor data
|
||||
*
|
||||
* The module executes in its own thread.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* 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 "flightstatus.h"
|
||||
#include "hwsettings.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "relaytuning.h"
|
||||
#include "relaytuningsettings.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "taskinfo.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
|
||||
|
||||
// Private types
|
||||
enum AUTOTUNE_STATE { AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET };
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
static bool autotuneEnabled;
|
||||
|
||||
// Private functions
|
||||
static void AutotuneTask(void *parameters);
|
||||
static void update_stabilization_settings();
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AutotuneInitialize(void)
|
||||
{
|
||||
// Create a queue, connect to manual control command and flightstatus
|
||||
#ifdef MODULE_AUTOTUNE_BUILTIN
|
||||
autotuneEnabled = true;
|
||||
#else
|
||||
HwSettingsInitialize();
|
||||
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
|
||||
|
||||
HwSettingsOptionalModulesGet(optionalModules);
|
||||
|
||||
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
|
||||
autotuneEnabled = true;
|
||||
} else {
|
||||
autotuneEnabled = false;
|
||||
}
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AutotuneStart(void)
|
||||
{
|
||||
// Start main task if it is enabled
|
||||
if (autotuneEnabled) {
|
||||
xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(AutotuneInitialize, AutotuneStart);
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void AutotuneTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
// AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
|
||||
enum AUTOTUNE_STATE state = AT_INIT;
|
||||
|
||||
portTickType lastUpdateTime = xTaskGetTickCount();
|
||||
|
||||
while (1) {
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
|
||||
// TODO:
|
||||
// 1. get from queue
|
||||
// 2. based on whether it is flightstatus or manualcontrol
|
||||
|
||||
portTickType diffTime;
|
||||
|
||||
const uint32_t PREPARE_TIME = 2000;
|
||||
const uint32_t MEAURE_TIME = 30000;
|
||||
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
// Only allow this module to run when autotuning
|
||||
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
|
||||
state = AT_INIT;
|
||||
vTaskDelay(50);
|
||||
continue;
|
||||
}
|
||||
|
||||
StabilizationDesiredData stabDesired;
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
|
||||
StabilizationSettingsData stabSettings;
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
ManualControlSettingsData manualSettings;
|
||||
ManualControlSettingsGet(&manualSettings);
|
||||
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
|
||||
RelayTuningSettingsData relaySettings;
|
||||
RelayTuningSettingsGet(&relaySettings);
|
||||
|
||||
bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE;
|
||||
|
||||
if (rate) { // rate mode
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
|
||||
stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
|
||||
stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
|
||||
} else {
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
|
||||
stabDesired.Roll = manualControl.Roll * stabSettings.RollMax;
|
||||
stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax;
|
||||
}
|
||||
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_THRUST] = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
stabDesired.Thrust = manualControl.Thrust;
|
||||
|
||||
switch (state) {
|
||||
case AT_INIT:
|
||||
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
|
||||
// Only start when armed and flying
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Thrust > 0) {
|
||||
state = AT_START;
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_START:
|
||||
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
|
||||
// Spend the first block of time in normal rate mode to get airborne
|
||||
if (diffTime > PREPARE_TIME) {
|
||||
state = AT_ROLL;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_ROLL:
|
||||
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
|
||||
// Run relay mode on the roll axis for the measurement time
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
|
||||
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
|
||||
if (diffTime > MEAURE_TIME) { // Move on to next state
|
||||
state = AT_PITCH;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_PITCH:
|
||||
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
|
||||
// Run relay mode on the pitch axis for the measurement time
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
|
||||
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
|
||||
if (diffTime > MEAURE_TIME) { // Move on to next state
|
||||
state = AT_FINISHED;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_FINISHED:
|
||||
|
||||
// Wait until disarmed and landed before updating the settings
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Thrust <= 0) {
|
||||
state = AT_SET;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case AT_SET:
|
||||
update_stabilization_settings();
|
||||
state = AT_INIT;
|
||||
break;
|
||||
|
||||
default:
|
||||
// Set an alarm or some shit like that
|
||||
break;
|
||||
}
|
||||
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
|
||||
vTaskDelay(10);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Called after measuring roll and pitch to update the
|
||||
* stabilization settings
|
||||
*
|
||||
* takes in @ref RelayTuning and outputs @ref StabilizationSettings
|
||||
*/
|
||||
static void update_stabilization_settings()
|
||||
{
|
||||
RelayTuningData relayTuning;
|
||||
|
||||
RelayTuningGet(&relayTuning);
|
||||
|
||||
RelayTuningSettingsData relaySettings;
|
||||
RelayTuningSettingsGet(&relaySettings);
|
||||
|
||||
StabilizationSettingsData stabSettings;
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
// Eventually get these settings from RelayTuningSettings
|
||||
const float gain_ratio_r = 1.0f / 3.0f;
|
||||
const float zero_ratio_r = 1.0f / 3.0f;
|
||||
const float gain_ratio_p = 1.0f / 5.0f;
|
||||
const float zero_ratio_p = 1.0f / 5.0f;
|
||||
|
||||
// For now just run over roll and pitch
|
||||
for (uint i = 0; i < 2; i++) {
|
||||
float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s)
|
||||
|
||||
float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s)
|
||||
float zc = wc * zero_ratio_r; // controller zero location (rad/s)
|
||||
float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity
|
||||
float kp = kpu * gain_ratio_r; // proportional gain
|
||||
float ki = zc * kp; // integral gain
|
||||
|
||||
// Now calculate gains for the next loop out knowing it is the integral of
|
||||
// the inner loop -- the plant is position/velocity = scale*1/s
|
||||
float wc2 = wc * gain_ratio_p; // crossover of the attitude loop
|
||||
float kp2 = wc2; // kp of attitude
|
||||
float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude
|
||||
|
||||
switch (i) {
|
||||
case 0: // roll
|
||||
stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
|
||||
stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
|
||||
stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
|
||||
stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
|
||||
break;
|
||||
case 1: // Pitch
|
||||
stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
|
||||
stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
|
||||
stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
|
||||
stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
|
||||
break;
|
||||
default:
|
||||
// Oh shit oh shit oh shit
|
||||
break;
|
||||
}
|
||||
}
|
||||
switch (relaySettings.Behavior) {
|
||||
case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE:
|
||||
// Just measure, don't update the stab settings
|
||||
break;
|
||||
case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE:
|
||||
StabilizationSettingsSet(&stabSettings);
|
||||
break;
|
||||
case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE:
|
||||
StabilizationSettingsSet(&stabSettings);
|
||||
UAVObjSave(StabilizationSettingsHandle(), 0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -1,37 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup Attitude Attitude Module
|
||||
* @{
|
||||
*
|
||||
* @file attitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief Acquires sensor data and fuses it into attitude estimate for CC
|
||||
*
|
||||
* @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 ATTITUDE_H
|
||||
#define ATTITUDE_H
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
int32_t AttitudeInitialize(void);
|
||||
|
||||
#endif // ATTITUDE_H
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file armhandler.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -342,6 +343,9 @@ static bool okToArm(void)
|
||||
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
return false;
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
|
||||
|
||||
@ -11,7 +11,8 @@
|
||||
* AttitudeDesired object (stabilized mode)
|
||||
*
|
||||
* @file manualcontrol.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief ManualControl module. Handles safety R/C link and flight mode.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -47,7 +48,7 @@
|
||||
#include <stabilizationsettings.h>
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
// Private constants
|
||||
#if defined(PIOS_MANUAL_STACK_SIZE)
|
||||
@ -118,7 +119,7 @@ static void commandUpdatedCb(UAVObjEvent *ev);
|
||||
static void manualControlTask(void);
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
|
||||
@ -146,8 +147,8 @@ int32_t ManualControlStart()
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
takeOffLocationHandlerInit();
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
#endif
|
||||
// Start main task
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
|
||||
@ -174,7 +175,7 @@ int32_t ManualControlInitialize()
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
SystemSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
|
||||
|
||||
return 0;
|
||||
@ -202,7 +203,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
}
|
||||
|
||||
/**
|
||||
@ -214,7 +215,7 @@ static void manualControlTask(void)
|
||||
armHandler(false, frameType);
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
takeOffLocationHandler();
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
// Process flight mode
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
@ -225,7 +226,7 @@ static void manualControlTask(void)
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
VtolPathFollowerSettingsThrustLimitsData thrustLimits;
|
||||
VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
FlightModeSettingsData modeSettings;
|
||||
FlightModeSettingsGet(&modeSettings);
|
||||
@ -273,6 +274,9 @@ static void manualControlTask(void)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
handler = &handler_STABILIZED;
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
@ -286,7 +290,6 @@ static void manualControlTask(void)
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
}
|
||||
|
||||
|
||||
if (newFlightModeAssist) {
|
||||
// assess roll/pitch state
|
||||
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
|
||||
@ -539,22 +542,21 @@ static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
}
|
||||
|
||||
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
/**
|
||||
* Check and set modes for gps assisted stablised flight modes
|
||||
*/
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
|
||||
{
|
||||
uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
|
||||
uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
|
||||
|
||||
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
|
||||
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
||||
flightModeAssistOption = FlightModeAssistMap[position];
|
||||
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE
|
||||
|| position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
||||
return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
}
|
||||
|
||||
switch (flightModeAssistOption) {
|
||||
switch (FlightModeAssistMap[position]) {
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
|
||||
break;
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST:
|
||||
@ -601,22 +603,22 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD:
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
|
||||
// this is only for use with stabi mods with althold/vario.
|
||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
||||
break;
|
||||
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
||||
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
|
||||
default:
|
||||
// this is the default for non stabi modes also
|
||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
||||
break;
|
||||
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return isAssistedFlag;
|
||||
// return isAssistedFlag;
|
||||
return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
}
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
/**
|
||||
* @}
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file stabilizedhandler.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -35,6 +36,7 @@
|
||||
#include <stabilizationdesired.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <flightstatus.h>
|
||||
|
||||
// Private constants
|
||||
|
||||
@ -48,6 +50,7 @@ static uint8_t currentFpvTiltAngle = 0;
|
||||
static float cosAngle = 0.0f;
|
||||
static float sinAngle = 0.0f;
|
||||
|
||||
|
||||
static float applyExpo(float value, float expo)
|
||||
{
|
||||
// note: fastPow makes a small error, therefore result needs to be bound
|
||||
@ -74,12 +77,16 @@ static float applyExpo(float value, float expo)
|
||||
* @input: ManualControlCommand
|
||||
* @output: StabilizationDesired
|
||||
*/
|
||||
void stabilizedHandler(bool newinit)
|
||||
void stabilizedHandler(__attribute__((unused)) bool newinit)
|
||||
{
|
||||
if (newinit) {
|
||||
static bool inited = false;
|
||||
|
||||
if (!inited) {
|
||||
inited = true;
|
||||
StabilizationDesiredInitialize();
|
||||
StabilizationBankInitialize();
|
||||
}
|
||||
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
|
||||
@ -116,7 +123,6 @@ void stabilizedHandler(bool newinit)
|
||||
uint8_t *stab_settings;
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
||||
@ -136,6 +142,13 @@ void stabilizedHandler(bool newinit)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
|
||||
break;
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||
// let autotune.c handle it
|
||||
// because it must switch to Attitude after <user configurable> seconds
|
||||
return;
|
||||
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
default:
|
||||
// Major error, this should not occur because only enter this block when one of these is true
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
@ -153,6 +166,9 @@ void stabilizedHandler(bool newinit)
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Roll * stabSettings.RollMax :
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
0; // this is an invalid mode
|
||||
|
||||
stabilization.Pitch =
|
||||
@ -165,6 +181,9 @@ void stabilizedHandler(bool newinit)
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Pitch * stabSettings.PitchMax :
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
0; // this is an invalid mode
|
||||
|
||||
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
||||
@ -187,6 +206,9 @@ void stabilizedHandler(bool newinit)
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
0; // this is an invalid mode
|
||||
}
|
||||
|
||||
|
||||
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file sequences.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Notify module, sequences configuration.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -184,6 +185,9 @@ const LedSequence_t *flightModeMap[] = {
|
||||
[FLIGHTSTATUS_FLIGHTMODE_POI] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND],
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
};
|
||||
|
||||
// List of alarms to show with attached sequences for each status
|
||||
|
||||
@ -9,7 +9,7 @@
|
||||
* @{
|
||||
*
|
||||
* @file innerloop.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
@ -51,6 +51,10 @@
|
||||
#include <virtualflybar.h>
|
||||
#include <cruisecontrol.h>
|
||||
#include <sanitycheck.h>
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
#include <systemidentstate.h>
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
// Private constants
|
||||
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
|
||||
@ -60,6 +64,16 @@
|
||||
#define UPDATE_MAX 1.0f
|
||||
#define UPDATE_ALPHA 1.0e-2f
|
||||
|
||||
#define SYSTEM_IDENT_PERIOD ((uint32_t)75)
|
||||
|
||||
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
#define powapprox fastpow
|
||||
#define expapprox fastexp
|
||||
#else
|
||||
#define powapprox powf
|
||||
#define expapprox expf
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
// Private variables
|
||||
static DelayedCallbackInfo *callbackHandle;
|
||||
static float gyro_filtered[3] = { 0, 0, 0 };
|
||||
@ -69,6 +83,10 @@ static PiOSDeltatimeConfig timeval;
|
||||
static float speedScaleFactor = 1.0f;
|
||||
static bool frame_is_multirotor;
|
||||
static bool measuredDterm_enabled;
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
static uint32_t systemIdentTimeVal = 0;
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
// Private functions
|
||||
static void stabilizationInnerloopTask();
|
||||
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
@ -86,6 +104,9 @@ void stabilizationInnerloopInit()
|
||||
ManualControlCommandInitialize();
|
||||
StabilizationDesiredInitialize();
|
||||
ActuatorDesiredInitialize();
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
SystemIdentStateInitialize();
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
#ifdef REVOLUTION
|
||||
AirspeedStateInitialize();
|
||||
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
|
||||
@ -100,6 +121,10 @@ void stabilizationInnerloopInit()
|
||||
|
||||
frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
|
||||
measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE);
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
// Settings for system identification
|
||||
systemIdentTimeVal = PIOS_DELAY_GetRaw();
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
}
|
||||
|
||||
static float get_pid_scale_source_value()
|
||||
@ -230,7 +255,6 @@ static void stabilizationInnerloopTask()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
RateDesiredData rateDesired;
|
||||
ActuatorDesiredData actuator;
|
||||
StabilizationStatusInnerLoopData enabled;
|
||||
@ -286,6 +310,7 @@ static void stabilizationInnerloopTask()
|
||||
// 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],
|
||||
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
|
||||
@ -293,6 +318,7 @@ static void stabilizationInnerloopTask()
|
||||
);
|
||||
pid_scaler scaler = create_pid_scaler(t);
|
||||
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
|
||||
}
|
||||
break;
|
||||
case STABILIZATIONSTATUS_INNERLOOP_ACRO:
|
||||
{
|
||||
@ -312,6 +338,67 @@ static void stabilizationInnerloopTask()
|
||||
actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
|
||||
}
|
||||
break;
|
||||
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT:
|
||||
{
|
||||
static int8_t identIteration = 0;
|
||||
static float identOffsets[3] = { 0 };
|
||||
|
||||
if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) {
|
||||
const float SCALE_BIAS = 7.1f;
|
||||
SystemIdentStateBetaData systemIdentBeta;
|
||||
|
||||
SystemIdentStateBetaGet(&systemIdentBeta);
|
||||
systemIdentTimeVal = PIOS_DELAY_GetRaw();
|
||||
identOffsets[0] = 0.0f;
|
||||
identOffsets[1] = 0.0f;
|
||||
identOffsets[2] = 0.0f;
|
||||
identIteration = (identIteration + 1) & 7;
|
||||
// why does yaw change twice a cycle and roll/pitch change only once?
|
||||
uint8_t index = ((uint8_t[]) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' }
|
||||
)[identIteration];
|
||||
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentBeta)[index]);
|
||||
// if roll or pitch limit to 25% of range
|
||||
if (identIteration & 1) {
|
||||
if (scale > 0.25f) {
|
||||
scale = 0.25f;
|
||||
}
|
||||
}
|
||||
// else it is yaw that can be a little more radical
|
||||
else {
|
||||
if (scale > 0.45f) {
|
||||
scale = 0.45f;
|
||||
}
|
||||
}
|
||||
if (identIteration & 2) {
|
||||
scale = -scale;
|
||||
}
|
||||
identOffsets[index] = scale;
|
||||
// this results in:
|
||||
// when identIteration==0: identOffsets[2] = yaw_scale;
|
||||
// when identIteration==1: identOffsets[0] = roll_scale;
|
||||
// when identIteration==2: identOffsets[2] = -yaw_scale;
|
||||
// when identIteration==3: identOffsets[0] = -roll_scale;
|
||||
// when identIteration==4: identOffsets[2] = yaw_scale;
|
||||
// when identIteration==5: identOffsets[1] = pitch_scale;
|
||||
// when identIteration==6: identOffsets[2] = -yaw_scale;
|
||||
// when identIteration==7: identOffsets[1] = -pitch_scale;
|
||||
// each change has one axis with an offset
|
||||
// and another axis coming back to zero from having an offset
|
||||
}
|
||||
|
||||
rate[t] = boundf(rate[t],
|
||||
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
|
||||
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
|
||||
);
|
||||
pid_scaler scaler = create_pid_scaler(t);
|
||||
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
|
||||
actuatorDesiredAxis[t] += identOffsets[t];
|
||||
}
|
||||
break;
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
|
||||
default:
|
||||
actuatorDesiredAxis[t] = rate[t];
|
||||
|
||||
@ -9,7 +9,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file stabilization.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -141,6 +142,24 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
|
||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
// roll or pitch
|
||||
if (t <= 1) {
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
}
|
||||
// else yaw (other modes don't worry about invalid thrust mode either)
|
||||
else {
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
}
|
||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT;
|
||||
break;
|
||||
#else /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
// no break, do not reorder this code
|
||||
// for low power FCs just fall through to Attitude mode
|
||||
// that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
// do not reorder this code
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file pios_settings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Settings functions header
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -39,6 +40,7 @@ extern uint32_t PIOS_DELAY_GetuS();
|
||||
extern uint32_t PIOS_DELAY_GetuSSince(uint32_t t);
|
||||
extern uint32_t PIOS_DELAY_GetRaw();
|
||||
extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw);
|
||||
extern uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later);
|
||||
|
||||
#endif /* PIOS_DELAY_H */
|
||||
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file pios_delay.c
|
||||
* @author Michael Smith Copyright (C) 2012
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* Michael Smith Copyright (C) 2012
|
||||
* @brief Delay Functions
|
||||
* - Provides a micro-second granular delay using the CPU
|
||||
* cycle counter.
|
||||
@ -168,6 +169,17 @@ uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
|
||||
return diff / us_ticks;
|
||||
}
|
||||
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
/**
|
||||
* @brief Subtract two raw times and convert to us.
|
||||
* @return Interval between raw times in microseconds
|
||||
*/
|
||||
uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later)
|
||||
{
|
||||
return (later - raw) / us_ticks;
|
||||
}
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
#endif /* PIOS_INCLUDE_DELAY */
|
||||
|
||||
/**
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#
|
||||
# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
#
|
||||
# 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
|
||||
@ -55,6 +55,7 @@ MODULES += Osd/osdoutout
|
||||
MODULES += Logging
|
||||
MODULES += Telemetry
|
||||
|
||||
OPTMODULES += AutoTune
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
#
|
||||
# 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
|
||||
@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemidentsettings
|
||||
UAVOBJSRCFILENAMES += systemidentstate
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#
|
||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
#
|
||||
# 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
|
||||
@ -52,6 +52,7 @@ MODULES += Logging
|
||||
MODULES += Telemetry
|
||||
MODULES += Notify
|
||||
|
||||
OPTMODULES += AutoTune
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
#
|
||||
# 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
|
||||
@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemidentsettings
|
||||
UAVOBJSRCFILENAMES += systemidentstate
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#
|
||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2009-2014, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2014, The OpenPilot Team, http://www.openpilot.org
|
||||
#
|
||||
# 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
|
||||
@ -51,6 +51,7 @@ MODULES += PathFollower
|
||||
MODULES += Telemetry
|
||||
MODULES += Notify
|
||||
|
||||
OPTMODULES += AutoTune
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
#
|
||||
# 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
|
||||
@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemidentsettings
|
||||
UAVOBJSRCFILENAMES += systemidentstate
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#
|
||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
#
|
||||
# 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
|
||||
@ -52,6 +52,7 @@ MODULES += Telemetry
|
||||
|
||||
SRC += $(FLIGHTLIB)/notification.c
|
||||
|
||||
OPTMODULES += AutoTune
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
#
|
||||
# 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
|
||||
@ -124,6 +125,9 @@ UAVOBJSRCFILENAMES += mpugyroaccelsettings
|
||||
UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
# UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemidentsettings
|
||||
UAVOBJSRCFILENAMES += systemidentstate
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
#
|
||||
# TODO: This file should be reworked. It will be done as a part of sim target refactoring.
|
||||
#
|
||||
# Copyright (c) 2015 The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2015-2016 The LibrePilot Project, http://www.librepilot.org
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
@ -50,6 +50,8 @@ MODULES += Airspeed
|
||||
|
||||
SRC += $(FLIGHTLIB)/notification.c
|
||||
|
||||
OPTMODULES += AutoTune
|
||||
|
||||
# Paths
|
||||
OPSYSTEM = .
|
||||
BOARDINC = ..
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
#
|
||||
# Makefile for OpenPilot UAVObject code
|
||||
#
|
||||
# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
@ -119,6 +120,9 @@ UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
# UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemidentsettings
|
||||
UAVOBJSRCFILENAMES += systemidentstate
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#
|
||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
#
|
||||
# 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
|
||||
@ -52,6 +52,7 @@ MODULES += Logging
|
||||
MODULES += Telemetry
|
||||
MODULES += Notify
|
||||
|
||||
OPTMODULES += AutoTune
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#
|
||||
# Copyright (c) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
#
|
||||
# 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
|
||||
@ -126,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemidentsettings
|
||||
UAVOBJSRCFILENAMES += systemidentstate
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -137,6 +137,8 @@ UAVOBJS = \
|
||||
$${UAVOBJ_XML_DIR}/statusvtolautotakeoff.xml \
|
||||
$${UAVOBJ_XML_DIR}/statusvtolland.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemalarms.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemidentsettings.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemidentstate.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemsettings.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemstats.xml \
|
||||
$${UAVOBJ_XML_DIR}/takeofflocation.xml \
|
||||
|
||||
@ -7,57 +7,57 @@
|
||||
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
|
||||
<field name="Stabilization1Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,AxisLock,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||
/>
|
||||
<field name="Stabilization2Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,Rate,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||
/>
|
||||
<field name="Stabilization3Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Rate,Rate,Rate,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||
/>
|
||||
<field name="Stabilization4Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,AxisLock,CruiseControl"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||
/>
|
||||
<field name="Stabilization5Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,Rate,CruiseControl"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||
/>
|
||||
<field name="Stabilization6Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Rate,Rate,Rate,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||
/>
|
||||
|
||||
<!-- Note these options values should be identical to those defined in FlightMode -->
|
||||
@ -66,7 +66,7 @@
|
||||
units=""
|
||||
type="enum"
|
||||
elements="6"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff,AutoTune"
|
||||
defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6"
|
||||
limits="%NE:POI:AutoCruise; \
|
||||
%NE:POI:AutoCruise; \
|
||||
|
||||
@ -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,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"/>
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff,AutoTune"/>
|
||||
<!-- Keep Armed (first pos) and FlightMode (second pos) before this line for OSD compatibility -->
|
||||
<field name="AlwaysStabilizeWhenArmed" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
|
||||
|
||||
@ -29,7 +29,7 @@
|
||||
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
|
||||
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
|
||||
|
||||
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,Fault,Altitude,Airspeed,TxPID,Battery,Overo,MagBaro,OsdHk" options="Disabled,Enabled" defaultvalue="Disabled"/>
|
||||
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,Fault,Altitude,Airspeed,TxPID,Battery,Overo,MagBaro,OsdHk,AutoTune" options="Disabled,Enabled" defaultvalue="Disabled"/>
|
||||
<field name="ADCRouting" units="" type="enum" elementnames="adc0,adc1,adc2,adc3,adc4,adc5,adc6,adc7" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/>
|
||||
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
|
||||
<field name="WS2811LED_Out" units="" type="enum" elements="1" options="ServoOut1,ServoOut2,ServoOut3,ServoOut4,ServoOut5,ServoOut6,FlexiIOPin3,FlexiIOPin4,Disabled" defaultvalue="Disabled"
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
<field name="Yaw" units="degrees" type="float" elements="1"/>
|
||||
<field name="Thrust" units="%" type="float" elements="1"/>
|
||||
<!-- 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,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"/>
|
||||
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
<description>Contains status information to control submodules for stabilization.</description>
|
||||
|
||||
|
||||
<field name="OuterLoop" units="" type="enum" options="Direct,DirectWithLimits,Attitude,Rattitude,Weakleveling,Altitude,AltitudeVario">
|
||||
<field name="OuterLoop" units="" type="enum" options="Direct,DirectWithLimits,Attitude,Rattitude,Weakleveling,Altitude,AltitudeVario,SystemIdent">
|
||||
<elementnames>
|
||||
<elementname>Roll</elementname>
|
||||
<elementname>Pitch</elementname>
|
||||
@ -11,7 +11,7 @@
|
||||
<elementname>Thrust</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="InnerLoop" units="" type="enum" options="Direct,VirtualFlyBar,Acro+,AxisLock,Rate,CruiseControl">
|
||||
<field name="InnerLoop" units="" type="enum" options="Direct,VirtualFlyBar,Acro+,AxisLock,Rate,CruiseControl,SystemIdent">
|
||||
<elementnames>
|
||||
<elementname>Roll</elementname>
|
||||
<elementname>Pitch</elementname>
|
||||
|
||||
@ -37,6 +37,7 @@
|
||||
<option>FlightMode</option>
|
||||
<option>UnsupportedConfig_OneShot</option>
|
||||
<option>BadThrottleOrCollectiveInputRange</option>
|
||||
<option>AutoTune</option>
|
||||
</options>
|
||||
</field>
|
||||
<field name="ExtendedAlarmSubStatus" units="" type="uint8" defaultvalue="0">
|
||||
|
||||
59
shared/uavobjectdefinition/systemidentsettings.xml
Normal file
59
shared/uavobjectdefinition/systemidentsettings.xml
Normal file
@ -0,0 +1,59 @@
|
||||
<xml>
|
||||
<object name="SystemIdentSettings" singleinstance="true" settings="true" category="Control">
|
||||
<description>The input to the PID tuning.</description>
|
||||
<field name="Tau" units="ln(sec)" type="float" elements="1" defaultvalue="-4.0"/>
|
||||
<!-- Beta default valuses 10.0 10.0 7.0 so that SystemIdent mode can be run without AutoTune -->
|
||||
<field name="Beta" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="10.0,10.0,7.0"/>
|
||||
<!-- Decrease damping to make your aircraft response more rapidly. Increase it for more stability. -->
|
||||
<!-- Increasing noise (sensitivity) will make your aircraft respond more rapidly, but will cause twitches due to noise. -->
|
||||
<!-- Use RateDamp 130 with RateNoise 08 for very smooth flight. -->
|
||||
<!-- Use RateDamp 110 with RateNoise 10 for default flight. -->
|
||||
<!-- Use RateDamp 100 with RateNoise 13 for very snappy flight. -->
|
||||
<!-- RateNoise is [0,30] default 10. -->
|
||||
<!-- RateDamp is [50,150] default 110. -->
|
||||
<!-- per https://github.com/d-ronin/dRonin/pull/811/files change min ratedamp to 85. -->
|
||||
<!-- So RateDamp is [85,150] default 110. -->
|
||||
<!-- Extrapolated multiplicatively: -->
|
||||
<!-- Use RateDamp 153.636363636 with RateNoise 06.4 for very very smooth flight. -->
|
||||
<!-- Use RateDamp 90.909090909 with RateNoise 16.9 for very very snappy flight. -->
|
||||
<!-- Extrapolated additively: -->
|
||||
<!-- Use RateDamp 150 with RateNoise 06 for very very smooth flight. -->
|
||||
<!-- Use RateDamp 90 with RateNoise 16 for very very snappy flight. -->
|
||||
<!-- use additive so the piecewise algorithm will give the exact recommended pairs at 25%, 50%, and 75% of slider -->
|
||||
<field name="DampMin" units="" type="uint8" elements="1" defaultvalue="90"/>
|
||||
<field name="DampRate" units="" type="uint8" elements="1" defaultvalue="110"/>
|
||||
<field name="DampMax" units="" type="uint8" elements="1" defaultvalue="150"/>
|
||||
<field name="NoiseMin" units="" type="uint8" elements="1" defaultvalue="6"/>
|
||||
<field name="NoiseRate" units="" type="uint8" elements="1" defaultvalue="10"/>
|
||||
<field name="NoiseMax" units="" type="uint8" elements="1" defaultvalue="16"/>
|
||||
<!-- <field name="CalculateYaw" units="" type="enum" elements="1" options="False,True,TrueIgnoreError" defaultvalue="True"/> -->
|
||||
<field name="CalculateYaw" units="" type="enum" elements="1" options="False,TrueLimitToRatio,TrueIgnoreLimit" defaultvalue="TrueLimitToRatio"/>
|
||||
<!-- <field name="YawBetaMin" units="" type="float" elements="1" defaultvalue="5.6"/> -->
|
||||
<!-- Mateuze quad needs yaw P to be at most 2.6 times roll/pitch P to avoid yaw oscillation -->
|
||||
<!-- Cliff sluggish 500 quad thinks that yaw P should be about 5.8 times roll/pitch P, but can easily (and better) live with 2.6 -->
|
||||
<field name="YawToRollPitchPIDRatioMin" units="" type="float" elements="1" defaultvalue="1.0"/>
|
||||
<field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5"/>
|
||||
<!-- <field name="YawPIDRatioFunction" units="" type="enum" elements="1" options="Disable,Limit" defaultvalue="Limit"/> -->
|
||||
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="2"/>
|
||||
<field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60"/>
|
||||
<!-- SmoothQuickSource: the smooth vs. quick PID selector -->
|
||||
<!-- 0 = disabled -->
|
||||
<!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs -->
|
||||
<!-- an accessory knob works as expected, with full left being smoothest and full right being quickest -->
|
||||
<!-- 23, 25, and 27 are discrete 3, 5, and 7 position rount robin selectors -->
|
||||
<!-- incremented by quickly double toggling the fms 3 times (starting outside autotune mode -->
|
||||
<!-- with each double toggle going into and back out of autotune) to go to the next position -->
|
||||
<!-- think of the positions on a scale from 0 to 100, with smoothest being 0 and quickest being 100, and you start out at 50 -->
|
||||
<!-- 23 (3 stops) means stops at 50, then 100, then 0 then back to 50 -->
|
||||
<!-- 25 (5 stops) means stops at 50, 75, 100, 0, 25 (repeat as you toggle) -->
|
||||
<!-- 27 (7 stops) means stops at 50, 67, 83, 100, 0, 17, 33 (repeat as you toggle) -->
|
||||
<!-- 25 is special in that the 3 middle values (25, 50, 75) are exactly those that are recommended for smooth, normal, and quick responses -->
|
||||
<field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/>
|
||||
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
19
shared/uavobjectdefinition/systemidentstate.xml
Normal file
19
shared/uavobjectdefinition/systemidentstate.xml
Normal file
@ -0,0 +1,19 @@
|
||||
<xml>
|
||||
<object name="SystemIdentState" singleinstance="true" settings="false" category="Control">
|
||||
<description>The input to the PID tuning.</description>
|
||||
<field name="Tau" units="ln(sec)" type="float" elements="1" defaultvalue="-4.0"/>
|
||||
<!-- Beta default valuses 10.0 10.0 7.0 so that SystemIdent mode can be run without AutoTune -->
|
||||
<field name="Beta" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="10.0,10.0,7.0"/>
|
||||
<field name="Bias" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Noise" units="(deg/s)^2" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Period" units="ms" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="NumAfPredicts" units="" type="uint32" elements="1" defaultvalue="0"/>
|
||||
<field name="NumSpilledPts" units="" type="uint32" elements="1" defaultvalue="0"/>
|
||||
<field name="HoverThrottle" units="%/100" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
<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>
|
||||
@ -32,6 +32,7 @@
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="Running" units="bool" type="enum">
|
||||
@ -65,6 +66,7 @@
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
<options>
|
||||
<option>False</option>
|
||||
@ -102,6 +104,7 @@
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<access gcs="readonly" flight="readwrite"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user