mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
LP-76 add UAV defaults and remove UAV inits
This commit is contained in:
Cliff Geerdes
parent
298fc947b4
commit
2105a49ca9
@ -5,7 +5,8 @@
|
||||
* @addtogroup OpenPilot Libraries OpenPilot System Libraries
|
||||
* @{
|
||||
* @file sanitycheck.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief Utilities to validate a flight configuration
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -163,6 +164,11 @@ int32_t configuration_check()
|
||||
ADDSEVERITY(!coptercontrol);
|
||||
ADDSEVERITY(navCapableFusion);
|
||||
break;
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
|
||||
ADDSEVERITY(!gps_assisted);
|
||||
break;
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
default:
|
||||
// Uncovered modes are automatically an error
|
||||
ADDSEVERITY(false);
|
||||
@ -265,14 +271,30 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
|
||||
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
|
||||
return false;
|
||||
}
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
// we want to be able to use systemident with or without autotune
|
||||
// If this axis allows enabling an autotune behavior without the module
|
||||
// running then set an alarm now that aututune module initializes the
|
||||
// appropriate objects
|
||||
//if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) &&
|
||||
// (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))) {
|
||||
// return false;
|
||||
//}
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
}
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
// don't allow playing with systemident (autotune) on thrust (yet)
|
||||
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) {
|
||||
return false;
|
||||
}
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
}
|
||||
|
||||
if (gpsassisted) {
|
||||
// For multirotors verify that roll/pitch are either attitude or rattitude
|
||||
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
|
||||
if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
|
||||
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
|
||||
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
1105
flight/modules/AutoTune/autotune.c
Normal file
1105
flight/modules/AutoTune/autotune.c
Normal file
@ -0,0 +1,1105 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup StabilizationModule Stabilization Module
|
||||
* @brief Stabilization PID loops in an airframe type independent manner
|
||||
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
|
||||
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
|
||||
* @{
|
||||
*
|
||||
* @file AutoTune/autotune.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* dRonin, http://dRonin.org/, Copyright (C) 2015-2016
|
||||
* Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief Automatic PID tuning module.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "pios.h"
|
||||
//#include "physical_constants.h"
|
||||
#include "flightstatus.h"
|
||||
//#include "modulesettings.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
//#include "gyros.h"
|
||||
#include "gyrosensor.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "systemident.h"
|
||||
#include <pios_board_info.h>
|
||||
//#include "pios_thread.h"
|
||||
#include "systemsettings.h"
|
||||
#include "taskinfo.h"
|
||||
|
||||
#include "stabilization.h"
|
||||
#include "hwsettings.h"
|
||||
#include "stabilizationsettingsbank1.h"
|
||||
#include "stabilizationsettingsbank2.h"
|
||||
#include "stabilizationsettingsbank3.h"
|
||||
|
||||
//#include "circqueue.h"
|
||||
//#include "misc_math.h"
|
||||
|
||||
#define PIOS_malloc pios_malloc
|
||||
|
||||
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
#define powapprox fastpow
|
||||
#define expapprox fastexp
|
||||
#else
|
||||
#define powapprox powf
|
||||
#define expapprox expf
|
||||
#endif /* defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// Private constants
|
||||
#undef STACK_SIZE_BYTES
|
||||
// Nano locks up it seems in UAVObjSav() with 1340
|
||||
// why did it lock up? 1540 now works (after a long initial delay) with 360 bytes left
|
||||
#define STACK_SIZE_BYTES 1340
|
||||
//#define TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
|
||||
|
||||
#define AF_NUMX 13
|
||||
#define AF_NUMP 43
|
||||
|
||||
// Private types <access gcs="readwrite" flight="readwrite"/>
|
||||
enum AUTOTUNE_STATE { AT_INIT, AT_START, AT_RUN, AT_FINISHED, AT_WAITING };
|
||||
|
||||
struct at_queued_data {
|
||||
float y[3]; /* Gyro measurements */
|
||||
float u[3]; /* Actuator desired */
|
||||
float throttle; /* Throttle desired */
|
||||
|
||||
uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */
|
||||
};
|
||||
|
||||
// Private variables
|
||||
//static struct pios_thread *taskHandle;
|
||||
static xTaskHandle taskHandle;
|
||||
static bool module_enabled;
|
||||
static circ_queue_t at_queue;
|
||||
static volatile uint32_t at_points_spilled;
|
||||
static uint32_t throttle_accumulator;
|
||||
static uint8_t rollMax, pitchMax;
|
||||
static StabilizationBankMaximumRateData maximumRate;
|
||||
static SystemSettingsAirframeTypeOptions airframe_type;
|
||||
static float gX[AF_NUMX] = {0};
|
||||
static float gP[AF_NUMP] = {0};
|
||||
SystemIdentData systemIdentData;
|
||||
|
||||
// Private functions
|
||||
static void AutotuneTask(void *parameters);
|
||||
static void af_predict(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 af_init(float X[AF_NUMX], float P[AF_NUMP]);
|
||||
|
||||
#ifndef AT_QUEUE_NUMELEM
|
||||
#define AT_QUEUE_NUMELEM 18
|
||||
#endif
|
||||
|
||||
/**
|
||||
* 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
|
||||
module_enabled = true;
|
||||
#else
|
||||
HwSettingsOptionalModulesData optionalModules;
|
||||
HwSettingsOptionalModulesGet(&optionalModules);
|
||||
if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) {
|
||||
module_enabled = true;
|
||||
} else {
|
||||
module_enabled = false;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (module_enabled) {
|
||||
SystemIdentInitialize();
|
||||
at_queue = circ_queue_new(sizeof(struct at_queued_data),
|
||||
AT_QUEUE_NUMELEM);
|
||||
if (!at_queue) {
|
||||
module_enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AutotuneStart(void)
|
||||
{
|
||||
// Start main task if it is enabled
|
||||
if (module_enabled) {
|
||||
//taskHandle = PIOS_Thread_Create(AutotuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
|
||||
//TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
|
||||
//PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
|
||||
xTaskCreate(AutotuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(AutotuneInitialize, AutotuneStart);
|
||||
|
||||
#if 0
|
||||
static void at_new_gyro_data(UAVObjEvent * ev, void *ctx, void *obj, int len) {
|
||||
(void) ev; (void) ctx;
|
||||
#else
|
||||
static void at_new_gyro_data(UAVObjEvent * ev) {
|
||||
#endif
|
||||
#if 0
|
||||
typedef struct {
|
||||
UAVObjHandle obj;
|
||||
uint16_t instId;
|
||||
UAVObjEventType event;
|
||||
bool lowPriority; /* if true prevents raising warnings */
|
||||
} UAVObjEvent;
|
||||
#endif
|
||||
|
||||
static bool last_sample_unpushed = 0;
|
||||
GyroSensorData gyro;
|
||||
ActuatorDesiredData actuators;
|
||||
|
||||
if (!ev || !ev->obj || ev->instId!=0 || ev->event!=EV_UPDATED) {
|
||||
return;
|
||||
}
|
||||
|
||||
// object can and possibly will at times change asynchronously so must copy data here, with locking
|
||||
// and do it as soon as possible
|
||||
GyroSensorGet(&gyro);
|
||||
ActuatorDesiredGet(&actuators);
|
||||
|
||||
// GyroSensorData *g = ev->obj;
|
||||
|
||||
// PIOS_Assert(len == sizeof(*g));
|
||||
|
||||
if (last_sample_unpushed) {
|
||||
/* Last time we were unable to advance the write pointer.
|
||||
* Try again, last chance! */
|
||||
if (circ_queue_advance_write(at_queue)) {
|
||||
at_points_spilled++;
|
||||
}
|
||||
}
|
||||
|
||||
struct at_queued_data *q_item = circ_queue_cur_write_pos(at_queue);
|
||||
|
||||
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 (circ_queue_advance_write(at_queue) != 0) {
|
||||
last_sample_unpushed = true;
|
||||
} else {
|
||||
last_sample_unpushed = false;
|
||||
}
|
||||
}
|
||||
|
||||
static void UpdateSystemIdent(const float *X, const float *noise,
|
||||
float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle) {
|
||||
memset(&systemIdentData, 0, sizeof(systemIdentData));
|
||||
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];
|
||||
|
||||
if (noise) {
|
||||
systemIdentData.Noise.Roll = noise[0];
|
||||
systemIdentData.Noise.Pitch = noise[1];
|
||||
systemIdentData.Noise.Yaw = noise[2];
|
||||
}
|
||||
systemIdentData.Period = dT_s * 1000.0f;
|
||||
|
||||
systemIdentData.NumAfPredicts = predicts;
|
||||
systemIdentData.NumSpilledPts = spills;
|
||||
|
||||
systemIdentData.HoverThrottle = hover_throttle;
|
||||
|
||||
SystemIdentSet(&systemIdentData);
|
||||
}
|
||||
|
||||
static void UpdateStabilizationDesired(bool doingIdent) {
|
||||
StabilizationDesiredData stabDesired;
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
|
||||
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 * maximumRate.Yaw;
|
||||
|
||||
if (doingIdent) {
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
|
||||
} else {
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
}
|
||||
|
||||
stabDesired.Thrust = (airframe_type == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle;
|
||||
// is this a race
|
||||
// control feels very sluggish too
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
void ComputeStabilization()
|
||||
{
|
||||
StabilizationSettingsBank1Data stabSettingsBank;
|
||||
|
||||
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 = systemIdentData.RateNoise / 1000.0f;
|
||||
const double damp = systemIdentData.RateDamp / 100.0f;
|
||||
|
||||
double tau = exp(systemIdentData.Tau);
|
||||
double beta_roll = systemIdentData.Beta.Roll;
|
||||
double beta_pitch = systemIdentData.Beta.Pitch;
|
||||
|
||||
double wn = 1.0f/tau;
|
||||
double tau_d = 0.0f;
|
||||
for (int i = 0; i < 30; i++) {
|
||||
double tau_d_roll = (2.0f*damp*tau*wn - 1.0f)/(4.0f*tau*damp*damp*wn*wn - 2.0f*damp*wn - tau*wn*wn + exp(beta_roll)*ghf);
|
||||
double tau_d_pitch = (2.0f*damp*tau*wn - 1.0f)/(4.0f*tau*damp*damp*wn*wn - 2.0f*damp*wn - tau*wn*wn + exp(beta_pitch)*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.0f * damp + 2.0f);
|
||||
}
|
||||
|
||||
// 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.0f * damp * wn) / 20.0f;
|
||||
const double b = ((tau+tau_d) / tau / tau_d - 2.0f * 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.3;
|
||||
const double kp_o = 1.0f / 4.0f / (zeta_o * zeta_o) / (1.0f/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]);
|
||||
|
||||
double ki = a * b * wn * wn * tau * tau_d / beta;
|
||||
double kp = tau * tau_d * ((a+b)*wn*wn + 2.0f*a*b*damp*wn) / beta - ki*tau_d;
|
||||
double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0f*damp*wn) - 1.0f) / 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;
|
||||
case 2: // optional Yaw
|
||||
stabSettingsBank.YawRatePID.Kp = kp;
|
||||
stabSettingsBank.YawRatePID.Ki = ki;
|
||||
stabSettingsBank.YawRatePID.Kd = kd;
|
||||
stabSettingsBank.YawPI.Kp = kp_o;
|
||||
stabSettingsBank.YawPI.Ki = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//stabSettingsBank.DerivativeCutoff = 1.0f / (2.0f*M_PI*tau_d);
|
||||
}
|
||||
#else
|
||||
void ComputeStabilizationAndSetPids()
|
||||
{
|
||||
StabilizationSettingsBank1Data stabSettingsBank;
|
||||
|
||||
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 = systemIdentData.RateNoise / 1000.0d;
|
||||
const double damp = systemIdentData.RateDamp / 100.0d;
|
||||
|
||||
double tau = exp(systemIdentData.Tau);
|
||||
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);
|
||||
|
||||
// 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 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]);
|
||||
|
||||
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;
|
||||
case 2: // optional Yaw
|
||||
stabSettingsBank.YawRatePID.Kp = kp;
|
||||
stabSettingsBank.YawRatePID.Ki = ki;
|
||||
stabSettingsBank.YawRatePID.Kd = kd;
|
||||
stabSettingsBank.YawPI.Kp = kp_o;
|
||||
stabSettingsBank.YawPI.Ki = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d);
|
||||
|
||||
switch (systemIdentData.DestinationPidBank) {
|
||||
case 1:
|
||||
StabilizationSettingsBank1Set((void *)&stabSettingsBank);
|
||||
break;
|
||||
case 2:
|
||||
StabilizationSettingsBank2Set((void *)&stabSettingsBank);
|
||||
break;
|
||||
case 3:
|
||||
StabilizationSettingsBank3Set((void *)&stabSettingsBank);
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#define MAX_PTS_PER_CYCLE 4
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void AutotuneTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
enum AUTOTUNE_STATE state = AT_INIT;
|
||||
|
||||
uint32_t last_update_time = xTaskGetTickCount();
|
||||
|
||||
float noise[3] = {0};
|
||||
|
||||
af_init(gX,gP);
|
||||
|
||||
uint32_t last_time = 0.0f;
|
||||
const uint32_t YIELD_MS = 2;
|
||||
|
||||
GyroSensorConnectCallback(at_new_gyro_data);
|
||||
|
||||
bool save_needed = false;
|
||||
|
||||
while(1) {
|
||||
//PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
|
||||
|
||||
uint32_t diff_time;
|
||||
|
||||
const uint32_t PREPARE_TIME = 2000;
|
||||
const uint32_t MEASURE_TIME = 60000;
|
||||
|
||||
static uint32_t update_counter = 0;
|
||||
|
||||
bool doing_ident = false;
|
||||
bool can_sleep = true;
|
||||
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
//this seems to lock up on Nano
|
||||
if (save_needed) {
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
|
||||
// Save the settings locally.
|
||||
UAVObjSave(SystemIdentHandle(), 0);
|
||||
state = AT_INIT;
|
||||
|
||||
save_needed = false;
|
||||
}
|
||||
}
|
||||
// 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
|
||||
|
||||
// Only allow this module to run when autotuning
|
||||
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
|
||||
state = AT_INIT;
|
||||
vTaskDelay(50 / portTICK_RATE_MS);
|
||||
continue;
|
||||
}
|
||||
|
||||
switch(state) {
|
||||
case AT_INIT:
|
||||
|
||||
// moved from UpdateStabilizationDesired()
|
||||
StabilizationBankRollMaxGet(&rollMax);
|
||||
StabilizationBankPitchMaxGet(&pitchMax);
|
||||
StabilizationBankMaximumRateGet(&maximumRate);
|
||||
SystemSettingsAirframeTypeGet(&airframe_type);
|
||||
|
||||
// Reset save status; only save if this tune
|
||||
// completes.
|
||||
save_needed = false;
|
||||
|
||||
last_update_time = xTaskGetTickCount();
|
||||
|
||||
// Only start when armed and flying
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
|
||||
// remove this one and let the other one init it
|
||||
// should wait on the other one if that is the case
|
||||
af_init(gX, gP);
|
||||
UpdateSystemIdent(gX, NULL, 0.0f, 0, 0, 0.0f);
|
||||
state = AT_START;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case AT_START:
|
||||
|
||||
diff_time = xTaskGetTickCount() - last_update_time;
|
||||
|
||||
// Spend the first block of time in normal rate mode to get stabilized
|
||||
if (diff_time > PREPARE_TIME) {
|
||||
last_time = PIOS_DELAY_GetRaw();
|
||||
|
||||
/* Drain the queue of all current data */
|
||||
while (circ_queue_read_pos(at_queue)) {
|
||||
circ_queue_read_completed(at_queue);
|
||||
}
|
||||
|
||||
/* And reset the point spill counter */
|
||||
|
||||
update_counter = 0;
|
||||
at_points_spilled = 0;
|
||||
|
||||
throttle_accumulator = 0;
|
||||
|
||||
state = AT_RUN;
|
||||
last_update_time = xTaskGetTickCount();
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
case AT_RUN:
|
||||
|
||||
diff_time = xTaskGetTickCount() - last_update_time;
|
||||
|
||||
doing_ident = true;
|
||||
can_sleep = false;
|
||||
|
||||
for (int i=0; i<MAX_PTS_PER_CYCLE; i++) {
|
||||
struct at_queued_data *pt;
|
||||
|
||||
/* Grab an autotune point */
|
||||
pt = circ_queue_read_pos(at_queue);
|
||||
|
||||
if (!pt) {
|
||||
/* We've drained the buffer
|
||||
* fully. Yay! */
|
||||
can_sleep = true;
|
||||
break;
|
||||
}
|
||||
|
||||
/* calculate time between successive
|
||||
* points */
|
||||
|
||||
float dT_s = PIOS_DELAY_DiffuS2(last_time,
|
||||
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;
|
||||
}
|
||||
|
||||
last_time = pt->raw_time;
|
||||
|
||||
af_predict(gX, gP, pt->u, pt->y, dT_s, pt->throttle);
|
||||
|
||||
for (int j=0; j<3; ++j) {
|
||||
const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz
|
||||
noise[j] = NOISE_ALPHA * noise[j] + (1-NOISE_ALPHA) * (pt->y[j] - gX[j]) * (pt->y[j] - gX[j]);
|
||||
}
|
||||
|
||||
//This will work up to 8kHz with an 89% throttle position before overflow
|
||||
throttle_accumulator += 10000 * pt->throttle;
|
||||
|
||||
// Update uavo every 256 cycles to avoid
|
||||
// telemetry spam
|
||||
if (!((update_counter++) & 0xff)) {
|
||||
float hover_throttle = ((float)(throttle_accumulator/update_counter))/10000.0f;
|
||||
UpdateSystemIdent(gX, noise, dT_s, update_counter, at_points_spilled, hover_throttle);
|
||||
}
|
||||
|
||||
/* Free the buffer containing an AT point */
|
||||
circ_queue_read_completed(at_queue);
|
||||
}
|
||||
|
||||
if (diff_time > MEASURE_TIME) { // Move on to next state
|
||||
state = AT_FINISHED;
|
||||
last_update_time = xTaskGetTickCount();
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case AT_FINISHED: ;
|
||||
|
||||
// Wait until disarmed and landed before saving the settings
|
||||
|
||||
float hover_throttle = ((float)(throttle_accumulator/update_counter))/10000.0f;
|
||||
UpdateSystemIdent(gX, noise, 0, update_counter, at_points_spilled, hover_throttle);
|
||||
|
||||
save_needed = true;
|
||||
state = AT_WAITING;
|
||||
|
||||
break;
|
||||
|
||||
case AT_WAITING:
|
||||
default:
|
||||
// Set an alarm or some shit like that
|
||||
break;
|
||||
}
|
||||
|
||||
// Update based on manual controls
|
||||
UpdateStabilizationDesired(doing_ident);
|
||||
|
||||
if (can_sleep) {
|
||||
vTaskDelay(YIELD_MS / portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Prediction step for EKF on control inputs to quad that
|
||||
* learns the system properties
|
||||
* @param X the current state estimate which is updated in place
|
||||
* @param P the current covariance matrix, updated in place
|
||||
* @param[in] the current control inputs (roll, pitch, yaw)
|
||||
* @param[in] the gyro measurements
|
||||
*/
|
||||
__attribute__((always_inline)) static inline void af_predict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in)
|
||||
{
|
||||
const float Ts = dT_s;
|
||||
const float Tsq = Ts * Ts;
|
||||
const float Tsq3 = Tsq * Ts;
|
||||
const float Tsq4 = Tsq * Tsq;
|
||||
|
||||
// for convenience and clarity code below uses the named versions of
|
||||
// the state variables
|
||||
float w1 = X[0]; // roll rate estimate
|
||||
float w2 = X[1]; // pitch rate estimate
|
||||
float w3 = X[2]; // yaw rate estimate
|
||||
float u1 = X[3]; // scaled roll torque
|
||||
float u2 = X[4]; // scaled pitch torque
|
||||
float u3 = X[5]; // scaled yaw torque
|
||||
const float e_b1 = expapprox(X[6]); // roll torque scale
|
||||
const float b1 = X[6];
|
||||
const float e_b2 = expapprox(X[7]); // pitch torque scale
|
||||
const float b2 = X[7];
|
||||
const float e_b3 = expapprox(X[8]); // yaw torque scale
|
||||
const float b3 = X[8];
|
||||
const float e_tau = expapprox(X[9]); // time response of the motors
|
||||
const float tau = X[9];
|
||||
const float bias1 = X[10]; // bias in the roll torque
|
||||
const float bias2 = X[11]; // bias in the pitch torque
|
||||
const float bias3 = X[12]; // bias in the yaw torque
|
||||
|
||||
// inputs to the system (roll, pitch, yaw)
|
||||
const float u1_in = 4*t_in*u_in[0];
|
||||
const float u2_in = 4*t_in*u_in[1];
|
||||
const float u3_in = 4*t_in*u_in[2];
|
||||
|
||||
// measurements from gyro
|
||||
const float gyro_x = gyro[0];
|
||||
const float gyro_y = gyro[1];
|
||||
const float gyro_z = gyro[2];
|
||||
|
||||
// update named variables because we want to use predicted
|
||||
// values below
|
||||
w1 = X[0] = w1 - Ts*bias1*e_b1 + Ts*u1*e_b1;
|
||||
w2 = X[1] = w2 - Ts*bias2*e_b2 + Ts*u2*e_b2;
|
||||
w3 = X[2] = w3 - Ts*bias3*e_b3 + Ts*u3*e_b3;
|
||||
u1 = X[3] = (Ts*u1_in)/(Ts + e_tau) + (u1*e_tau)/(Ts + e_tau);
|
||||
u2 = X[4] = (Ts*u2_in)/(Ts + e_tau) + (u2*e_tau)/(Ts + e_tau);
|
||||
u3 = X[5] = (Ts*u3_in)/(Ts + e_tau) + (u3*e_tau)/(Ts + e_tau);
|
||||
// X[6] to X[12] unchanged
|
||||
|
||||
/**** filter parameters ****/
|
||||
const float q_w = 1e-3f;
|
||||
const float q_ud = 1e-3f;
|
||||
const float q_B = 1e-6f;
|
||||
const float q_tau = 1e-6f;
|
||||
const float q_bias = 1e-19f;
|
||||
const float s_a = 150.0f; // expected gyro measurment noise
|
||||
|
||||
const float Q[AF_NUMX] = {q_w, q_w, q_w, q_ud, q_ud, q_ud, q_B, q_B, q_B, q_tau, q_bias, q_bias, q_bias};
|
||||
|
||||
float D[AF_NUMP];
|
||||
for (uint32_t i = 0; i < AF_NUMP; i++)
|
||||
D[i] = P[i];
|
||||
|
||||
const float e_tau2 = e_tau * e_tau;
|
||||
const float e_tau3 = e_tau * e_tau2;
|
||||
const float e_tau4 = e_tau2 * e_tau2;
|
||||
const float Ts_e_tau2 = (Ts + e_tau) * (Ts + e_tau);
|
||||
const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2;
|
||||
|
||||
// covariance propagation - D is stored copy of covariance
|
||||
P[0] = D[0] + Q[0] + 2*Ts*e_b1*(D[3] - D[28] - D[9]*bias1 + D[9]*u1) + Tsq*(e_b1*e_b1)*(D[4] - 2*D[29] + D[32] - 2*D[10]*bias1 + 2*D[30]*bias1 + 2*D[10]*u1 - 2*D[30]*u1 + D[11]*(bias1*bias1) + D[11]*(u1*u1) - 2*D[11]*bias1*u1);
|
||||
P[1] = D[1] + Q[1] + 2*Ts*e_b2*(D[5] - D[33] - D[12]*bias2 + D[12]*u2) + Tsq*(e_b2*e_b2)*(D[6] - 2*D[34] + D[37] - 2*D[13]*bias2 + 2*D[35]*bias2 + 2*D[13]*u2 - 2*D[35]*u2 + D[14]*(bias2*bias2) + D[14]*(u2*u2) - 2*D[14]*bias2*u2);
|
||||
P[2] = D[2] + Q[2] + 2*Ts*e_b3*(D[7] - D[38] - D[15]*bias3 + D[15]*u3) + Tsq*(e_b3*e_b3)*(D[8] - 2*D[39] + D[42] - 2*D[16]*bias3 + 2*D[40]*bias3 + 2*D[16]*u3 - 2*D[40]*u3 + D[17]*(bias3*bias3) + D[17]*(u3*u3) - 2*D[17]*bias3*u3);
|
||||
P[3] = (D[3]*(e_tau2 + Ts*e_tau) + Ts*e_b1*e_tau2*(D[4] - D[29]) + Tsq*e_b1*e_tau*(D[4] - D[29]) + D[18]*Ts*e_tau*(u1 - u1_in) + D[10]*e_b1*(u1*(Ts*e_tau2 + Tsq*e_tau) - bias1*(Ts*e_tau2 + Tsq*e_tau)) + D[21]*Tsq*e_b1*e_tau*(u1 - u1_in) + D[31]*Tsq*e_b1*e_tau*(u1_in - u1) + D[24]*Tsq*e_b1*e_tau*(u1*(u1 - bias1) + u1_in*(bias1 - u1)))/Ts_e_tau2;
|
||||
P[4] = (Q[3]*Tsq4 + e_tau4*(D[4] + Q[3]) + 2*Ts*e_tau3*(D[4] + 2*Q[3]) + 4*Q[3]*Tsq3*e_tau + Tsq*e_tau2*(D[4] + 6*Q[3] + u1*(D[27]*u1 + 2*D[21]) + u1_in*(D[27]*u1_in - 2*D[21])) + 2*D[21]*Ts*e_tau3*(u1 - u1_in) - 2*D[27]*Tsq*u1*u1_in*e_tau2)/Ts_e_tau4;
|
||||
P[5] = (D[5]*(e_tau2 + Ts*e_tau) + Ts*e_b2*e_tau2*(D[6] - D[34]) + Tsq*e_b2*e_tau*(D[6] - D[34]) + D[19]*Ts*e_tau*(u2 - u2_in) + D[13]*e_b2*(u2*(Ts*e_tau2 + Tsq*e_tau) - bias2*(Ts*e_tau2 + Tsq*e_tau)) + D[22]*Tsq*e_b2*e_tau*(u2 - u2_in) + D[36]*Tsq*e_b2*e_tau*(u2_in - u2) + D[25]*Tsq*e_b2*e_tau*(u2*(u2 - bias2) + u2_in*(bias2 - u2)))/Ts_e_tau2;
|
||||
P[6] = (Q[4]*Tsq4 + e_tau4*(D[6] + Q[4]) + 2*Ts*e_tau3*(D[6] + 2*Q[4]) + 4*Q[4]*Tsq3*e_tau + Tsq*e_tau2*(D[6] + 6*Q[4] + u2*(D[27]*u2 + 2*D[22]) + u2_in*(D[27]*u2_in - 2*D[22])) + 2*D[22]*Ts*e_tau3*(u2 - u2_in) - 2*D[27]*Tsq*u2*u2_in*e_tau2)/Ts_e_tau4;
|
||||
P[7] = (D[7]*(e_tau2 + Ts*e_tau) + Ts*e_b3*e_tau2*(D[8] - D[39]) + Tsq*e_b3*e_tau*(D[8] - D[39]) + D[20]*Ts*e_tau*(u3 - u3_in) + D[16]*e_b3*(u3*(Ts*e_tau2 + Tsq*e_tau) - bias3*(Ts*e_tau2 + Tsq*e_tau)) + D[23]*Tsq*e_b3*e_tau*(u3 - u3_in) + D[41]*Tsq*e_b3*e_tau*(u3_in - u3) + D[26]*Tsq*e_b3*e_tau*(u3*(u3 - bias3) + u3_in*(bias3 - u3)))/Ts_e_tau2;
|
||||
P[8] = (Q[5]*Tsq4 + e_tau4*(D[8] + Q[5]) + 2*Ts*e_tau3*(D[8] + 2*Q[5]) + 4*Q[5]*Tsq3*e_tau + Tsq*e_tau2*(D[8] + 6*Q[5] + u3*(D[27]*u3 + 2*D[23]) + u3_in*(D[27]*u3_in - 2*D[23])) + 2*D[23]*Ts*e_tau3*(u3 - u3_in) - 2*D[27]*Tsq*u3*u3_in*e_tau2)/Ts_e_tau4;
|
||||
P[9] = D[9] - Ts*e_b1*(D[30] - D[10] + D[11]*(bias1 - u1));
|
||||
P[10] = (D[10]*(Ts + e_tau) + D[24]*Ts*(u1 - u1_in))*(e_tau/Ts_e_tau2);
|
||||
P[11] = D[11] + Q[6];
|
||||
P[12] = D[12] - Ts*e_b2*(D[35] - D[13] + D[14]*(bias2 - u2));
|
||||
P[13] = (D[13]*(Ts + e_tau) + D[25]*Ts*(u2 - u2_in))*(e_tau/Ts_e_tau2);
|
||||
P[14] = D[14] + Q[7];
|
||||
P[15] = D[15] - Ts*e_b3*(D[40] - D[16] + D[17]*(bias3 - u3));
|
||||
P[16] = (D[16]*(Ts + e_tau) + D[26]*Ts*(u3 - u3_in))*(e_tau/Ts_e_tau2);
|
||||
P[17] = D[17] + Q[8];
|
||||
P[18] = D[18] - Ts*e_b1*(D[31] - D[21] + D[24]*(bias1 - u1));
|
||||
P[19] = D[19] - Ts*e_b2*(D[36] - D[22] + D[25]*(bias2 - u2));
|
||||
P[20] = D[20] - Ts*e_b3*(D[41] - D[23] + D[26]*(bias3 - u3));
|
||||
P[21] = (D[21]*(Ts + e_tau) + D[27]*Ts*(u1 - u1_in))*(e_tau/Ts_e_tau2);
|
||||
P[22] = (D[22]*(Ts + e_tau) + D[27]*Ts*(u2 - u2_in))*(e_tau/Ts_e_tau2);
|
||||
P[23] = (D[23]*(Ts + e_tau) + D[27]*Ts*(u3 - u3_in))*(e_tau/Ts_e_tau2);
|
||||
P[24] = D[24];
|
||||
P[25] = D[25];
|
||||
P[26] = D[26];
|
||||
P[27] = D[27] + Q[9];
|
||||
P[28] = D[28] - Ts*e_b1*(D[32] - D[29] + D[30]*(bias1 - u1));
|
||||
P[29] = (D[29]*(Ts + e_tau) + D[31]*Ts*(u1 - u1_in))*(e_tau/Ts_e_tau2);
|
||||
P[30] = D[30];
|
||||
P[31] = D[31];
|
||||
P[32] = D[32] + Q[10];
|
||||
P[33] = D[33] - Ts*e_b2*(D[37] - D[34] + D[35]*(bias2 - u2));
|
||||
P[34] = (D[34]*(Ts + e_tau) + D[36]*Ts*(u2 - u2_in))*(e_tau/Ts_e_tau2);
|
||||
P[35] = D[35];
|
||||
P[36] = D[36];
|
||||
P[37] = D[37] + Q[11];
|
||||
P[38] = D[38] - Ts*e_b3*(D[42] - D[39] + D[40]*(bias3 - u3));
|
||||
P[39] = (D[39]*(Ts + e_tau) + D[41]*Ts*(u3 - u3_in))*(e_tau/Ts_e_tau2);
|
||||
P[40] = D[40];
|
||||
P[41] = D[41];
|
||||
P[42] = D[42] + Q[12];
|
||||
|
||||
|
||||
/********* this is the update part of the equation ***********/
|
||||
|
||||
float S[3] = {P[0] + s_a, P[1] + s_a, P[2] + s_a};
|
||||
|
||||
X[0] = w1 + P[0]*((gyro_x - w1)/S[0]);
|
||||
X[1] = w2 + P[1]*((gyro_y - w2)/S[1]);
|
||||
X[2] = w3 + P[2]*((gyro_z - w3)/S[2]);
|
||||
X[3] = u1 + P[3]*((gyro_x - w1)/S[0]);
|
||||
X[4] = u2 + P[5]*((gyro_y - w2)/S[1]);
|
||||
X[5] = u3 + P[7]*((gyro_z - w3)/S[2]);
|
||||
X[6] = b1 + P[9]*((gyro_x - w1)/S[0]);
|
||||
X[7] = b2 + P[12]*((gyro_y - w2)/S[1]);
|
||||
X[8] = b3 + P[15]*((gyro_z - w3)/S[2]);
|
||||
X[9] = tau + P[18]*((gyro_x - w1)/S[0]) + P[19]*((gyro_y - w2)/S[1]) + P[20]*((gyro_z - w3)/S[2]);
|
||||
X[10] = bias1 + P[28]*((gyro_x - w1)/S[0]);
|
||||
X[11] = bias2 + P[33]*((gyro_y - w2)/S[1]);
|
||||
X[12] = bias3 + P[38]*((gyro_z - w3)/S[2]);
|
||||
|
||||
// update the duplicate cache
|
||||
for (uint32_t i = 0; i < AF_NUMP; i++)
|
||||
D[i] = P[i];
|
||||
|
||||
// This is an approximation that removes some cross axis uncertainty but
|
||||
// substantially reduces the number of calculations
|
||||
P[0] = -D[0]*(D[0]/S[0] - 1);
|
||||
P[1] = -D[1]*(D[1]/S[1] - 1);
|
||||
P[2] = -D[2]*(D[2]/S[2] - 1);
|
||||
P[3] = -D[3]*(D[0]/S[0] - 1);
|
||||
P[4] = D[4] - D[3]*D[3]/S[0];
|
||||
P[5] = -D[5]*(D[1]/S[1] - 1);
|
||||
P[6] = D[6] - D[5]*D[5]/S[1];
|
||||
P[7] = -D[7]*(D[2]/S[2] - 1);
|
||||
P[8] = D[8] - D[7]*D[7]/S[2];
|
||||
P[9] = -D[9]*(D[0]/S[0] - 1);
|
||||
P[10] = D[10] - D[3]*(D[9]/S[0]);
|
||||
P[11] = D[11] - D[9]*(D[9]/S[0]);
|
||||
P[12] = -D[12]*(D[1]/S[1] - 1);
|
||||
P[13] = D[13] - D[5]*(D[12]/S[1]);
|
||||
P[14] = D[14] - D[12]*(D[12]/S[1]);
|
||||
P[15] = -D[15]*(D[2]/S[2] - 1);
|
||||
P[16] = D[16] - D[7]*(D[15]/S[2]);
|
||||
P[17] = D[17] - D[15]*(D[15]/S[2]);
|
||||
P[18] = -D[18]*(D[0]/S[0] - 1);
|
||||
P[19] = -D[19]*(D[1]/S[1] - 1);
|
||||
P[20] = -D[20]*(D[2]/S[2] - 1);
|
||||
P[21] = D[21] - D[3]*(D[18]/S[0]);
|
||||
P[22] = D[22] - D[5]*(D[19]/S[1]);
|
||||
P[23] = D[23] - D[7]*(D[20]/S[2]);
|
||||
P[24] = D[24] - D[9]*(D[18]/S[0]);
|
||||
P[25] = D[25] - D[12]*(D[19]/S[1]);
|
||||
P[26] = D[26] - D[15]*(D[20]/S[2]);
|
||||
P[27] = D[27] - D[18]*(D[18]/S[0]) - D[19]*(D[19]/S[1]) - D[20]*(D[20]/S[2]);
|
||||
P[28] = -D[28]*(D[0]/S[0] - 1);
|
||||
P[29] = D[29] - D[3]*(D[28]/S[0]);
|
||||
P[30] = D[30] - D[9]*(D[28]/S[0]);
|
||||
P[31] = D[31] - D[18]*(D[28]/S[0]);
|
||||
P[32] = D[32] - D[28]*(D[28]/S[0]);
|
||||
P[33] = -D[33]*(D[1]/S[1] - 1);
|
||||
P[34] = D[34] - D[5]*(D[33]/S[1]);
|
||||
P[35] = D[35] - D[12]*(D[33]/S[1]);
|
||||
P[36] = D[36] - D[19]*(D[33]/S[1]);
|
||||
P[37] = D[37] - D[33]*(D[33]/S[1]);
|
||||
P[38] = -D[38]*(D[2]/S[2] - 1);
|
||||
P[39] = D[39] - D[7]*(D[38]/S[2]);
|
||||
P[40] = D[40] - D[15]*(D[38]/S[2]);
|
||||
P[41] = D[41] - D[20]*(D[38]/S[2]);
|
||||
P[42] = D[42] - D[38]*(D[38]/S[2]);
|
||||
|
||||
// apply limits to some of the state variables
|
||||
if (X[9] > -1.5f)
|
||||
X[9] = -1.5f;
|
||||
else if (X[9] < -5.5f) /* 4ms */
|
||||
X[9] = -5.5f;
|
||||
if (X[10] > 0.5f)
|
||||
X[10] = 0.5f;
|
||||
else if (X[10] < -0.5f)
|
||||
X[10] = -0.5f;
|
||||
if (X[11] > 0.5f)
|
||||
X[11] = 0.5f;
|
||||
else if (X[11] < -0.5f)
|
||||
X[11] = -0.5f;
|
||||
if (X[12] > 0.5f)
|
||||
X[12] = 0.5f;
|
||||
else if (X[12] < -0.5f)
|
||||
X[12] = -0.5f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the state variable and covariance matrix
|
||||
* for the system identification EKF
|
||||
*/
|
||||
static void af_init(float X[AF_NUMX], float P[AF_NUMP])
|
||||
{
|
||||
static const float q_init[AF_NUMX] = {
|
||||
1.0f, 1.0f, 1.0f,
|
||||
1.0f, 1.0f, 1.0f,
|
||||
0.05f, 0.05f, 0.005f,
|
||||
0.05f,
|
||||
0.05f, 0.05f, 0.05f
|
||||
};
|
||||
|
||||
// X[0] = X[1] = X[2] = 0.0f; // assume no rotation
|
||||
// X[3] = X[4] = X[5] = 0.0f; // and no net torque
|
||||
// X[6] = X[7] = 10.0f; // medium amount of strength
|
||||
// X[8] = 7.0f; // yaw
|
||||
// X[9] = -4.0f; // and 50 (18?) ms time scale
|
||||
// X[10] = X[11] = X[12] = 0.0f; // zero bias
|
||||
|
||||
// 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]);
|
||||
|
||||
// P initialization
|
||||
// Could zero this like: *P = *((float [AF_NUMP]){});
|
||||
memset(P, 0, AF_NUMP*sizeof(P[0]));
|
||||
P[0] = q_init[0];
|
||||
P[1] = q_init[1];
|
||||
P[2] = q_init[2];
|
||||
P[4] = q_init[3];
|
||||
P[6] = q_init[4];
|
||||
P[8] = q_init[5];
|
||||
P[11] = q_init[6];
|
||||
P[14] = q_init[7];
|
||||
P[17] = q_init[8];
|
||||
P[27] = q_init[9];
|
||||
P[32] = q_init[10];
|
||||
P[37] = q_init[11];
|
||||
P[42] = q_init[12];
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -342,6 +342,9 @@ static bool okToArm(void)
|
||||
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
return false;
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
|
||||
|
||||
@ -47,7 +47,8 @@
|
||||
#include <stabilizationsettings.h>
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#endif
|
||||
#include <systemident.h>
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
// Private constants
|
||||
#if defined(PIOS_MANUAL_STACK_SIZE)
|
||||
@ -118,7 +119,10 @@ 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);
|
||||
|
||||
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
|
||||
@ -146,8 +150,8 @@ int32_t ManualControlStart()
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
takeOffLocationHandlerInit();
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
#endif
|
||||
// Start main task
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
|
||||
@ -174,7 +178,7 @@ int32_t ManualControlInitialize()
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
SystemSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
|
||||
|
||||
return 0;
|
||||
@ -202,7 +206,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
}
|
||||
|
||||
/**
|
||||
@ -214,7 +218,7 @@ static void manualControlTask(void)
|
||||
armHandler(false, frameType);
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
takeOffLocationHandler();
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
// Process flight mode
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
@ -225,7 +229,7 @@ static void manualControlTask(void)
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
VtolPathFollowerSettingsThrustLimitsData thrustLimits;
|
||||
VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
FlightModeSettingsData modeSettings;
|
||||
FlightModeSettingsGet(&modeSettings);
|
||||
@ -251,6 +255,13 @@ 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
|
||||
@ -265,6 +276,9 @@ static void manualControlTask(void)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
handler = &handler_STABILIZED;
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
@ -278,7 +292,6 @@ static void manualControlTask(void)
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
}
|
||||
|
||||
|
||||
if (newFlightModeAssist) {
|
||||
// assess roll/pitch state
|
||||
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
|
||||
@ -530,22 +543,22 @@ static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
}
|
||||
|
||||
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
/**
|
||||
* Check and set modes for gps assisted stablised flight modes
|
||||
*/
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
|
||||
{
|
||||
uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
|
||||
uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
//uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
|
||||
|
||||
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
|
||||
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
||||
flightModeAssistOption = FlightModeAssistMap[position];
|
||||
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE
|
||||
|| position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
||||
return (FLIGHTSTATUS_FLIGHTMODEASSIST_NONE);
|
||||
}
|
||||
|
||||
switch (flightModeAssistOption) {
|
||||
switch (FlightModeAssistMap[position]) {
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
|
||||
break;
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST:
|
||||
@ -592,22 +605,71 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD:
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
|
||||
// this is only for use with stabi mods with althold/vario.
|
||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
||||
break;
|
||||
//isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
||||
//break;
|
||||
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
|
||||
default:
|
||||
// this is the default for non stabi modes also
|
||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
||||
break;
|
||||
//isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
||||
//break;
|
||||
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return isAssistedFlag;
|
||||
//return isAssistedFlag;
|
||||
return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
}
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
#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) */
|
||||
|
||||
/**
|
||||
* @}
|
||||
|
||||
@ -35,6 +35,8 @@
|
||||
#include <stabilizationdesired.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <flightstatus.h>
|
||||
#include <systemident.h>
|
||||
|
||||
// Private constants
|
||||
|
||||
@ -47,6 +49,22 @@ static float applyExpo(float value, float expo);
|
||||
static uint8_t currentFpvTiltAngle = 0;
|
||||
static float cosAngle = 0.0f;
|
||||
static float sinAngle = 0.0f;
|
||||
#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) */
|
||||
|
||||
static float applyExpo(float value, float expo)
|
||||
{
|
||||
@ -74,9 +92,11 @@ static float applyExpo(float value, float expo)
|
||||
* @input: ManualControlCommand
|
||||
* @output: StabilizationDesired
|
||||
*/
|
||||
void stabilizedHandler(bool newinit)
|
||||
void stabilizedHandler(__attribute__((unused)) bool newinit)
|
||||
{
|
||||
if (newinit) {
|
||||
static bool inited=false;
|
||||
if (!inited) {
|
||||
inited = true;
|
||||
StabilizationDesiredInitialize();
|
||||
StabilizationBankInitialize();
|
||||
}
|
||||
@ -116,7 +136,6 @@ void stabilizedHandler(bool newinit)
|
||||
uint8_t *stab_settings;
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
||||
@ -136,6 +155,15 @@ void stabilizedHandler(bool newinit)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
|
||||
break;
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
||||
// if (SystemIdentHandle()) {
|
||||
stab_settings = (uint8_t *)&autotuneSettings;
|
||||
// } else {
|
||||
// stab_settings = (uint8_t *)&attitudeSettings;
|
||||
// }
|
||||
break;
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
default:
|
||||
// Major error, this should not occur because only enter this block when one of these is true
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
@ -153,6 +181,9 @@ void stabilizedHandler(bool newinit)
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Roll * stabSettings.RollMax :
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
0; // this is an invalid mode
|
||||
|
||||
stabilization.Pitch =
|
||||
@ -165,6 +196,9 @@ void stabilizedHandler(bool newinit)
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Pitch * stabSettings.PitchMax :
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
0; // this is an invalid mode
|
||||
|
||||
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
||||
@ -187,6 +221,9 @@ void stabilizedHandler(bool newinit)
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
0; // this is an invalid mode
|
||||
}
|
||||
|
||||
|
||||
@ -184,6 +184,9 @@ const LedSequence_t *flightModeMap[] = {
|
||||
[FLIGHTSTATUS_FLIGHTMODE_POI] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND],
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
};
|
||||
|
||||
// List of alarms to show with attached sequences for each status
|
||||
|
||||
@ -9,7 +9,7 @@
|
||||
* @{
|
||||
*
|
||||
* @file innerloop.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
@ -51,6 +51,10 @@
|
||||
#include <virtualflybar.h>
|
||||
#include <cruisecontrol.h>
|
||||
#include <sanitycheck.h>
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
#include <systemident.h>
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
// Private constants
|
||||
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
|
||||
@ -60,6 +64,16 @@
|
||||
#define UPDATE_MAX 1.0f
|
||||
#define UPDATE_ALPHA 1.0e-2f
|
||||
|
||||
#define SYSTEM_IDENT_PERIOD ((uint32_t) 75)
|
||||
|
||||
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
#define powapprox fastpow
|
||||
#define expapprox fastexp
|
||||
#else
|
||||
#define powapprox powf
|
||||
#define expapprox expf
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
// Private variables
|
||||
static DelayedCallbackInfo *callbackHandle;
|
||||
static float gyro_filtered[3] = { 0, 0, 0 };
|
||||
@ -69,6 +83,10 @@ static PiOSDeltatimeConfig timeval;
|
||||
static float speedScaleFactor = 1.0f;
|
||||
static bool frame_is_multirotor;
|
||||
static bool measuredDterm_enabled;
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
static uint32_t system_ident_timeval = 0;
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
// Private functions
|
||||
static void stabilizationInnerloopTask();
|
||||
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
@ -86,6 +104,9 @@ void stabilizationInnerloopInit()
|
||||
ManualControlCommandInitialize();
|
||||
StabilizationDesiredInitialize();
|
||||
ActuatorDesiredInitialize();
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
SystemIdentInitialize();
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
#ifdef REVOLUTION
|
||||
AirspeedStateInitialize();
|
||||
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
|
||||
@ -100,6 +121,10 @@ void stabilizationInnerloopInit()
|
||||
|
||||
frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
|
||||
measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE);
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
// Settings for system identification
|
||||
system_ident_timeval = PIOS_DELAY_GetRaw();
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
}
|
||||
|
||||
static float get_pid_scale_source_value()
|
||||
@ -230,7 +255,6 @@ static void stabilizationInnerloopTask()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
RateDesiredData rateDesired;
|
||||
ActuatorDesiredData actuator;
|
||||
StabilizationStatusInnerLoopData enabled;
|
||||
@ -286,6 +310,7 @@ static void stabilizationInnerloopTask()
|
||||
// IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
|
||||
// keep order as it is, RATE must follow!
|
||||
case STABILIZATIONSTATUS_INNERLOOP_RATE:
|
||||
{
|
||||
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
|
||||
rate[t] = boundf(rate[t],
|
||||
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
|
||||
@ -293,6 +318,7 @@ static void stabilizationInnerloopTask()
|
||||
);
|
||||
pid_scaler scaler = create_pid_scaler(t);
|
||||
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
|
||||
}
|
||||
break;
|
||||
case STABILIZATIONSTATUS_INNERLOOP_ACRO:
|
||||
{
|
||||
@ -312,6 +338,263 @@ static void stabilizationInnerloopTask()
|
||||
actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
|
||||
}
|
||||
break;
|
||||
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT:
|
||||
{
|
||||
static uint32_t ident_iteration = 0;
|
||||
static float ident_offsets[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();
|
||||
|
||||
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++;
|
||||
|
||||
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 & 0x07) {
|
||||
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;
|
||||
}
|
||||
#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];
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
//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) };
|
||||
|
||||
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;
|
||||
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
|
||||
#endif
|
||||
|
||||
//aborted mod 9
|
||||
#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;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
rate[t] = boundf(rate[t],
|
||||
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
|
||||
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
|
||||
);
|
||||
pid_scaler scaler = create_pid_scaler(t);
|
||||
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
|
||||
actuatorDesiredAxis[t] += ident_offsets[t];
|
||||
// we shouldn't do any clamping until after the motors are calculated and scaled?
|
||||
//actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
|
||||
}
|
||||
break;
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
|
||||
default:
|
||||
actuatorDesiredAxis[t] = rate[t];
|
||||
|
||||
@ -141,6 +141,24 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
|
||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
// roll or pitch
|
||||
if (t<=1) {
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
}
|
||||
// else yaw (other modes don't worry about invalid thrust mode either)
|
||||
else {
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
}
|
||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT;
|
||||
break;
|
||||
#else /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
// no break, do not reorder this code
|
||||
// for low power FCs just fall through to Attitude mode
|
||||
// that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code
|
||||
// default:
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
|
||||
@ -39,6 +39,7 @@ extern uint32_t PIOS_DELAY_GetuS();
|
||||
extern uint32_t PIOS_DELAY_GetuSSince(uint32_t t);
|
||||
extern uint32_t PIOS_DELAY_GetRaw();
|
||||
extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw);
|
||||
extern uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later);
|
||||
|
||||
#endif /* PIOS_DELAY_H */
|
||||
|
||||
|
||||
@ -168,6 +168,16 @@ uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
|
||||
return diff / us_ticks;
|
||||
}
|
||||
|
||||
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
||||
/**
|
||||
* @brief Subrtact two raw times and convert to us.
|
||||
* @return Interval between raw times in microseconds
|
||||
*/
|
||||
uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) {
|
||||
return (later - raw) / us_ticks;
|
||||
}
|
||||
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
||||
|
||||
#endif /* PIOS_INCLUDE_DELAY */
|
||||
|
||||
/**
|
||||
|
||||
@ -54,6 +54,7 @@ MODULES += Notify
|
||||
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
OPTMODULES += AutoTune
|
||||
|
||||
SRC += $(FLIGHTLIB)/notification.c
|
||||
|
||||
|
||||
@ -125,6 +125,7 @@ UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemident
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -53,6 +53,7 @@ MODULES += Notify
|
||||
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
OPTMODULES += AutoTune
|
||||
|
||||
SRC += $(FLIGHTLIB)/notification.c
|
||||
|
||||
|
||||
@ -125,6 +125,7 @@ UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += txpidstatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += systemident
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -137,6 +137,7 @@ UAVOBJS = \
|
||||
$${UAVOBJ_XML_DIR}/statusvtolautotakeoff.xml \
|
||||
$${UAVOBJ_XML_DIR}/statusvtolland.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemalarms.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemident.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemsettings.xml \
|
||||
$${UAVOBJ_XML_DIR}/systemstats.xml \
|
||||
$${UAVOBJ_XML_DIR}/takeofflocation.xml \
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
|
||||
<field name="Stabilization1Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,AxisLock,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
@ -16,7 +16,7 @@
|
||||
/>
|
||||
<field name="Stabilization2Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,Rate,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
@ -25,7 +25,7 @@
|
||||
/>
|
||||
<field name="Stabilization3Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Rate,Rate,Rate,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
@ -34,7 +34,7 @@
|
||||
/>
|
||||
<field name="Stabilization4Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,AxisLock,CruiseControl"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
@ -43,7 +43,7 @@
|
||||
/>
|
||||
<field name="Stabilization5Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Attitude,Attitude,Rate,CruiseControl"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
@ -52,7 +52,7 @@
|
||||
/>
|
||||
<field name="Stabilization6Settings" units="" type="enum"
|
||||
elementnames="Roll,Pitch,Yaw,Thrust"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
|
||||
options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"
|
||||
defaultvalue="Rate,Rate,Rate,Manual"
|
||||
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
|
||||
@ -66,7 +66,7 @@
|
||||
units=""
|
||||
type="enum"
|
||||
elements="6"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"
|
||||
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff,AutoTune"
|
||||
defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6"
|
||||
limits="%NE:POI:AutoCruise; \
|
||||
%NE:POI:AutoCruise; \
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
|
||||
|
||||
<!-- Note these enumerated values should be the same as ManualControlSettings -->
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"/>
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff,AutoTune"/>
|
||||
<!-- Keep Armed (first pos) and FlightMode (second pos) before this line for OSD compatibility -->
|
||||
<field name="AlwaysStabilizeWhenArmed" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
|
||||
|
||||
@ -29,7 +29,7 @@
|
||||
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
|
||||
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
|
||||
|
||||
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,Fault,Altitude,Airspeed,TxPID,Battery,Overo,MagBaro,OsdHk" options="Disabled,Enabled" defaultvalue="Disabled"/>
|
||||
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,Fault,Altitude,Airspeed,TxPID,Battery,Overo,MagBaro,OsdHk,AutoTune" options="Disabled,Enabled" defaultvalue="Disabled"/>
|
||||
<field name="ADCRouting" units="" type="enum" elementnames="adc0,adc1,adc2,adc3,adc4,adc5,adc6,adc7" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/>
|
||||
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
|
||||
<field name="WS2811LED_Out" units="" type="enum" elements="1" options="ServoOut1,ServoOut2,ServoOut3,ServoOut4,ServoOut5,ServoOut6,FlexiIOPin3,FlexiIOPin4,Disabled" defaultvalue="Disabled"
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
<field name="Yaw" units="degrees" type="float" elements="1"/>
|
||||
<field name="Thrust" units="%" type="float" elements="1"/>
|
||||
<!-- These values should match those in FlightModeSettings.Stabilization{1,2,3}Settings -->
|
||||
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"/>
|
||||
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
<description>Contains status information to control submodules for stabilization.</description>
|
||||
|
||||
|
||||
<field name="OuterLoop" units="" type="enum" options="Direct,DirectWithLimits,Attitude,Rattitude,Weakleveling,Altitude,AltitudeVario">
|
||||
<field name="OuterLoop" units="" type="enum" options="Direct,DirectWithLimits,Attitude,Rattitude,Weakleveling,Altitude,AltitudeVario,SystemIdent">
|
||||
<elementnames>
|
||||
<elementname>Roll</elementname>
|
||||
<elementname>Pitch</elementname>
|
||||
@ -11,7 +11,7 @@
|
||||
<elementname>Thrust</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="InnerLoop" units="" type="enum" options="Direct,VirtualFlyBar,Acro+,AxisLock,Rate,CruiseControl">
|
||||
<field name="InnerLoop" units="" type="enum" options="Direct,VirtualFlyBar,Acro+,AxisLock,Rate,CruiseControl,SystemIdent">
|
||||
<elementnames>
|
||||
<elementname>Roll</elementname>
|
||||
<elementname>Pitch</elementname>
|
||||
|
||||
29
shared/uavobjectdefinition/systemident.xml
Normal file
29
shared/uavobjectdefinition/systemident.xml
Normal file
@ -0,0 +1,29 @@
|
||||
<xml>
|
||||
<object name="SystemIdent" singleinstance="true" settings="true" category="Control">
|
||||
<description>The input to the PID tuning.</description>
|
||||
<field name="Tau" units="s" type="float" elements="1" defaultvalue="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 damping 1.3 (130) and noise sensitivity 0.8 (08) for very smooth flight. -->
|
||||
<!-- Use damping 1.1 (110) and noise sensitivity 1.0 (10) for default flight. -->
|
||||
<!-- Use damping 1.0 (100) and noise sensitivity 1.3 (13) for very snappy flight. -->
|
||||
<!-- RateNoise is [0,30] default 10. -->
|
||||
<!-- RateDamp is [50,150] default 110. -->
|
||||
<field name="RateDamp" units="" type="uint8" elements="1" defaultvalue="110"/>
|
||||
<field name="RateNoise" units="" type="uint8" elements="1" defaultvalue="10"/>
|
||||
<field name="CalculateYaw" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
<field name="DestinationPidBank" units="" type="uint8" elements="1" defaultvalue="2"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="onchange" period="1000"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -32,6 +32,7 @@
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="Running" units="bool" type="enum">
|
||||
@ -65,6 +66,7 @@
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
<options>
|
||||
<option>False</option>
|
||||
@ -102,6 +104,7 @@
|
||||
<elementname>GPS</elementname>
|
||||
<elementname>OSDGen</elementname>
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<access gcs="readonly" flight="readwrite"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user