rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

LP-76 better yaw-calc yaw by default-yaw overrides-cleanup for review

Browse Source
This commit is contained in:
Cliff Geerdes 2016-03-11 16:27:13 -05:00
parent ab50008e07
commit a217d0015f
15 changed files with 760 additions and 1311 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1108
flight/modules/AutoTune/autotune.c
View File

@ -43,11 +43,11 @@
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "systemident.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"
@ -55,9 +55,6 @@
#include "stabilizationsettingsbank3.h"
#include "accessorydesired.h"
#define PIOS_malloc pios_malloc
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
#define powapprox fastpow
#define expapprox fastexp
@ -66,180 +63,6 @@
#define expapprox expf
#endif /* defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
//#define USE_CIRC_QUEUE
#if defined(USE_CIRC_QUEUE)
/**
******************************************************************************
* @file circqueue.h
* @author dRonin, http://dRonin.org/, Copyright (C) 2015
* @brief Public header for 1 reader, 1 writer circular queue
*****************************************************************************/
typedef struct circ_queue *circ_queue_t;
circ_queue_t circ_queue_new(uint16_t elem_size, uint16_t num_elem);
void *circ_queue_cur_write_pos(circ_queue_t q);
int circ_queue_advance_write(circ_queue_t q);
void *circ_queue_read_pos(circ_queue_t q);
void circ_queue_read_completed(circ_queue_t q);
/**
******************************************************************************
* @file circqueue.c
* @author dRonin, http://dRonin.org/, Copyright (C) 2015
* @brief Implements a 1 reader, 1 writer nonblocking circular queue
*****************************************************************************/
/*
* 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 <circqueue.h>
struct circ_queue {
uint16_t elem_size; /**< Element size in octets */
uint16_t num_elem; /**< Number of elements in circqueue (capacity+1) */
volatile uint16_t write_head; /**< Element position writer is at */
volatile uint16_t read_tail; /**< Element position reader is at */
/* head == tail: empty.
* head == tail-1: full.
*/
/* This is declared as a uint32_t for alignment reasons. */
uint32_t contents[]; /**< Contents of the circular queue */
};
/** Allocate a new circular queue.
* @param[in] elem_size The size of each element, as obtained from sizeof().
* @param[in] num_elem The number of elements in the queue. The capacity is
* one less than this (it may not be completely filled).
* @returns The handle to the circular queue.
*/
circ_queue_t circ_queue_new(uint16_t elem_size, uint16_t num_elem) {
PIOS_Assert(elem_size > 0);
PIOS_Assert(num_elem > 2);
uint32_t size = elem_size * num_elem;
/* PIOS_malloc_no_dma may not be safe for some later uses.. hmmm */
struct circ_queue *ret = PIOS_malloc(sizeof(*ret) + size);
memset(ret, 0, sizeof(*ret) + size);
CSS pixel
ret->elem_size = elem_size;
ret->num_elem = num_elem;
return ret;
}
/** Get a pointer to the current queue write position.
* This position is unavailable to any present readers and may be filled in
* with the desired data without respect to any synchronization.
*
* @param[in] q Handle to circular queue.
* @returns The position for new data to be written to (of size elem_size).
*/
void *circ_queue_cur_write_pos(circ_queue_t q) {
void *contents = q->contents;
return contents + q->write_head * q->elem_size;
}
static inline uint16_t next_pos(uint16_t num_pos, uint16_t current_pos) {
PIOS_Assert(current_pos < num_pos);
current_pos++;
/* Also save on uint16_t wrap */
if (current_pos >= num_pos) {
current_pos = 0;
}
return current_pos;
}
/** Makes the current block of data available to readers and advances write pos.
* This may fail if the queue contain num_elems -1 elements, in which case the
* advance may be retried in the future. In this case, data already written to
* write_pos is preserved and the advance may be retried (or overwritten with
* new data).
*
* @param[in] q Handle to circular queue.
* @returns 0 if the write succeeded, nonzero on error.
*/
int circ_queue_advance_write(circ_queue_t q) {
uint16_t new_write_head = next_pos(q->num_elem, q->write_head);
/* the head is not allowed to advance to meet the tail */
if (new_write_head == q->read_tail) {
return -1; /* Full */
/* Caller can either let the data go away, or try again to
* advance later */
}
q->write_head = new_write_head;
return 0;
}
/** Returns a block of data to the reader.
* The block is "claimed" until released with circ_queue_read_completed.
* No new data is available until that call is made (instead the same
* block-in-progress will be returned).
*
* @param[in] q Handle to circular queue.
* @returns pointer to the data, or NULL if the queue is empty.
*/
void *circ_queue_read_pos(circ_queue_t q) {
uint16_t read_tail = q->read_tail;
void *contents = q->contents;
if (q->write_head == read_tail) {
/* There is nothing new to read. */
return NULL;
}
return contents + q->read_tail * q->elem_size;
}
/** Releases a block of read data obtained by circ_queue_read_pos.
* Behavior is undefined if circ_queue_read_pos did not previously return
* a block of data.
*
* @param[in] q Handle to the circular queue.
*/
void circ_queue_read_completed(circ_queue_t q) {
/* Avoid multiple accesses to a volatile */
uint16_t read_tail = q->read_tail;
/* If this is being called, the queue had better not be empty--
* we're supposed to finish consuming this element after a prior call
* to circ_queue_read_pos.
*/
PIOS_Assert(read_tail != q->write_head);
q->read_tail = next_pos(q->num_elem, read_tail);
}
#endif
// Private constants
#undef STACK_SIZE_BYTES
@ -256,24 +79,25 @@ void circ_queue_read_completed(circ_queue_t q) {
#define AT_QUEUE_NUMELEM 18
#endif
#define MAX_PTS_PER_CYCLE 4
//#define START_TIME_DELAY_MS 2000
#define INIT_TIME_DELAY_MS 2000
#define YIELD_MS 2
#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 */
#if defined(USE_CIRC_QUEUE)
#define DEREFERENCE(str, ele) (str->ele)
#else
#define DEREFERENCE(str, ele) (str.ele)
#endif
#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
#define SMOOTH_QUICK_DISABLED 0
#define SMOOTH_QUICK_AUX_BASE 10
#define SMOOTH_QUICK_TOGGLE_BASE 21
#define SMOOTH_QUICK_DISABLED 0
#define SMOOTH_QUICK_ACCESSORY_BASE 10
#define SMOOTH_QUICK_TOGGLE_BASE 21
// Private types <access gcs="readwrite" flight="readwrite"/>
enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_START, AT_RUN, AT_FINISHED, AT_WAITING };
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 */
@ -285,22 +109,17 @@ struct at_queued_data {
// Private variables
//static struct pios_thread *taskHandle;
static xTaskHandle taskHandle;
static bool moduleEnabled;
#if defined(USE_CIRC_QUEUE)
static circ_queue_t atQueue;
#else
static xQueueHandle atQueue;
#endif
static volatile uint32_t atPointsSpilled;
static uint32_t throttleAccumulator;
static uint8_t rollMax, pitchMax;
static StabilizationBankManualRateData manualRate;
//static SystemSettingsAirframeTypeOptions airframeType;
static float gX[AF_NUMX] = {0};
static float gP[AF_NUMP] = {0};
SystemIdentData systemIdentData;
SystemIdentSettingsData systemIdentSettings;
SystemIdentStateData systemIdentState;
int8_t accessoryToUse;
int8_t flightModeSwitchTogglePosition;
@ -309,12 +128,13 @@ int8_t flightModeSwitchTogglePosition;
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 UpdateSystemIdent(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle);
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);
@ -340,12 +160,9 @@ int32_t AutoTuneInitialize(void)
#endif
if (moduleEnabled) {
SystemIdentInitialize();
#if defined(USE_CIRC_QUEUE)
atQueue = circ_queue_new(sizeof(struct at_queued_data), AT_QUEUE_NUMELEM);
#else
SystemIdentSettingsInitialize();
SystemIdentStateInitialize();
atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data));
#endif
if (!atQueue) {
moduleEnabled = false;
}
@ -385,18 +202,19 @@ 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};
// is this needed?
// AfInit(gX,gP);
uint32_t lastTime = 0.0f;
bool saveSiNeeded = false;
bool savePidNeeded = false;
// should this be put in Init()?
// GyroSensorConnectCallback(AtNewGyroData);
// 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 a following flight
// so that the user can use the PID smooth->quick slider in following flights
InitSystemIdent(false);
while (1) {
@ -405,25 +223,22 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
uint32_t measureTime = 60000;
bool doingIdent = false;
bool canSleep = true;
//PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// can't restart till after you save that's OK I guess
// but you should be able to stop in mid tune and restart from beginning
// maybe reset state in that fn that gets called on mode change
// I have never seen this module misbehave so not bothering making a watchdog
//PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
if (saveSiNeeded) {
saveSiNeeded = false;
// Save SystemIdent to permanent settings
UAVObjSave(SystemIdentHandle(), 0);
//so how to restart if it failed and both saves are false
// Save SystemIdentSettings to permanent settings
UAVObjSave(SystemIdentSettingsHandle(), 0);
}
if (savePidNeeded) {
savePidNeeded = false;
// Save PIDs to permanent settings
switch (systemIdentData.DestinationPidBank) {
switch (systemIdentSettings.DestinationPidBank) {
case 1:
UAVObjSave(StabilizationSettingsBank1Handle(), 0);
break;
@ -435,42 +250,49 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
break;
}
}
// can't set to AT_INIT because when we land and disarm it will jump to init and clear things out after 2 seconds
//state = AT_INIT;
}
// if using flight mode switch quick toggle to "try smooth -> quick PIDs" is enabled
// and user toggled into and back out of AutoTune
// three times in the last two seconds
// CheckFlightModeSwitchForPidRequest(mode) only returns true if mode is not autotune
if (flightModeSwitchTogglePosition!=-1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentData.Complete && !CheckSettings()) {
// and the data gathering is complete
// and the data gathered is good
// note: CheckFlightModeSwitchForPidRequest(mode) only returns true if 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 3,4,5,1,2 or 2,3,1
// if user toggled while armed set PID's to next in sequence 2,3,4,0,1... or 1,2,0...
// if smoothest is -100 and quickest is +100 this corresponds to 0,+50,+100,-100,-50... or 0,+100,-100
++flightModeSwitchTogglePosition;
if (flightModeSwitchTogglePosition > systemIdentData.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) {
if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) {
flightModeSwitchTogglePosition = 0;
}
} else {
// if they did it disarmed, then set PID's back to AT default
flightModeSwitchTogglePosition = (systemIdentData.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2;
// if they did it disarmed, then set PID's back to AutoTune default
flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2;
}
ProportionPidsSmoothToQuick(0.0f, (float) flightModeSwitchTogglePosition, (float) (systemIdentData.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE));
ProportionPidsSmoothToQuick(0.0f,
(float) flightModeSwitchTogglePosition,
(float) (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE));
savePidNeeded = true;
}
// 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) {
// if accessory0-3 is configured as a PID vario over the smooth to quick range
// 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/900 of full range
// 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 && CheckAccessoryForPidRequest(accessoryToUse)) {
if (accessoryToUse != -1 && systemIdentData.Complete && !CheckSettings()) {
// (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 };
// static float accessoryValueOld = 0.0f;
AccessoryDesiredData accessoryValue;
// float accessoryValue;
AccessoryDesiredInstGet(accessoryToUse, &accessoryValue);
// if the accessory changed more than 1/900
// (this test is intended to remove one unit jitter)
if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (1.0f/900.0f)) {
accessoryValueOld = accessoryValue;
ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f);
@ -486,155 +308,157 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
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
// that must wait until after a delay has passed to make sure they intended to stay in this mode
// is this a race? is it possible that flightStatus.FlightMode has been changed, but the stab bank hasn't been changed yet?
StabilizationBankRollMaxGet(&rollMax);
StabilizationBankPitchMaxGet(&pitchMax);
StabilizationBankManualRateGet(&manualRate);
// 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;
// Spend the first block of time in normal rate mode to get stabilized
// after a small delay, get the stab bank values and SystemIdentSettings in case they changed
// this is a very small delay, so fms toggle gets in here
if (diffTime > INIT_TIME_DELAY_MS) {
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
// SystemSettingsAirframeTypeGet(&airframeType);
// Reset save status; only save if this tune completes.
saveSiNeeded = false;
savePidNeeded = false;
// systemIdentData.Complete = false;
//// lastUpdateTime = xTaskGetTickCount();
InitSystemIdent(true);
AfInit(gX, gP);
UpdateSystemIdent(gX, NULL, 0.0f, 0, 0, 0.0f);
measureTime = (uint32_t)systemIdentData.TuningDuration * (uint32_t)1000;
state = AT_START;
#if 0
lastUpdateTime = xTaskGetTickCount();
#endif
// 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);
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".
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) {
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
// 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);
AfInit(gX, gP);
UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f);
measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
state = AT_START;
}
}
//// lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_START:
#if 0
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Spend the first block of time in normal rate mode to get stabilized
if (diffTime > START_TIME_DELAY_MS) {
#else
{
#endif
lastTime = PIOS_DELAY_GetRaw();
/* Drain the queue of all current data */
#if defined(USE_CIRC_QUEUE)
while (circ_queue_read_pos(atQueue)) {
circ_queue_read_completed(atQueue);
}
#else
xQueueReset(atQueue);
#endif
/* And reset the point spill counter */
updateCounter = 0;
atPointsSpilled = 0;
throttleAccumulator = 0;
state = AT_RUN;
lastUpdateTime = xTaskGetTickCount();
}
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++) {
#if defined(USE_CIRC_QUEUE)
struct at_queued_data *pt;
/* Grab an autotune point */
pt = circ_queue_read_pos(atQueue);
if (!pt) {
#else
struct at_queued_data pt;
/* Grab an autotune point */
if (xQueueReceive(atQueue, &pt, 0) != pdTRUE) {
#endif
/* We've drained the buffer fully */
canSleep = true;
break;
}
/* calculate time between successive points */
float dT_s = PIOS_DELAY_DiffuS2(lastTime,
DEREFERENCE(pt,raw_time)) * 1.0e-6f;
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 = DEREFERENCE(pt,raw_time);
AfPredict(gX, gP, DEREFERENCE(pt,u), DEREFERENCE(pt,y), dT_s, DEREFERENCE(pt,throttle));
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) * (DEREFERENCE(pt,y[j]) - gX[j]) * (DEREFERENCE(pt,y[j]) - gX[j]);
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 * DEREFERENCE(pt,throttle);
// 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)) {
if (((updateCounter++) & 0xff) == 0) {
float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f;
UpdateSystemIdent(gX, noise, dT_s, updateCounter, atPointsSpilled, hover_throttle);
UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hover_throttle);
}
#if defined(USE_CIRC_QUEUE)
/* Free the buffer containing an AT point */
circ_queue_read_completed(atQueue);
#endif
}
if (diffTime > measureTime) { // Move on to next state
// permanent flag that AT is complete and PIDs can be calculated
state = AT_FINISHED;
// lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_FINISHED:
;
float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f;
uint8_t failureBits;
UpdateSystemIdent(gX, noise, 0, updateCounter, atPointsSpilled, hover_throttle);
saveSiNeeded = true;
// data is bad if FC was disarmed at the time AT completed
// data is automatically considered bad if FC was disarmed at the time AT completed
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
failureBits = CheckSettings();
if (!failureBits) {
// always calculate and save PIDs if disabling sanity checks
if (!CheckSettings()) {
ComputeStabilizationAndSetPids();
savePidNeeded = true;
systemIdentData.Complete = true;
} else {
//is this right
// default to disable PID changing with flight mode switch and accessory0-3
accessoryToUse = -1;
flightModeSwitchTogglePosition = -1;
// systemIdentData.Complete = false;
// raise a warning
// 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 hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f;
UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hover_throttle);
SystemIdentSettingsSet(&systemIdentSettings);
state = AT_WAITING;
break;
case AT_WAITING:
default:
// maybe set an alarm to notify user that tuning is done
// after tuning, wait here till user switches to another flight mode
// or disarms
break;
}
// Update based on ManualControlCommand
// fly in Attitude mode or in SystemIdent mode
UpdateStabilizationDesired(doingIdent);
if (canSleep) {
vTaskDelay(YIELD_MS / portTICK_RATE_MS);
}
@ -642,12 +466,11 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
}
// 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) {
#if defined(USE_CIRC_QUEUE)
struct at_queued_data *q_item;
#else
static struct at_queued_data q_item;
#endif
static bool last_sample_unpushed = false;
GyroSensorData gyro;
ActuatorDesiredData actuators;
@ -656,40 +479,29 @@ static void AtNewGyroData(UAVObjEvent * ev) {
return;
}
// object can and possibly will at times change asynchronously so must copy data here, with locking
// object will at times change asynchronously so must copy data here, with locking
// and do it as soon as possible
GyroSensorGet(&gyro);
ActuatorDesiredGet(&actuators);
if (last_sample_unpushed) {
/* Last time we were unable to advance the write pointer.
/* Last time we were unable to queue up the gyro data.
* Try again, last chance! */
#if defined(USE_CIRC_QUEUE)
if (circ_queue_advance_write(atQueue)) {
#else
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
#endif
atPointsSpilled++;
}
}
#if defined(USE_CIRC_QUEUE)
q_item = circ_queue_cur_write_pos(atQueue);
#endif
DEREFERENCE(q_item,raw_time) = PIOS_DELAY_GetRaw();
DEREFERENCE(q_item,y[0]) = gyro.x;
DEREFERENCE(q_item,y[1]) = gyro.y;
DEREFERENCE(q_item,y[2]) = gyro.z;
DEREFERENCE(q_item,u[0]) = actuators.Roll;
DEREFERENCE(q_item,u[1]) = actuators.Pitch;
DEREFERENCE(q_item,u[2]) = actuators.Yaw;
DEREFERENCE(q_item,throttle) = actuators.Thrust;
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 defined(USE_CIRC_QUEUE)
if (circ_queue_advance_write(atQueue) != 0) {
#else
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
#endif
last_sample_unpushed = true;
} else {
last_sample_unpushed = false;
@ -697,6 +509,11 @@ static void AtNewGyroData(UAVObjEvent * ev) {
}
// 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;
@ -726,103 +543,100 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) {
}
// 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) {
SystemIdentGet(&systemIdentData);
uint8_t smoothQuick = systemIdentData.SmoothQuick;
SystemIdentSettingsGet(&systemIdentSettings);
uint8_t smoothQuick = systemIdentSettings.SmoothQuick;
if (loadDefaults) {
// these are values that could be changed by the user
// save them through the following xSetDefaults() call
uint8_t dampMin = systemIdentData.DampMin;
uint8_t dampRate = systemIdentData.DampRate;
uint8_t dampMax = systemIdentData.DampMax;
uint8_t noiseMin = systemIdentData.NoiseMin;
uint8_t noiseRate = systemIdentData.NoiseRate;
uint8_t noiseMax = systemIdentData.NoiseMax;
bool calcYaw = systemIdentData.CalculateYaw;
uint8_t destBank = systemIdentData.DestinationPidBank;
uint8_t tuningDuration = systemIdentData.TuningDuration;
// bool complete = systemIdentData.Complete;
// 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
SystemIdentSetDefaults(SystemIdentHandle(), 0);
SystemIdentGet(&systemIdentData);
// restore the user changeable values
systemIdentData.DampMin = dampMin;
systemIdentData.DampRate = dampRate;
systemIdentData.DampMax = dampMax;
systemIdentData.NoiseMin = noiseMin;
systemIdentData.NoiseRate = noiseRate;
systemIdentData.NoiseMax = noiseMax;
systemIdentData.CalculateYaw = calcYaw;
systemIdentData.DestinationPidBank = destBank;
systemIdentData.TuningDuration = tuningDuration;
// systemIdentData.Complete = complete;
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;
}
// default to disable PID changing with flight mode switch and accessory0-3
accessoryToUse = -1;
flightModeSwitchTogglePosition = -1;
systemIdentData.SmoothQuick = 0;
systemIdentSettings.SmoothQuick = SMOOTH_QUICK_DISABLED;
switch (smoothQuick) {
case 10: // use accessory0
case 11: // use accessory1
case 12: // use accessory2
case 13: // use accessory3
accessoryToUse = smoothQuick - 10;
systemIdentData.SmoothQuick = smoothQuick;
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
accessoryToUse = smoothQuick - SMOOTH_QUICK_ACCESSORY_BASE;
systemIdentSettings.SmoothQuick = smoothQuick;
break;
case 23: // use flight mode switch toggle with 3 points
case 25: // use flight mode switch toggle with 5 points
case SMOOTH_QUICK_TOGGLE_BASE+2: // use flight mode switch toggle with 3 points
case SMOOTH_QUICK_TOGGLE_BASE+4: // use flight mode switch toggle with 5 points
// first test PID is in the middle of the smooth -> quick range
flightModeSwitchTogglePosition = (smoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2;
systemIdentData.SmoothQuick = smoothQuick;
systemIdentSettings.SmoothQuick = smoothQuick;
break;
}
}
static void UpdateSystemIdent(const float *X, const float *noise,
// 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) {
systemIdentData.Beta.Roll = X[6];
systemIdentData.Beta.Pitch = X[7];
systemIdentData.Beta.Yaw = X[8];
systemIdentData.Bias.Roll = X[10];
systemIdentData.Bias.Pitch = X[11];
systemIdentData.Bias.Yaw = X[12];
systemIdentData.Tau = X[9];
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) {
systemIdentData.Noise.Roll = noise[0];
systemIdentData.Noise.Pitch = noise[1];
systemIdentData.Noise.Yaw = noise[2];
systemIdentState.Noise.Roll = noise[0];
systemIdentState.Noise.Pitch = noise[1];
systemIdentState.Noise.Yaw = noise[2];
}
systemIdentData.Period = dT_s * 1000.0f;
systemIdentState.Period = dT_s * 1000.0f;
systemIdentData.NumAfPredicts = predicts;
systemIdentData.NumSpilledPts = spills;
systemIdentState.NumAfPredicts = predicts;
systemIdentState.NumSpilledPts = spills;
systemIdentData.HoverThrottle = hover_throttle;
systemIdentState.HoverThrottle = hover_throttle;
SystemIdentSet(&systemIdentData);
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;
// unneeded since we are setting everything in this uavo
//StabilizationDesiredGet(&stabDesired);
ManualControlCommandData manualControlCommand;
ManualControlCommandGet(&manualControlCommand);
ManualControlCommandData manual_control_command;
ManualControlCommandGet(&manual_control_command);
stabDesired.Roll = manual_control_command.Roll * rollMax;
stabDesired.Pitch = manual_control_command.Pitch * pitchMax;
stabDesired.Yaw = manual_control_command.Yaw * manualRate.Yaw;
//stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle;
stabDesired.Thrust = manual_control_command.Thrust;
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;
@ -835,47 +649,67 @@ static void UpdateStabilizationDesired(bool doingIdent) {
}
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
// is this a race
// control feels very sluggish too
StabilizationDesiredSet(&stabDesired);
}
static uint8_t CheckSettings()
// 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;
#if 1
// Check the axis gains
// Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values.
if (systemIdentData.Beta.Roll < 6) {
retVal |= 1;
if (systemIdentState.Beta.Roll < 6) {
retVal |= ROLL_BETA_LOW;
}
if (systemIdentData.Beta.Pitch < 6) {
retVal |= 2;
if (systemIdentState.Beta.Pitch < 6) {
retVal |= PITCH_BETA_LOW;
}
if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) {
retVal |= YAW_BETA_LOW;
}
// Check the response speed
// Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values.
if (expf(systemIdentData.Tau) > 0.1f) {
retVal |= 4;
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(systemIdentData.Tau) < 0.008f) {
retVal |= 8;
else if (expf(systemIdentState.Tau) < 0.008f) {
retVal |= TAU_TOO_SHORT;
}
#endif
return retVal;
}
#if 0
// 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 == YAW_BETA_LOW) && (!systemIdentSettings.CalculateYaw || systemIdentSettings.OverrideYawBeta))) {
retVal = 0;
}
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)
{
StabilizationSettingsBank1Data stabSettingsBank;
switch (systemIdentData.DestinationPidBank) {
switch (systemIdentSettings.DestinationPidBank) {
case 1:
StabilizationSettingsBank1Get((void *)&stabSettingsBank);
break;
@ -897,26 +731,15 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
const double ghf = (double) noiseRate / 1000.0d;
const double damp = (double) dampRate / 100.0d;
double tau = exp(systemIdentData.Tau);
#if 0
double beta_roll = systemIdentData.Beta.Roll;
double beta_pitch = systemIdentData.Beta.Pitch;
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)*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)*ghf);
#else
double exp_beta_roll_times_ghf = exp(systemIdentData.Beta.Roll)*ghf;
double exp_beta_pitch_times_ghf = exp(systemIdentData.Beta.Pitch)*ghf;
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);
#endif
// 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);
@ -934,10 +757,15 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
const double zeta_o = 1.3d;
const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d/wn);
// For now just run over roll and pitch
int axes = ((systemIdentData.CalculateYaw) ? 3 : 2);
for (int i = 0; i < axes; i++) {
double beta = exp(SystemIdentBetaToArray(systemIdentData.Beta)[i]);
// dRonin simply uses default PID settings for yaw
for (int i = 0; i < ((systemIdentSettings.CalculateYaw) ? 3 : 2); i++) {
double beta;
// if yaw axis and yaw beta is too low and user wants to override it if too low
if (i == 2 && systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin && systemIdentSettings.OverrideYawBeta) {
beta = exp(systemIdentSettings.YawBetaMin);
} else {
beta = exp(SystemIdentStateBetaToArray(systemIdentState.Beta)[i]);
}
double ki = a * b * wn * wn * tau * tau_d / beta;
double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d;
@ -958,7 +786,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
stabSettingsBank.PitchPI.Kp = kp_o;
stabSettingsBank.PitchPI.Ki = 0;
break;
case 2: // optional Yaw
case 2: // Yaw
stabSettingsBank.YawRatePID.Kp = kp;
stabSettingsBank.YawRatePID.Ki = ki;
stabSettingsBank.YawRatePID.Kd = kd;
@ -968,10 +796,11 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
}
}
//stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d);
// Librepilot might do something with this some time
// stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d);
// Save PIDs to permanent settings
switch (systemIdentData.DestinationPidBank) {
// Save PIDs to UAVO RAM (not permanently yet)
switch (systemIdentSettings.DestinationPidBank) {
case 1:
StabilizationSettingsBank1Set((void *)&stabSettingsBank);
break;
@ -983,154 +812,16 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
break;
}
}
#else
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate)
{
StabilizationSettingsBank1Data stabSettingsBank;
#if 0
systemIdentData.Bias.Roll = dampRate;
systemIdentData.Bias.Pitch = noiseRate;
SystemIdentSet(&systemIdentData);
#endif
switch (systemIdentData.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(systemIdentData.Tau);
{
double exp_beta_roll_times_ghf = exp(systemIdentData.Beta.Roll)*ghf;
double exp_beta_pitch_times_ghf = exp(systemIdentData.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);
for (int i = 0; i < 2; i++) {
double beta = exp(SystemIdentBetaToArray(systemIdentData.Beta)[i]);
double ki = a * b * wn * wn * tau * tau_d / beta;
double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d;
double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d;
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 = 0;
break;
case 1: // Pitch
stabSettingsBank.PitchRatePID.Kp = kp;
stabSettingsBank.PitchRatePID.Ki = ki;
stabSettingsBank.PitchRatePID.Kd = kd;
stabSettingsBank.PitchPI.Kp = kp_o;
stabSettingsBank.PitchPI.Ki = 0;
break;
}
}
}
// do yaw if requested
if (systemIdentData.CalculateYaw) {
double exp_beta_yaw_times_ghf = exp(systemIdentData.Beta.Yaw)*ghf;
double wn = 1.0d/tau;
double tau_d = 0.0d;
for (int i = 0; i < 30; i++) {
tau_d = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp_beta_yaw_times_ghf);
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);
double beta = exp(systemIdentData.Beta.Yaw);
double ki = a * b * wn * wn * tau * tau_d / beta;
double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d;
double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d;
// optional Yaw
stabSettingsBank.YawRatePID.Kp = kp;
stabSettingsBank.YawRatePID.Ki = ki;
stabSettingsBank.YawRatePID.Kd = kd;
stabSettingsBank.YawPI.Kp = kp_o;
stabSettingsBank.YawPI.Ki = 0;
}
//stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d);
// Save PIDs to permanent settings
switch (systemIdentData.DestinationPidBank) {
case 1:
StabilizationSettingsBank1Set((void *)&stabSettingsBank);
break;
case 2:
StabilizationSettingsBank2Set((void *)&stabSettingsBank);
break;
case 3:
StabilizationSettingsBank3Set((void *)&stabSettingsBank);
break;
}
}
#endif
// calculate PIDs using default smooth-quick settings
static void ComputeStabilizationAndSetPids()
{
ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentData.DampRate, systemIdentData.NoiseRate);
ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentSettings.DampRate, systemIdentSettings.NoiseRate);
}
// scale damp and noise to generate PIDs according to how a slider or other ratio is set
// 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
@ -1141,12 +832,6 @@ static void ComputeStabilizationAndSetPids()
//
// 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
#define dampMin systemIdentData.DampMin
#define dampDefault systemIdentData.DampRate
#define dampMax systemIdentData.DampMax
#define noiseMin systemIdentData.NoiseMin
#define noiseDefault systemIdentData.NoiseRate
#define noiseMax systemIdentData.NoiseMax
static void ProportionPidsSmoothToQuick(float min, float val, float max)
{
float ratio, damp, noise;
@ -1160,13 +845,13 @@ static void ProportionPidsSmoothToQuick(float min, float val, float max)
if (ratio <= 0.5f) {
// scale ratio in [0,0.5] to produce PIDs in [smoothest,default]
ratio *= 2.0f;
damp = (dampMax * (1.0f - ratio)) + (dampDefault * ratio);
noise = (noiseMin * (1.0f - ratio)) + (noiseDefault * ratio);
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 = (dampDefault * (1.0f - ratio)) + (dampMin * ratio);
noise = (noiseDefault * (1.0f - ratio)) + (noiseMax * ratio);
damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio);
noise = (systemIdentSettings.NoiseRate * (1.0f - ratio)) + (systemIdentSettings.NoiseMax * ratio);
}
ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise);
@ -1204,7 +889,7 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
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 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
@ -1248,16 +933,186 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
const float Ts_e_tau2 = (Ts + e_tau) * (Ts + e_tau);
const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2;
#if 0
// 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[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;
#endif
// 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];
@ -1396,45 +1251,20 @@ static void AfInit(float X[AF_NUMX], float P[AF_NUMP])
// 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; // medium amount of strength
// X[8] = 7.0f; // yaw
// 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
// lets not set SystemIdent to defaults at all
// that way the user can run it a second time, with initial values from the first run
#if 0
// these are values that could be changed by the user
// save them through the following xSetDefaults() call
uint8_t damp = systemIdentData.DampRate;
uint8_t noise = systemIdentData.NoiseRate;
bool yaw = systemIdentData.CalculateYaw;
uint8_t bank = systemIdentData.DestinationPidBank;
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
memset(X, 0, AF_NUMX*sizeof(X[0]));
SystemIdentSetDefaults(SystemIdentHandle(), 0);
SystemIdentBetaArrayGet(&X[6]);
SystemIdentTauGet(&X[9]);
// restore the user changeable values
systemIdentData.DampRate = damp;
systemIdentData.NoiseRate = noise;
systemIdentData.CalculateYaw = yaw;
systemIdentData.DestinationPidBank = bank;
#else
// 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
memset(X, 0, AF_NUMX*sizeof(X[0]));
//SystemIdentSetDefaults(SystemIdentHandle(), 0);
//SystemIdentBetaArrayGet(&X[6]);
memcpy(&X[6], &systemIdentData.Beta, sizeof(systemIdentData.Beta));
memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta));
//SystemIdentTauGet(&X[9]);
X[9] = systemIdentData.Tau;
#endif
X[9] = systemIdentState.Tau;
// P initialization
// Could zero this like: *P = *((float [AF_NUMP]){});
memset(P, 0, AF_NUMP*sizeof(P[0]));
P[0] = qInit[0];
P[1] = qInit[1];

337
flight/modules/Autotune/autotune.c
View File

@ -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;
}
}
/**
* @}
* @}
*/

37
flight/modules/Autotune/inc/autotune.h
View File

@ -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

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

@ -47,7 +47,6 @@
#include <stabilizationsettings.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <vtolpathfollowersettings.h>
#include <systemident.h>
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
// Private constants
@ -119,9 +118,6 @@ static void commandUpdatedCb(UAVObjEvent *ev);
static void manualControlTask(void);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
#if 0
static bool isSystemIdentFlightMode(uint8_t flightMode, FlightModeSettingsData *modeSettings);
#endif
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
static void SettingsUpdatedCb(UAVObjEvent *ev);
@ -255,13 +251,6 @@ static void manualControlTask(void)
// check the flightmodeassist state.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
#if 0
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
if (isSystemIdentFlightMode(newMode, &modeSettings)) {
SystemIdentInitData();
}
#endif
#endif
}
// Depending on the mode update the Stabilization or Actuator objects
@ -623,52 +612,6 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
//return isAssistedFlag;
return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
}
#if 0
/**
* Check if this flight mode uses SystemIdent stabilization mode
*/
static bool isSystemIdentFlightMode(uint8_t flightMode, FlightModeSettingsData *modeSettings)
{
#if 0
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
return true;
}
if (flightMode >= FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 && flightMode <= FLIGHTSTATUS_FLIGHTMODE_STABILIZED6) {
if (modeSettings->Stabilization1Settings.Roll == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT
|| modeSettings->Stabilization1Settings.Pitch == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT
|| modeSettings->Stabilization1Settings.Yaw == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) {
return true;
}
}
return false;
#else
if (
flightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE && (
(
flightMode < FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 || flightMode > FLIGHTSTATUS_FLIGHTMODE_STABILIZED6
) || (
modeSettings->Stabilization1Settings.Roll != FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT
&& modeSettings->Stabilization1Settings.Pitch != FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT
&& modeSettings->Stabilization1Settings.Yaw != FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT
)
)
) {
return false;
}
#if 1
static bool inited=false;
if (!inited) {
inited = true;
#else
if (!SystemIdentHandle()) {
#endif /* 1 */
SystemIdentInitialize();
}
return true;
#endif /* 0 */
}
#endif /* 0 */
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
/**

29
flight/modules/ManualControl/stabilizedhandler.c
View File

@ -36,7 +36,6 @@
#include <flightmodesettings.h>
#include <stabilizationbank.h>
#include <flightstatus.h>
#include <systemident.h>
// Private constants
@ -49,24 +48,7 @@ static float applyExpo(float value, float expo);
static uint8_t currentFpvTiltAngle = 0;
static float cosAngle = 0.0f;
static float sinAngle = 0.0f;
#if 0
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
static FlightModeSettingsStabilization1SettingsData autotuneSettings = {
.Roll = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
.Pitch = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
.Yaw = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
.Thrust = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
};
#if 0
static FlightModeSettingsStabilization1SettingsData attitudeSettings = {
.Roll = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE,
.Pitch = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE,
.Yaw = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE,
.Thrust = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
};
#endif
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
#endif
static float applyExpo(float value, float expo)
{
@ -160,18 +142,9 @@ void stabilizedHandler(__attribute__((unused)) bool newinit)
break;
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
#if 0
// if (SystemIdentHandle()) {
stab_settings = (uint8_t *)&autotuneSettings;
// } else {
// stab_settings = (uint8_t *)&attitudeSettings;
// }
break;
#else
// let autotune.c handle it
// because it must switch to Attitude after <user configurable> seconds
return;
#endif
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
default:
// Major error, this should not occur because only enter this block when one of these is true

442
flight/modules/Stabilization/innerloop.c
View File

@ -52,7 +52,7 @@
#include <cruisecontrol.h>
#include <sanitycheck.h>
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
#include <systemident.h>
#include <systemidentstate.h>
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// Private constants
@ -84,7 +84,7 @@ static float speedScaleFactor = 1.0f;
static bool frame_is_multirotor;
static bool measuredDterm_enabled;
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
static uint32_t system_ident_timeval = 0;
static uint32_t systemIdentTimeVal = 0;
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// Private functions
@ -105,7 +105,7 @@ void stabilizationInnerloopInit()
StabilizationDesiredInitialize();
ActuatorDesiredInitialize();
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
SystemIdentInitialize();
SystemIdentStateInitialize();
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
#ifdef REVOLUTION
AirspeedStateInitialize();
@ -123,7 +123,7 @@ void stabilizationInnerloopInit()
measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE);
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
// Settings for system identification
system_ident_timeval = PIOS_DELAY_GetRaw();
systemIdentTimeVal = PIOS_DELAY_GetRaw();
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
}
@ -342,22 +342,21 @@ static void stabilizationInnerloopTask()
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT:
{
static uint32_t ident_iteration = 0;
static float ident_offsets[3] = {0};
static int8_t identIteration = 0;
static float identOffsets[3] = {0};
/////////////// if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) {
if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD) {
system_ident_timeval = PIOS_DELAY_GetRaw();
if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) {
systemIdentTimeVal = PIOS_DELAY_GetRaw();
SystemIdentData systemIdent;
SystemIdentGet(&systemIdent);
// original code
#if 1
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
float roll_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Roll);
float pitch_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Pitch);
float yaw_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Yaw);
ident_iteration++;
SystemIdentStateData systemIdentState;
SystemIdentStateGet(&systemIdentState);
// original code used 32 bit identIteration
#if 0
const float SCALE_BIAS = 7.1f;
float roll_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Roll);
float pitch_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch);
float yaw_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Yaw);
identIteration++;
if (roll_scale > 0.25f)
roll_scale = 0.25f;
@ -366,96 +365,47 @@ static void stabilizationInnerloopTask()
if (yaw_scale > 0.25f)
yaw_scale = 0.25f;
//why did he do this fsm?
//with yaw changing twice a cycle and roll/pitch changing once?
switch(ident_iteration & 0x07) {
// yaw changes twice a cycle and roll/pitch changes once ?
switch(identIteration & 0x07) {
case 0:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = yaw_scale;
break;
case 1:
ident_offsets[0] = roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
identOffsets[0] = roll_scale;
identOffsets[1] = 0;
identOffsets[2] = 0;
break;
case 2:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = -yaw_scale;
break;
case 3:
ident_offsets[0] = -roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
identOffsets[0] = -roll_scale;
identOffsets[1] = 0;
identOffsets[2] = 0;
break;
case 4:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = yaw_scale;
break;
case 5:
ident_offsets[0] = 0;
ident_offsets[1] = pitch_scale;
ident_offsets[2] = 0;
identOffsets[0] = 0;
identOffsets[1] = pitch_scale;
identOffsets[2] = 0;
break;
case 6:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = -yaw_scale;
break;
case 7:
ident_offsets[0] = 0;
ident_offsets[1] = -pitch_scale;
ident_offsets[2] = 0;
break;
}
#endif
// old partially completed good stuff
#if 0
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
float scale[3] = { expapprox(SCALE_BIAS - systemIdent.Beta.Roll),
expapprox(SCALE_BIAS - systemIdent.Beta.Pitch),
expapprox(SCALE_BIAS - systemIdent.Beta.Yaw) };
ident_iteration++;
if (scale[0] > 0.25f)
scale[0] = 0.25f;
if (scale[1] > 0.25f)
scale[1] = 0.25f;
if (scale[2] > 0.25f)
scale[2] = 0.25f;
//why did he do this fsm?
//with yaw changing twice a cycle and roll/pitch changing once?
ident_offsets[0] = 0.0f;
ident_offsets[1] = 0.0f;
ident_offsets[2] = 0.0f;
switch(ident_iteration & 7) {
case 0:
ident_offsets[2] = scale[2];
break;
case 1:
ident_offsets[0] = scale[0];
break;
case 2:
ident_offsets[2] = -scale[2];
break;
case 3:
ident_offsets[0] = -scale[0];
break;
case 4:
ident_offsets[2] = scale[2];
break;
case 5:
ident_offsets[1] = scale[1];
break;
case 6:
ident_offsets[2] = -scale[2];
break;
case 7:
ident_offsets[1] = -scale[1];
identOffsets[0] = 0;
identOffsets[1] = -pitch_scale;
identOffsets[2] = 0;
break;
}
#endif
@ -463,9 +413,9 @@ static void stabilizationInnerloopTask()
//good stuff here
#if 0
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
float scale[3] = { expapprox(SCALE_BIAS - systemIdent.Beta.Roll),
expapprox(SCALE_BIAS - systemIdent.Beta.Pitch),
expapprox(SCALE_BIAS - systemIdent.Beta.Yaw) };
float scale[3] = { expapprox(SCALE_BIAS - systemIdentState.Beta.Roll),
expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch),
expapprox(SCALE_BIAS - systemIdentState.Beta.Yaw) };
if (scale[0] > 0.25f)
scale[0] = 0.25f;
@ -476,110 +426,214 @@ static void stabilizationInnerloopTask()
//why did he do this fsm?
//with yaw changing twice a cycle and roll/pitch changing once?
ident_offsets[0] = 0.0f;
ident_offsets[1] = 0.0f;
ident_offsets[2] = 0.0f;
ident_iteration = (ident_iteration+1) & 7;
uint8_t index = ((uint8_t *) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [ident_iteration];
// if (ident_iteration & 2) scale[index] = -scale[index];
((uint8_t *)(&scale[index]))[3] ^= (ident_iteration & 2) << 6;
ident_offsets[index] = scale[index];
#if 0
switch(ident_iteration) {
case 0:
ident_offsets[2] = scale[2];
break;
case 1:
ident_offsets[0] = scale[0];
break;
case 2:
ident_offsets[2] = -scale[2];
break;
case 3:
ident_offsets[0] = -scale[0];
break;
case 4:
ident_offsets[2] = scale[2];
break;
case 5:
ident_offsets[1] = scale[1];
break;
case 6:
ident_offsets[2] = -scale[2];
break;
case 7:
ident_offsets[1] = -scale[1];
break;
}
#endif
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 7;
uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
// if (identIteration & 2) scale[index] = -scale[index];
((uint8_t *)(&scale[index]))[3] ^= (identIteration & 2) << 6;
identOffsets[index] = scale[index];
#endif
//aborted mod 9
// same as stock
#if 1
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 7;
uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
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
#endif
// since we are not calculating yaw, remove it and test roll/pitch more frequently
// perhaps this will converge faster
#if 0
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
float roll_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Roll);
float pitch_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Pitch);
float yaw_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Yaw);
ident_iteration++;
if (roll_scale > 0.25f)
roll_scale = 0.25f;
if (pitch_scale > 0.25f)
pitch_scale = 0.25f;
if (yaw_scale > 0.25f)
yaw_scale = 0.25f;
//why did he do this fsm?
//with yaw changing twice a cycle and roll/pitch changing once?
switch(ident_iteration % 9) {
case 0:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
break;
case 1:
ident_offsets[0] = roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
break;
case 2:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
break;
case 3:
ident_offsets[0] = -roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
break;
case 4:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
break;
case 5:
ident_offsets[0] = 0;
ident_offsets[1] = pitch_scale;
ident_offsets[2] = 0;
break;
case 6:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
break;
case 7:
ident_offsets[0] = 0;
ident_offsets[1] = -pitch_scale;
ident_offsets[2] = 0;
break;
case 8:
ident_offsets[0] = 0;
ident_offsets[1] = -pitch_scale;
ident_offsets[2] = 0;
break;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 3;
uint8_t index = ((uint8_t []) { '\0', '\0', '\1', '\1' } ) [identIteration];
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if (identIteration & 2) {
scale = -scale;
}
identOffsets[index] = scale;
// this results in:
// when identIteration==0: identOffsets[0] = roll_scale;
// when identIteration==1: identOffsets[1] = pitch_scale;
// when identIteration==2: identOffsets[0] = -roll_scale;
// when identIteration==3: identOffsets[1] = -pitch_scale;
// each change has one axis with an offset
// and another axis coming back to zero from having an offset
#endif
// since we are not calculating yaw, remove it
// for a cleaner roll / pitch signal
#if 0
const float SCALE_BIAS = 7.1f;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 7;
uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
//recode to this uint8_t index = identIteration >> 2;
if (identIteration & 1) {
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if (identIteration & 2) {
scale = -scale;
}
identOffsets[index] = scale;
}
// this results in:
// when identIteration==0: no offset
// when identIteration==1: identOffsets[0] = roll_scale;
// when identIteration==2: no offset
// when identIteration==3: identOffsets[0] = -roll_scale;
// when identIteration==4: no offset
// when identIteration==5: identOffsets[1] = pitch_scale;
// when identIteration==6: no offset
// when identIteration==7: identOffsets[1] = -pitch_scale;
// each change is either one axis with an offset
// or one axis coming back to zero from having an offset
#endif
// since we are not calculating yaw, remove it
// for a cleaner roll / pitch signal
#if 0
const float SCALE_BIAS = 7.1f;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) % 12;
// uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
//recode to this uint8_t index = identIteration >> 2;
#if 0
if (identIteration < 5) {
index = 0;
} else {
index = 1;
}
#endif
uint8_t index = identIteration % 6 / 3;
uint8_t identIterationMod3 = identIteration % 3;
if (identIterationMod3 <= 1) {
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if ((identIterationMod3 == 1) ^ (identIteration >= 6)) {
scale = -scale;
}
identOffsets[index] = scale;
}
// this results in:
// when identIteration== 0: identOffsets[0] = roll_scale;
// when identIteration== 1: identOffsets[0] = -roll_scale;
// when identIteration== 2: no offset
// when identIteration== 3: identOffsets[1] = pitch_scale;
// when identIteration== 4: identOffsets[1] = -pitch_scale;
// when identIteration== 5: no offset
// when identIteration== 6: identOffsets[0] = -roll_scale;
// when identIteration== 7: identOffsets[0] = roll_scale;
// when identIteration== 8: no offset
// when identIteration== 9: identOffsets[1] = -pitch_scale;
// when identIteration==10: identOffsets[1] = pitch_scale;
// when identIteration==11: no offset
//
// each change is either an axis going from zero to +-scale
// or going from +-scale to -+scale
// there is a delay when changing axes
//
// it's not clear whether AfPredict() is designed to handle double scale perturbances on a particular axis
// resulting from -offset to +offset and needs -offset to zero to +offset
// as an EKF it should handle it
#endif
// one axis at a time
// full stroke with delay between axes
// for a cleaner signal
// a little more difficult to fly?
// makes slightly lower PIDs
// yaw pids seem way high and incorrect
#if 0
const float SCALE_BIAS = 7.1f;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) % 18;
uint8_t index = identIteration % 9 / 3;
uint8_t identIterationMod3 = identIteration % 3;
// if (identIterationMod3 <= 1) {
{
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if ((identIterationMod3 == 1) ^ (identIteration >= 9)) {
scale = -scale;
}
identOffsets[index] = scale;
}
// this results in:
// when identIteration== 0: identOffsets[0] = roll_scale;
// when identIteration== 1: identOffsets[0] = -roll_scale;
// when identIteration== 2: no offset
// when identIteration== 3: identOffsets[1] = pitch_scale;
// when identIteration== 4: identOffsets[1] = -pitch_scale;
// when identIteration== 5: no offset
// when identIteration== 6: identOffsets[2] = yaw_scale;
// when identIteration== 7: identOffsets[2] = -yaw_scale;
// when identIteration== 8: no offset
// when identIteration== 9: identOffsets[0] = -roll_scale;
// when identIteration==10: identOffsets[0] = roll_scale;
// when identIteration==11: no offset
// when identIteration==12: identOffsets[1] = -pitch_scale;
// when identIteration==13: identOffsets[1] = pitch_scale;
// when identIteration==14: no offset
// when identIteration==15: identOffsets[2] = -yaw_scale;
// when identIteration==16: identOffsets[2] = yaw_scale;
// when identIteration==17: no offset
//
// each change is either an axis going from zero to +-scale
// or going from +-scale to -+scale
// there is a delay when changing axes
//
// it's not clear whether AfPredict() is designed to handle 2x scale perturbations on a particular axis
// resulting from -offset to +offset and instead needs -offset to zero to +offset
// ... as an EKF it should handle it
#endif
}
rate[t] = boundf(rate[t],
@ -588,7 +642,7 @@ static void stabilizationInnerloopTask()
);
pid_scaler scaler = create_pid_scaler(t);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
actuatorDesiredAxis[t] += ident_offsets[t];
actuatorDesiredAxis[t] += identOffsets[t];
// we shouldn't do any clamping until after the motors are calculated and scaled?
//actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}

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

@ -157,8 +157,8 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
// 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
// default:
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// do not reorder this code
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;

3
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -125,7 +125,8 @@ UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemident
UAVOBJSRCFILENAMES += systemidentsettings
UAVOBJSRCFILENAMES += systemidentstate
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

3
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -125,7 +125,8 @@ UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemident
UAVOBJSRCFILENAMES += systemidentsettings
UAVOBJSRCFILENAMES += systemidentstate
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

3
flight/targets/boards/revonano/firmware/UAVObjects.inc
View File

@ -125,7 +125,8 @@ UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemident
UAVOBJSRCFILENAMES += systemidentsettings
UAVOBJSRCFILENAMES += systemidentstate
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

3
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -126,7 +126,8 @@ UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += takeofflocation
# this was missing when systemident was added
# UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemident
UAVOBJSRCFILENAMES += systemidentsettings
UAVOBJSRCFILENAMES += systemidentstate
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

3
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -121,7 +121,8 @@ UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += takeofflocation
# this was missing when systemident was added
# UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemident
UAVOBJSRCFILENAMES += systemidentsettings
UAVOBJSRCFILENAMES += systemidentstate
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

3
ground/gcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -137,7 +137,8 @@ UAVOBJS = \
$${UAVOBJ_XML_DIR}/statusvtolautotakeoff.xml \
$${UAVOBJ_XML_DIR}/statusvtolland.xml \
$${UAVOBJ_XML_DIR}/systemalarms.xml \
$${UAVOBJ_XML_DIR}/systemident.xml \
$${UAVOBJ_XML_DIR}/systemidentsettings.xml \
$${UAVOBJ_XML_DIR}/systemidentstate.xml \
$${UAVOBJ_XML_DIR}/systemsettings.xml \
$${UAVOBJ_XML_DIR}/systemstats.xml \
$${UAVOBJ_XML_DIR}/takeofflocation.xml \

22
shared/uavobjectdefinition/systemident.xml → shared/uavobjectdefinition/systemidentsettings.xml
View File

@ -1,15 +1,9 @@
<xml>
<object name="SystemIdent" singleinstance="true" settings="true" category="Control">
<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"/>
<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"/>
<!-- 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. -->
@ -23,20 +17,24 @@
<field name="NoiseMin" units="" type="uint8" elements="1" defaultvalue="8"/>
<field name="NoiseRate" units="" type="uint8" elements="1" defaultvalue="10"/>
<field name="NoiseMax" units="" type="uint8" elements="1" defaultvalue="13"/>
<field name="CalculateYaw" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<field name="CalculateYaw" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True"/>
<field name="YawBetaMin" units="" type="float" elements="1" defaultvalue="5.8"/>
<field name="OverrideYawBeta" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True"/>
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="2"/>
<field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60"/>
<!-- smooth vs. quick PID selector: -->
<!-- 0 = disabled -->
<!-- 10 thru 13 correspond to aux0 -> aux3 transmitter knobs -->
<!-- 23 and 25 are discrete 3 and 5 stop rount robin selectors accessed by quickly toggling the fms 3 times -->
<!-- 23 means stops at 1/2, 1, 0 (repeat) -->
<!-- 25 means stops at 1/2, 3/4, 1, 0, 1/4 (repeat) -->
<field name="SmoothQuick" units="sec" type="uint8" elements="1" defaultvalue="25"/>
<!-- where -100 is smoothest, 0 is middle, +100 is quickest -->
<!-- 23 means stops at 0, 100, -100 (repeat) -->
<!-- 25 means stops at 0, 50, 100, -100, -50 (repeat) -->
<field name="SmoothQuick" 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="false" updatemode="onchange" period="5000"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

19
shared/uavobjectdefinition/systemidentstate.xml Normal file
View 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>