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:
commit
c9188fc5a2
@ -164,6 +164,14 @@ int32_t configuration_check()
|
|||||||
ADDSEVERITY(!coptercontrol);
|
ADDSEVERITY(!coptercontrol);
|
||||||
ADDSEVERITY(navCapableFusion);
|
ADDSEVERITY(navCapableFusion);
|
||||||
break;
|
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:
|
default:
|
||||||
// Uncovered modes are automatically an error
|
// Uncovered modes are automatically an error
|
||||||
ADDSEVERITY(false);
|
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
|
* @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
|
* @see The GNU Public License (GPL) Version 3
|
||||||
*
|
*
|
||||||
@ -342,6 +343,9 @@ static bool okToArm(void)
|
|||||||
|
|
||||||
break;
|
break;
|
||||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||||
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||||
|
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||||
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
|
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
|
||||||
|
@ -11,7 +11,8 @@
|
|||||||
* AttitudeDesired object (stabilized mode)
|
* AttitudeDesired object (stabilized mode)
|
||||||
*
|
*
|
||||||
* @file manualcontrol.c
|
* @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.
|
* @brief ManualControl module. Handles safety R/C link and flight mode.
|
||||||
*
|
*
|
||||||
* @see The GNU Public License (GPL) Version 3
|
* @see The GNU Public License (GPL) Version 3
|
||||||
@ -47,7 +48,7 @@
|
|||||||
#include <stabilizationsettings.h>
|
#include <stabilizationsettings.h>
|
||||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||||
#include <vtolpathfollowersettings.h>
|
#include <vtolpathfollowersettings.h>
|
||||||
#endif
|
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||||
|
|
||||||
// Private constants
|
// Private constants
|
||||||
#if defined(PIOS_MANUAL_STACK_SIZE)
|
#if defined(PIOS_MANUAL_STACK_SIZE)
|
||||||
@ -118,7 +119,7 @@ static void commandUpdatedCb(UAVObjEvent *ev);
|
|||||||
static void manualControlTask(void);
|
static void manualControlTask(void);
|
||||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||||
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
|
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
|
||||||
#endif
|
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||||
|
|
||||||
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
|
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
|
||||||
@ -146,8 +147,8 @@ int32_t ManualControlStart()
|
|||||||
|
|
||||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||||
takeOffLocationHandlerInit();
|
takeOffLocationHandlerInit();
|
||||||
|
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||||
|
|
||||||
#endif
|
|
||||||
// Start main task
|
// Start main task
|
||||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||||
|
|
||||||
@ -174,7 +175,7 @@ int32_t ManualControlInitialize()
|
|||||||
VtolPathFollowerSettingsInitialize();
|
VtolPathFollowerSettingsInitialize();
|
||||||
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
||||||
SystemSettingsConnectCallback(&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);
|
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
@ -202,7 +203,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -214,7 +215,7 @@ static void manualControlTask(void)
|
|||||||
armHandler(false, frameType);
|
armHandler(false, frameType);
|
||||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||||
takeOffLocationHandler();
|
takeOffLocationHandler();
|
||||||
#endif
|
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||||
// Process flight mode
|
// Process flight mode
|
||||||
FlightStatusData flightStatus;
|
FlightStatusData flightStatus;
|
||||||
|
|
||||||
@ -225,7 +226,7 @@ static void manualControlTask(void)
|
|||||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||||
VtolPathFollowerSettingsThrustLimitsData thrustLimits;
|
VtolPathFollowerSettingsThrustLimitsData thrustLimits;
|
||||||
VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
|
VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
|
||||||
#endif
|
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||||
|
|
||||||
FlightModeSettingsData modeSettings;
|
FlightModeSettingsData modeSettings;
|
||||||
FlightModeSettingsGet(&modeSettings);
|
FlightModeSettingsGet(&modeSettings);
|
||||||
@ -273,6 +274,9 @@ static void manualControlTask(void)
|
|||||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
||||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||||
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||||
|
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||||
|
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||||
handler = &handler_STABILIZED;
|
handler = &handler_STABILIZED;
|
||||||
|
|
||||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||||
@ -286,7 +290,6 @@ static void manualControlTask(void)
|
|||||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
if (newFlightModeAssist) {
|
if (newFlightModeAssist) {
|
||||||
// assess roll/pitch state
|
// assess roll/pitch state
|
||||||
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
|
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
|
* 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)
|
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];
|
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
|
||||||
|
|
||||||
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
|
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
|
||||||
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE
|
||||||
flightModeAssistOption = FlightModeAssistMap[position];
|
|| position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
||||||
|
return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (flightModeAssistOption) {
|
switch (FlightModeAssistMap[position]) {
|
||||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
|
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
|
||||||
break;
|
break;
|
||||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST:
|
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_ALTITUDEHOLD:
|
||||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
|
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
|
||||||
// this is only for use with stabi mods with althold/vario.
|
// this is only for use with stabi mods with althold/vario.
|
||||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
||||||
break;
|
|
||||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
|
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
|
||||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
|
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
|
||||||
default:
|
default:
|
||||||
// this is the default for non stabi modes also
|
// this is the default for non stabi modes also
|
||||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
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
|
* @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
|
* @see The GNU Public License (GPL) Version 3
|
||||||
*
|
*
|
||||||
@ -35,6 +36,7 @@
|
|||||||
#include <stabilizationdesired.h>
|
#include <stabilizationdesired.h>
|
||||||
#include <flightmodesettings.h>
|
#include <flightmodesettings.h>
|
||||||
#include <stabilizationbank.h>
|
#include <stabilizationbank.h>
|
||||||
|
#include <flightstatus.h>
|
||||||
|
|
||||||
// Private constants
|
// Private constants
|
||||||
|
|
||||||
@ -48,6 +50,7 @@ static uint8_t currentFpvTiltAngle = 0;
|
|||||||
static float cosAngle = 0.0f;
|
static float cosAngle = 0.0f;
|
||||||
static float sinAngle = 0.0f;
|
static float sinAngle = 0.0f;
|
||||||
|
|
||||||
|
|
||||||
static float applyExpo(float value, float expo)
|
static float applyExpo(float value, float expo)
|
||||||
{
|
{
|
||||||
// note: fastPow makes a small error, therefore result needs to be bound
|
// 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
|
* @input: ManualControlCommand
|
||||||
* @output: StabilizationDesired
|
* @output: StabilizationDesired
|
||||||
*/
|
*/
|
||||||
void stabilizedHandler(bool newinit)
|
void stabilizedHandler(__attribute__((unused)) bool newinit)
|
||||||
{
|
{
|
||||||
if (newinit) {
|
static bool inited = false;
|
||||||
|
|
||||||
|
if (!inited) {
|
||||||
|
inited = true;
|
||||||
StabilizationDesiredInitialize();
|
StabilizationDesiredInitialize();
|
||||||
StabilizationBankInitialize();
|
StabilizationBankInitialize();
|
||||||
}
|
}
|
||||||
|
|
||||||
ManualControlCommandData cmd;
|
ManualControlCommandData cmd;
|
||||||
ManualControlCommandGet(&cmd);
|
ManualControlCommandGet(&cmd);
|
||||||
|
|
||||||
@ -116,7 +123,6 @@ void stabilizedHandler(bool newinit)
|
|||||||
uint8_t *stab_settings;
|
uint8_t *stab_settings;
|
||||||
FlightStatusData flightStatus;
|
FlightStatusData flightStatus;
|
||||||
FlightStatusGet(&flightStatus);
|
FlightStatusGet(&flightStatus);
|
||||||
|
|
||||||
switch (flightStatus.FlightMode) {
|
switch (flightStatus.FlightMode) {
|
||||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||||
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
||||||
@ -136,6 +142,13 @@ void stabilizedHandler(bool newinit)
|
|||||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||||
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
|
||||||
break;
|
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:
|
default:
|
||||||
// Major error, this should not occur because only enter this block when one of these is true
|
// Major error, this should not occur because only enter this block when one of these is true
|
||||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
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_VIRTUALBAR) ? cmd.Roll :
|
||||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
||||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
|
(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
|
0; // this is an invalid mode
|
||||||
|
|
||||||
stabilization.Pitch =
|
stabilization.Pitch =
|
||||||
@ -165,6 +181,9 @@ void stabilizedHandler(bool newinit)
|
|||||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
|
||||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
||||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
|
(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
|
0; // this is an invalid mode
|
||||||
|
|
||||||
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
||||||
@ -187,6 +206,9 @@ void stabilizedHandler(bool newinit)
|
|||||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
|
||||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
||||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
|
(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
|
0; // this is an invalid mode
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -2,7 +2,8 @@
|
|||||||
******************************************************************************
|
******************************************************************************
|
||||||
*
|
*
|
||||||
* @file sequences.h
|
* @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.
|
* @brief Notify module, sequences configuration.
|
||||||
*
|
*
|
||||||
* @see The GNU Public License (GPL) Version 3
|
* @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_POI] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND],
|
[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
|
// List of alarms to show with attached sequences for each status
|
||||||
|
@ -9,7 +9,7 @@
|
|||||||
* @{
|
* @{
|
||||||
*
|
*
|
||||||
* @file innerloop.c
|
* @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.
|
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||||
* @brief Attitude stabilization module.
|
* @brief Attitude stabilization module.
|
||||||
*
|
*
|
||||||
@ -51,6 +51,10 @@
|
|||||||
#include <virtualflybar.h>
|
#include <virtualflybar.h>
|
||||||
#include <cruisecontrol.h>
|
#include <cruisecontrol.h>
|
||||||
#include <sanitycheck.h>
|
#include <sanitycheck.h>
|
||||||
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||||
|
#include <systemidentstate.h>
|
||||||
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||||
|
|
||||||
// Private constants
|
// Private constants
|
||||||
|
|
||||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
|
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
|
||||||
@ -60,6 +64,16 @@
|
|||||||
#define UPDATE_MAX 1.0f
|
#define UPDATE_MAX 1.0f
|
||||||
#define UPDATE_ALPHA 1.0e-2f
|
#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
|
// Private variables
|
||||||
static DelayedCallbackInfo *callbackHandle;
|
static DelayedCallbackInfo *callbackHandle;
|
||||||
static float gyro_filtered[3] = { 0, 0, 0 };
|
static float gyro_filtered[3] = { 0, 0, 0 };
|
||||||
@ -69,6 +83,10 @@ static PiOSDeltatimeConfig timeval;
|
|||||||
static float speedScaleFactor = 1.0f;
|
static float speedScaleFactor = 1.0f;
|
||||||
static bool frame_is_multirotor;
|
static bool frame_is_multirotor;
|
||||||
static bool measuredDterm_enabled;
|
static bool measuredDterm_enabled;
|
||||||
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||||
|
static uint32_t systemIdentTimeVal = 0;
|
||||||
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
static void stabilizationInnerloopTask();
|
static void stabilizationInnerloopTask();
|
||||||
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||||
@ -86,6 +104,9 @@ void stabilizationInnerloopInit()
|
|||||||
ManualControlCommandInitialize();
|
ManualControlCommandInitialize();
|
||||||
StabilizationDesiredInitialize();
|
StabilizationDesiredInitialize();
|
||||||
ActuatorDesiredInitialize();
|
ActuatorDesiredInitialize();
|
||||||
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||||
|
SystemIdentStateInitialize();
|
||||||
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||||
#ifdef REVOLUTION
|
#ifdef REVOLUTION
|
||||||
AirspeedStateInitialize();
|
AirspeedStateInitialize();
|
||||||
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
|
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
|
||||||
@ -100,6 +121,10 @@ void stabilizationInnerloopInit()
|
|||||||
|
|
||||||
frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
|
frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
|
||||||
measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE);
|
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()
|
static float get_pid_scale_source_value()
|
||||||
@ -230,7 +255,6 @@ static void stabilizationInnerloopTask()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
RateDesiredData rateDesired;
|
RateDesiredData rateDesired;
|
||||||
ActuatorDesiredData actuator;
|
ActuatorDesiredData actuator;
|
||||||
StabilizationStatusInnerLoopData enabled;
|
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!
|
// IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
|
||||||
// keep order as it is, RATE must follow!
|
// keep order as it is, RATE must follow!
|
||||||
case STABILIZATIONSTATUS_INNERLOOP_RATE:
|
case STABILIZATIONSTATUS_INNERLOOP_RATE:
|
||||||
|
{
|
||||||
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
|
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
|
||||||
rate[t] = boundf(rate[t],
|
rate[t] = boundf(rate[t],
|
||||||
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
|
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
|
||||||
@ -293,6 +318,7 @@ static void stabilizationInnerloopTask()
|
|||||||
);
|
);
|
||||||
pid_scaler scaler = create_pid_scaler(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] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
case STABILIZATIONSTATUS_INNERLOOP_ACRO:
|
case STABILIZATIONSTATUS_INNERLOOP_ACRO:
|
||||||
{
|
{
|
||||||
@ -312,6 +338,67 @@ static void stabilizationInnerloopTask()
|
|||||||
actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
|
actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
|
||||||
}
|
}
|
||||||
break;
|
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:
|
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
|
||||||
default:
|
default:
|
||||||
actuatorDesiredAxis[t] = rate[t];
|
actuatorDesiredAxis[t] = rate[t];
|
||||||
|
@ -9,7 +9,8 @@
|
|||||||
* @{
|
* @{
|
||||||
*
|
*
|
||||||
* @file stabilization.c
|
* @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.
|
* @brief Attitude stabilization module.
|
||||||
*
|
*
|
||||||
* @see The GNU Public License (GPL) Version 3
|
* @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;
|
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
|
||||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||||
break;
|
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:
|
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||||
|
@ -7,7 +7,8 @@
|
|||||||
* @{
|
* @{
|
||||||
*
|
*
|
||||||
* @file pios_settings.h
|
* @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
|
* @brief Settings functions header
|
||||||
* @see The GNU Public License (GPL) Version 3
|
* @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_GetuSSince(uint32_t t);
|
||||||
extern uint32_t PIOS_DELAY_GetRaw();
|
extern uint32_t PIOS_DELAY_GetRaw();
|
||||||
extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw);
|
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 */
|
#endif /* PIOS_DELAY_H */
|
||||||
|
|
||||||
|
@ -7,7 +7,8 @@
|
|||||||
* @{
|
* @{
|
||||||
*
|
*
|
||||||
* @file pios_delay.c
|
* @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
|
* @brief Delay Functions
|
||||||
* - Provides a micro-second granular delay using the CPU
|
* - Provides a micro-second granular delay using the CPU
|
||||||
* cycle counter.
|
* cycle counter.
|
||||||
@ -168,6 +169,17 @@ uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
|
|||||||
return diff / us_ticks;
|
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 */
|
#endif /* PIOS_INCLUDE_DELAY */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#
|
#
|
||||||
# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||||
#
|
#
|
||||||
# This program is free software; you can redistribute it and/or modify
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -55,6 +55,7 @@ MODULES += Osd/osdoutout
|
|||||||
MODULES += Logging
|
MODULES += Logging
|
||||||
MODULES += Telemetry
|
MODULES += Telemetry
|
||||||
|
|
||||||
|
OPTMODULES += AutoTune
|
||||||
OPTMODULES += ComUsbBridge
|
OPTMODULES += ComUsbBridge
|
||||||
OPTMODULES += UAVOMSPBridge
|
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
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
|||||||
UAVOBJSRCFILENAMES += txpidstatus
|
UAVOBJSRCFILENAMES += txpidstatus
|
||||||
UAVOBJSRCFILENAMES += takeofflocation
|
UAVOBJSRCFILENAMES += takeofflocation
|
||||||
UAVOBJSRCFILENAMES += perfcounter
|
UAVOBJSRCFILENAMES += perfcounter
|
||||||
|
UAVOBJSRCFILENAMES += systemidentsettings
|
||||||
|
UAVOBJSRCFILENAMES += systemidentstate
|
||||||
|
|
||||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#
|
#
|
||||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||||
#
|
#
|
||||||
# This program is free software; you can redistribute it and/or modify
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -52,6 +52,7 @@ MODULES += Logging
|
|||||||
MODULES += Telemetry
|
MODULES += Telemetry
|
||||||
MODULES += Notify
|
MODULES += Notify
|
||||||
|
|
||||||
|
OPTMODULES += AutoTune
|
||||||
OPTMODULES += ComUsbBridge
|
OPTMODULES += ComUsbBridge
|
||||||
OPTMODULES += UAVOMSPBridge
|
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
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
|||||||
UAVOBJSRCFILENAMES += txpidstatus
|
UAVOBJSRCFILENAMES += txpidstatus
|
||||||
UAVOBJSRCFILENAMES += takeofflocation
|
UAVOBJSRCFILENAMES += takeofflocation
|
||||||
UAVOBJSRCFILENAMES += perfcounter
|
UAVOBJSRCFILENAMES += perfcounter
|
||||||
|
UAVOBJSRCFILENAMES += systemidentsettings
|
||||||
|
UAVOBJSRCFILENAMES += systemidentstate
|
||||||
|
|
||||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
#
|
#
|
||||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||||
# Copyright (c) 2009-2014, The OpenPilot Team, http://www.openpilot.org
|
# Copyright (C) 2009-2014, The OpenPilot Team, http://www.openpilot.org
|
||||||
#
|
#
|
||||||
# This program is free software; you can redistribute it and/or modify
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -51,6 +51,7 @@ MODULES += PathFollower
|
|||||||
MODULES += Telemetry
|
MODULES += Telemetry
|
||||||
MODULES += Notify
|
MODULES += Notify
|
||||||
|
|
||||||
|
OPTMODULES += AutoTune
|
||||||
OPTMODULES += ComUsbBridge
|
OPTMODULES += ComUsbBridge
|
||||||
OPTMODULES += UAVOMSPBridge
|
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
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
|||||||
UAVOBJSRCFILENAMES += txpidstatus
|
UAVOBJSRCFILENAMES += txpidstatus
|
||||||
UAVOBJSRCFILENAMES += takeofflocation
|
UAVOBJSRCFILENAMES += takeofflocation
|
||||||
UAVOBJSRCFILENAMES += perfcounter
|
UAVOBJSRCFILENAMES += perfcounter
|
||||||
|
UAVOBJSRCFILENAMES += systemidentsettings
|
||||||
|
UAVOBJSRCFILENAMES += systemidentstate
|
||||||
|
|
||||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#
|
#
|
||||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||||
#
|
#
|
||||||
# This program is free software; you can redistribute it and/or modify
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -52,6 +52,7 @@ MODULES += Telemetry
|
|||||||
|
|
||||||
SRC += $(FLIGHTLIB)/notification.c
|
SRC += $(FLIGHTLIB)/notification.c
|
||||||
|
|
||||||
|
OPTMODULES += AutoTune
|
||||||
OPTMODULES += ComUsbBridge
|
OPTMODULES += ComUsbBridge
|
||||||
OPTMODULES += UAVOMSPBridge
|
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
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -124,6 +125,9 @@ UAVOBJSRCFILENAMES += mpugyroaccelsettings
|
|||||||
UAVOBJSRCFILENAMES += txpidsettings
|
UAVOBJSRCFILENAMES += txpidsettings
|
||||||
UAVOBJSRCFILENAMES += txpidstatus
|
UAVOBJSRCFILENAMES += txpidstatus
|
||||||
UAVOBJSRCFILENAMES += takeofflocation
|
UAVOBJSRCFILENAMES += takeofflocation
|
||||||
|
# UAVOBJSRCFILENAMES += perfcounter
|
||||||
|
UAVOBJSRCFILENAMES += systemidentsettings
|
||||||
|
UAVOBJSRCFILENAMES += systemidentstate
|
||||||
|
|
||||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
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.
|
# 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.
|
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||||
#
|
#
|
||||||
#
|
#
|
||||||
@ -50,6 +50,8 @@ MODULES += Airspeed
|
|||||||
|
|
||||||
SRC += $(FLIGHTLIB)/notification.c
|
SRC += $(FLIGHTLIB)/notification.c
|
||||||
|
|
||||||
|
OPTMODULES += AutoTune
|
||||||
|
|
||||||
# Paths
|
# Paths
|
||||||
OPSYSTEM = .
|
OPSYSTEM = .
|
||||||
BOARDINC = ..
|
BOARDINC = ..
|
||||||
|
@ -3,6 +3,7 @@
|
|||||||
#
|
#
|
||||||
# Makefile for OpenPilot UAVObject code
|
# 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.
|
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
|
||||||
#
|
#
|
||||||
# This program is free software; you can redistribute it and/or modify
|
# This program is free software; you can redistribute it and/or modify
|
||||||
@ -119,6 +120,9 @@ UAVOBJSRCFILENAMES += altitudeholdstatus
|
|||||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||||
UAVOBJSRCFILENAMES += takeofflocation
|
UAVOBJSRCFILENAMES += takeofflocation
|
||||||
|
# UAVOBJSRCFILENAMES += perfcounter
|
||||||
|
UAVOBJSRCFILENAMES += systemidentsettings
|
||||||
|
UAVOBJSRCFILENAMES += systemidentstate
|
||||||
|
|
||||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#
|
#
|
||||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||||
#
|
#
|
||||||
# This program is free software; you can redistribute it and/or modify
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -52,6 +52,7 @@ MODULES += Logging
|
|||||||
MODULES += Telemetry
|
MODULES += Telemetry
|
||||||
MODULES += Notify
|
MODULES += Notify
|
||||||
|
|
||||||
|
OPTMODULES += AutoTune
|
||||||
OPTMODULES += ComUsbBridge
|
OPTMODULES += ComUsbBridge
|
||||||
OPTMODULES += UAVOMSPBridge
|
OPTMODULES += UAVOMSPBridge
|
||||||
|
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
#
|
#
|
||||||
# Copyright (c) 2016, The LibrePilot Project, http://www.librepilot.org
|
# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org
|
||||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||||
#
|
#
|
||||||
# This program is free software; you can redistribute it and/or modify
|
# 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
|
# it under the terms of the GNU General Public License as published by
|
||||||
@ -126,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings
|
|||||||
UAVOBJSRCFILENAMES += txpidstatus
|
UAVOBJSRCFILENAMES += txpidstatus
|
||||||
UAVOBJSRCFILENAMES += takeofflocation
|
UAVOBJSRCFILENAMES += takeofflocation
|
||||||
UAVOBJSRCFILENAMES += perfcounter
|
UAVOBJSRCFILENAMES += perfcounter
|
||||||
|
UAVOBJSRCFILENAMES += systemidentsettings
|
||||||
|
UAVOBJSRCFILENAMES += systemidentstate
|
||||||
|
|
||||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||||
|
@ -137,6 +137,8 @@ UAVOBJS = \
|
|||||||
$${UAVOBJ_XML_DIR}/statusvtolautotakeoff.xml \
|
$${UAVOBJ_XML_DIR}/statusvtolautotakeoff.xml \
|
||||||
$${UAVOBJ_XML_DIR}/statusvtolland.xml \
|
$${UAVOBJ_XML_DIR}/statusvtolland.xml \
|
||||||
$${UAVOBJ_XML_DIR}/systemalarms.xml \
|
$${UAVOBJ_XML_DIR}/systemalarms.xml \
|
||||||
|
$${UAVOBJ_XML_DIR}/systemidentsettings.xml \
|
||||||
|
$${UAVOBJ_XML_DIR}/systemidentstate.xml \
|
||||||
$${UAVOBJ_XML_DIR}/systemsettings.xml \
|
$${UAVOBJ_XML_DIR}/systemsettings.xml \
|
||||||
$${UAVOBJ_XML_DIR}/systemstats.xml \
|
$${UAVOBJ_XML_DIR}/systemstats.xml \
|
||||||
$${UAVOBJ_XML_DIR}/takeofflocation.xml \
|
$${UAVOBJ_XML_DIR}/takeofflocation.xml \
|
||||||
|
@ -7,57 +7,57 @@
|
|||||||
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
|
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
|
||||||
<field name="Stabilization1Settings" units="" type="enum"
|
<field name="Stabilization1Settings" units="" type="enum"
|
||||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
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"
|
defaultvalue="Attitude,Attitude,AxisLock,Manual"
|
||||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||||
/>
|
/>
|
||||||
<field name="Stabilization2Settings" units="" type="enum"
|
<field name="Stabilization2Settings" units="" type="enum"
|
||||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
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"
|
defaultvalue="Attitude,Attitude,Rate,Manual"
|
||||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||||
/>
|
/>
|
||||||
<field name="Stabilization3Settings" units="" type="enum"
|
<field name="Stabilization3Settings" units="" type="enum"
|
||||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
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"
|
defaultvalue="Rate,Rate,Rate,Manual"
|
||||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||||
/>
|
/>
|
||||||
<field name="Stabilization4Settings" units="" type="enum"
|
<field name="Stabilization4Settings" units="" type="enum"
|
||||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
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"
|
defaultvalue="Attitude,Attitude,AxisLock,CruiseControl"
|
||||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||||
/>
|
/>
|
||||||
<field name="Stabilization5Settings" units="" type="enum"
|
<field name="Stabilization5Settings" units="" type="enum"
|
||||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
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"
|
defaultvalue="Attitude,Attitude,Rate,CruiseControl"
|
||||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||||
/>
|
/>
|
||||||
<field name="Stabilization6Settings" units="" type="enum"
|
<field name="Stabilization6Settings" units="" type="enum"
|
||||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
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"
|
defaultvalue="Rate,Rate,Rate,Manual"
|
||||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \
|
||||||
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
|
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \
|
||||||
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
|
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;"
|
||||||
/>
|
/>
|
||||||
|
|
||||||
<!-- Note these options values should be identical to those defined in FlightMode -->
|
<!-- Note these options values should be identical to those defined in FlightMode -->
|
||||||
@ -66,7 +66,7 @@
|
|||||||
units=""
|
units=""
|
||||||
type="enum"
|
type="enum"
|
||||||
elements="6"
|
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"
|
defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6"
|
||||||
limits="%NE:POI:AutoCruise; \
|
limits="%NE:POI:AutoCruise; \
|
||||||
%NE:POI:AutoCruise; \
|
%NE:POI:AutoCruise; \
|
||||||
|
@ -4,7 +4,7 @@
|
|||||||
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
|
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
|
||||||
|
|
||||||
<!-- Note these enumerated values should be the same as ManualControlSettings -->
|
<!-- 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 -->
|
<!-- 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"/>
|
<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_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="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="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="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"
|
<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="Yaw" units="degrees" type="float" elements="1"/>
|
||||||
<field name="Thrust" units="%" type="float" elements="1"/>
|
<field name="Thrust" units="%" type="float" elements="1"/>
|
||||||
<!-- These values should match those in FlightModeSettings.Stabilization{1,2,3}Settings -->
|
<!-- 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"/>
|
<access gcs="readwrite" flight="readwrite"/>
|
||||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||||
|
@ -3,7 +3,7 @@
|
|||||||
<description>Contains status information to control submodules for stabilization.</description>
|
<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>
|
<elementnames>
|
||||||
<elementname>Roll</elementname>
|
<elementname>Roll</elementname>
|
||||||
<elementname>Pitch</elementname>
|
<elementname>Pitch</elementname>
|
||||||
@ -11,7 +11,7 @@
|
|||||||
<elementname>Thrust</elementname>
|
<elementname>Thrust</elementname>
|
||||||
</elementnames>
|
</elementnames>
|
||||||
</field>
|
</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>
|
<elementnames>
|
||||||
<elementname>Roll</elementname>
|
<elementname>Roll</elementname>
|
||||||
<elementname>Pitch</elementname>
|
<elementname>Pitch</elementname>
|
||||||
|
@ -37,6 +37,7 @@
|
|||||||
<option>FlightMode</option>
|
<option>FlightMode</option>
|
||||||
<option>UnsupportedConfig_OneShot</option>
|
<option>UnsupportedConfig_OneShot</option>
|
||||||
<option>BadThrottleOrCollectiveInputRange</option>
|
<option>BadThrottleOrCollectiveInputRange</option>
|
||||||
|
<option>AutoTune</option>
|
||||||
</options>
|
</options>
|
||||||
</field>
|
</field>
|
||||||
<field name="ExtendedAlarmSubStatus" units="" type="uint8" defaultvalue="0">
|
<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>GPS</elementname>
|
||||||
<elementname>OSDGen</elementname>
|
<elementname>OSDGen</elementname>
|
||||||
<elementname>UAVOMSPBridge</elementname>
|
<elementname>UAVOMSPBridge</elementname>
|
||||||
|
<elementname>AutoTune</elementname>
|
||||||
</elementnames>
|
</elementnames>
|
||||||
</field>
|
</field>
|
||||||
<field name="Running" units="bool" type="enum">
|
<field name="Running" units="bool" type="enum">
|
||||||
@ -65,6 +66,7 @@
|
|||||||
<elementname>GPS</elementname>
|
<elementname>GPS</elementname>
|
||||||
<elementname>OSDGen</elementname>
|
<elementname>OSDGen</elementname>
|
||||||
<elementname>UAVOMSPBridge</elementname>
|
<elementname>UAVOMSPBridge</elementname>
|
||||||
|
<elementname>AutoTune</elementname>
|
||||||
</elementnames>
|
</elementnames>
|
||||||
<options>
|
<options>
|
||||||
<option>False</option>
|
<option>False</option>
|
||||||
@ -102,6 +104,7 @@
|
|||||||
<elementname>GPS</elementname>
|
<elementname>GPS</elementname>
|
||||||
<elementname>OSDGen</elementname>
|
<elementname>OSDGen</elementname>
|
||||||
<elementname>UAVOMSPBridge</elementname>
|
<elementname>UAVOMSPBridge</elementname>
|
||||||
|
<elementname>AutoTune</elementname>
|
||||||
</elementnames>
|
</elementnames>
|
||||||
</field>
|
</field>
|
||||||
<access gcs="readonly" flight="readwrite"/>
|
<access gcs="readonly" flight="readwrite"/>
|
||||||
|
Loading…
x
Reference in New Issue
Block a user