From 2105a49ca9b4fe62bd86f7ec59fd651195705f3e Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 28 Feb 2016 03:48:13 -0500 Subject: [PATCH 01/23] LP-76 add UAV defaults and remove UAV inits --- flight/libraries/sanitycheck.c | 26 +- flight/modules/AutoTune/autotune.c | 1105 +++++++++++++++++ flight/modules/ManualControl/armhandler.c | 3 + flight/modules/ManualControl/manualcontrol.c | 100 +- .../modules/ManualControl/stabilizedhandler.c | 43 +- flight/modules/Notify/inc/sequences.h | 3 + flight/modules/Stabilization/innerloop.c | 287 ++++- flight/modules/Stabilization/stabilization.c | 18 + flight/pios/inc/pios_delay.h | 1 + flight/pios/stm32f4xx/pios_delay.c | 10 + .../boards/revolution/firmware/Makefile | 1 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../targets/boards/revonano/firmware/Makefile | 1 + .../boards/revonano/firmware/UAVObjects.inc | 1 + .../gcs/src/plugins/uavobjects/uavobjects.pro | 1 + .../flightmodesettings.xml | 14 +- shared/uavobjectdefinition/flightstatus.xml | 2 +- shared/uavobjectdefinition/hwsettings.xml | 2 +- .../stabilizationdesired.xml | 2 +- .../stabilizationstatus.xml | 4 +- shared/uavobjectdefinition/systemident.xml | 29 + shared/uavobjectdefinition/taskinfo.xml | 3 + 22 files changed, 1619 insertions(+), 38 deletions(-) create mode 100644 flight/modules/AutoTune/autotune.c create mode 100644 shared/uavobjectdefinition/systemident.xml diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index bfb4b792b..844eb402e 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -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; } } diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c new file mode 100644 index 000000000..fc2b14d20 --- /dev/null +++ b/flight/modules/AutoTune/autotune.c @@ -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 +//#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 + +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 +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; iraw_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]; +} + +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index c44e8f3fe..e9d811c9e 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -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: diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index ec9a300b7..ce94846d5 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -47,7 +47,8 @@ #include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include -#endif +#include +#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) */ /** * @} diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 8cb6d3156..4ec0c5cc2 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -35,6 +35,8 @@ #include #include #include +#include +#include // 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 } diff --git a/flight/modules/Notify/inc/sequences.h b/flight/modules/Notify/inc/sequences.h index 3cf46d832..2e9099895 100644 --- a/flight/modules/Notify/inc/sequences.h +++ b/flight/modules/Notify/inc/sequences.h @@ -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 diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 629e87c20..f396bf551 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -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 #include #include +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) +#include +#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]; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index d9c3c01cb..ae6cb1663 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -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; diff --git a/flight/pios/inc/pios_delay.h b/flight/pios/inc/pios_delay.h index 4a2da6347..6bc298b21 100644 --- a/flight/pios/inc/pios_delay.h +++ b/flight/pios/inc/pios_delay.h @@ -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 */ diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index 55970d203..56c8afab7 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -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 */ /** diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 2327c2d71..5ae61fbb3 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -54,6 +54,7 @@ MODULES += Notify OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge +OPTMODULES += AutoTune SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 0b751c1d0..a10cbd5ec 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -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) ) diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile index 7e3701a2f..e7fa840cf 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -53,6 +53,7 @@ MODULES += Notify OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge +OPTMODULES += AutoTune SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/revonano/firmware/UAVObjects.inc b/flight/targets/boards/revonano/firmware/UAVObjects.inc index 0b751c1d0..a10cbd5ec 100644 --- a/flight/targets/boards/revonano/firmware/UAVObjects.inc +++ b/flight/targets/boards/revonano/firmware/UAVObjects.inc @@ -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) ) diff --git a/ground/gcs/src/plugins/uavobjects/uavobjects.pro b/ground/gcs/src/plugins/uavobjects/uavobjects.pro index 25372e372..1553dfb1f 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -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 \ diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 71f2b6453..8228649d9 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -7,7 +7,7 @@ - + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index e51a01934..56e6b2bf0 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -29,7 +29,7 @@ - + - + diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index 5126ed9c1..4f78fc8f3 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -3,7 +3,7 @@ Contains status information to control submodules for stabilization. - + Roll Pitch @@ -11,7 +11,7 @@ Thrust - + Roll Pitch diff --git a/shared/uavobjectdefinition/systemident.xml b/shared/uavobjectdefinition/systemident.xml new file mode 100644 index 000000000..b0579bde9 --- /dev/null +++ b/shared/uavobjectdefinition/systemident.xml @@ -0,0 +1,29 @@ + + + The input to the PID tuning. + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index b9facf626..4518b9250 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -32,6 +32,7 @@ GPS OSDGen UAVOMSPBridge + AutoTune @@ -65,6 +66,7 @@ GPS OSDGen UAVOMSPBridge + AutoTune @@ -102,6 +104,7 @@ GPS OSDGen UAVOMSPBridge + AutoTune From 3312d9892c743efb2995899a52abf8c92651526b Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 28 Feb 2016 18:37:34 -0500 Subject: [PATCH 02/23] LP-76 write calculated PIDs to requested stab bank --- flight/modules/AutoTune/autotune.c | 102 ++++++++++++++++++-- shared/uavobjectdefinition/systemalarms.xml | 1 + shared/uavobjectdefinition/systemident.xml | 6 +- 3 files changed, 99 insertions(+), 10 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index fc2b14d20..ee4b09971 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -270,7 +270,7 @@ 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 StabilizationBankManualRateData manualRate; static SystemSettingsAirframeTypeOptions airframe_type; static float gX[AF_NUMX] = {0}; static float gP[AF_NUMP] = {0}; @@ -280,6 +280,8 @@ SystemIdentData systemIdentData; 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]); +static uint8_t checkSettings(); +static void ComputeStabilizationAndSetPids(); #ifndef AT_QUEUE_NUMELEM #define AT_QUEUE_NUMELEM 18 @@ -397,8 +399,7 @@ typedef struct { } 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)); + 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]; @@ -431,7 +432,7 @@ static void UpdateStabilizationDesired(bool doingIdent) { stabDesired.Roll = manual_control_command.Roll * rollMax; stabDesired.Pitch = manual_control_command.Pitch * pitchMax; - stabDesired.Yaw = manual_control_command.Yaw * maximumRate.Yaw; + stabDesired.Yaw = manual_control_command.Yaw * manualRate.Yaw; if (doingIdent) { stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; @@ -450,6 +451,33 @@ static void UpdateStabilizationDesired(bool doingIdent) { } +static uint8_t checkSettings() +{ + uint8_t retVal = 0; + + // Check the axis gains + // Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values. + if (systemIdentData.Beta.Roll < 6) { + retVal |= 1; + } + if (systemIdentData.Beta.Pitch < 6) { + retVal |= 2; + } + + // 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; + } + // 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; + } + + return retVal; +} + + #if 0 void ComputeStabilization() { @@ -540,7 +568,7 @@ void ComputeStabilization() //stabSettingsBank.DerivativeCutoff = 1.0f / (2.0f*M_PI*tau_d); } #else -void ComputeStabilizationAndSetPids() +static void ComputeStabilizationAndSetPids() { StabilizationSettingsBank1Data stabSettingsBank; @@ -632,12 +660,15 @@ void ComputeStabilizationAndSetPids() switch (systemIdentData.DestinationPidBank) { case 1: StabilizationSettingsBank1Set((void *)&stabSettingsBank); + UAVObjSave(StabilizationSettingsBank1Handle(), 0); break; case 2: StabilizationSettingsBank2Set((void *)&stabSettingsBank); + UAVObjSave(StabilizationSettingsBank2Handle(), 0); break; case 3: StabilizationSettingsBank3Set((void *)&stabSettingsBank); + UAVObjSave(StabilizationSettingsBank3Handle(), 0); break; } } @@ -657,11 +688,13 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) float noise[3] = {0}; +// is this needed? af_init(gX,gP); uint32_t last_time = 0.0f; const uint32_t YIELD_MS = 2; +// should this be put in Init()? GyroSensorConnectCallback(at_new_gyro_data); bool save_needed = false; @@ -683,12 +716,21 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) FlightStatusGet(&flightStatus); //this seems to lock up on Nano +// maybe it was only the first time when allocating new flash block? if (save_needed) { if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { + uint8_t failureBits; // Save the settings locally. UAVObjSave(SystemIdentHandle(), 0); + failureBits = checkSettings(); + if (!failureBits) { + ComputeStabilizationAndSetPids(); + } else { + // raise a warning + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING, + SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); + } state = AT_INIT; - save_needed = false; } } @@ -709,7 +751,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) // moved from UpdateStabilizationDesired() StabilizationBankRollMaxGet(&rollMax); StabilizationBankPitchMaxGet(&pitchMax); - StabilizationBankMaximumRateGet(&maximumRate); + StabilizationBankManualRateGet(&manualRate); SystemSettingsAirframeTypeGet(&airframe_type); // Reset save status; only save if this tune @@ -720,6 +762,26 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) // Only start when armed and flying if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { +#if 1 + SystemIdentGet(&systemIdentData); + // these are values that could be changed by the user + // save them through the following xSetDefaults() call + uint8_t rateDamp = systemIdentData.RateDamp; + uint8_t rateNoise = systemIdentData.RateNoise; + bool calcYaw = systemIdentData.CalculateYaw; + uint8_t destBank = systemIdentData.DestinationPidBank; + + // 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.RateDamp = rateDamp; + systemIdentData.RateNoise = rateNoise; + systemIdentData.CalculateYaw = calcYaw; + systemIdentData.DestinationPidBank = destBank; +#endif // 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); @@ -1074,6 +1136,16 @@ static void af_init(float X[AF_NUMX], float P[AF_NUMP]) // 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.RateDamp; + uint8_t noise = systemIdentData.RateNoise; + bool yaw = systemIdentData.CalculateYaw; + uint8_t bank = systemIdentData.DestinationPidBank; + // 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])); @@ -1081,6 +1153,22 @@ static void af_init(float X[AF_NUMX], float P[AF_NUMP]) SystemIdentBetaArrayGet(&X[6]); SystemIdentTauGet(&X[9]); + // restore the user changeable values + systemIdentData.RateDamp = damp; + systemIdentData.RateNoise = 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)); + //SystemIdentTauGet(&X[9]); + X[9] = systemIdentData.Tau; +#endif + // P initialization // Could zero this like: *P = *((float [AF_NUMP]){}); memset(P, 0, AF_NUMP*sizeof(P[0])); diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index ba4cd24de..7cf9d55ed 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -37,6 +37,7 @@ + diff --git a/shared/uavobjectdefinition/systemident.xml b/shared/uavobjectdefinition/systemident.xml index b0579bde9..cecb94984 100644 --- a/shared/uavobjectdefinition/systemident.xml +++ b/shared/uavobjectdefinition/systemident.xml @@ -1,7 +1,7 @@ The input to the PID tuning. - + @@ -19,8 +19,8 @@ - - + + From db075503030488fbbd840f7d1bf8926527858f88 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 4 Mar 2016 11:16:17 -0500 Subject: [PATCH 03/23] LP-76 smooth to quick slider on accessoryx untested --- flight/modules/AutoTune/autotune.c | 914 ++++++++++-------- .../boards/discoveryf4bare/firmware/Makefile | 1 + .../discoveryf4bare/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/Makefile | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 3 + .../targets/boards/simposix/firmware/Makefile | 2 + .../boards/simposix/firmware/UAVObjects.inc | 3 + shared/uavobjectdefinition/systemident.xml | 27 +- 8 files changed, 528 insertions(+), 424 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index ee4b09971..a30611cba 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -36,19 +36,15 @@ #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 -//#include "pios_thread.h" #include "systemsettings.h" #include "taskinfo.h" @@ -57,9 +53,8 @@ #include "stabilizationsettingsbank1.h" #include "stabilizationsettingsbank2.h" #include "stabilizationsettingsbank3.h" +#include "accessorydesired.h" -//#include "circqueue.h" -//#include "misc_math.h" #define PIOS_malloc pios_malloc @@ -71,6 +66,10 @@ #define expapprox expf #endif /* defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ +//#define USE_CIRC_QUEUE + + +#if defined(USE_CIRC_QUEUE) /** ****************************************************************************** * @file circqueue.h @@ -144,7 +143,7 @@ circ_queue_t circ_queue_new(uint16_t elem_size, uint16_t num_elem) { 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; @@ -239,6 +238,8 @@ void circ_queue_read_completed(circ_queue_t q) { q->read_tail = next_pos(q->num_elem, read_tail); } +#endif + // Private constants #undef STACK_SIZE_BYTES @@ -251,6 +252,26 @@ void circ_queue_read_completed(circ_queue_t q) { #define AF_NUMX 13 #define AF_NUMP 43 +#if !defined(AT_QUEUE_NUMELEM) +#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 + +#if defined(USE_CIRC_QUEUE) +#define DEREFERENCE(str, ele) (str->ele) +#else +#define DEREFERENCE(str, ele) (str.ele) +#endif + +#define SMOOTH_QUICK_DISABLED 0 +#define SMOOTH_QUICK_AUX_BASE 10 +#define SMOOTH_QUICK_TOGGLE_BASE 21 + + // Private types enum AUTOTUNE_STATE { AT_INIT, AT_START, AT_RUN, AT_FINISHED, AT_WAITING }; @@ -262,97 +283,359 @@ struct at_queued_data { 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 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 airframe_type; +static SystemSettingsAirframeTypeOptions airframeType; static float gX[AF_NUMX] = {0}; static float gP[AF_NUMP] = {0}; SystemIdentData systemIdentData; +int8_t accessoryToUse; +int8_t flightModeSwitchTogglePosition; + // 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]); -static uint8_t checkSettings(); +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 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 UpdateStabilizationDesired(bool doingIdent); +static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); +static void InitSystemIdent(); -#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) +int32_t AutoTuneInitialize(void) { // Create a queue, connect to manual control command and flightstatus #ifdef MODULE_AutoTune_BUILTIN - module_enabled = true; + moduleEnabled = true; #else HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) { - module_enabled = true; + moduleEnabled = true; } else { - module_enabled = false; + moduleEnabled = false; } #endif - if (module_enabled) { + if (moduleEnabled) { SystemIdentInitialize(); - at_queue = circ_queue_new(sizeof(struct at_queued_data), - AT_QUEUE_NUMELEM); - if (!at_queue) { - module_enabled = false; +#if defined(USE_CIRC_QUEUE) + atQueue = circ_queue_new(sizeof(struct at_queued_data), AT_QUEUE_NUMELEM); +#else + atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data)); +#endif + if (!atQueue) { + moduleEnabled = false; } } return 0; } + /** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ -int32_t AutotuneStart(void) +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); + if (moduleEnabled) { + //taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY); //TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle); //PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); - xTaskCreate(AutotuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + GyroSensorConnectCallback(AtNewGyroData); + xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); } return 0; } -MODULE_INITCALL(AutotuneInitialize, AutotuneStart); -#if 0 -static void at_new_gyro_data(UAVObjEvent * ev, void *ctx, void *obj, int len) { - (void) ev; (void) ctx; +MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart); + + +/** + * Module thread, should not return. + */ +static void AutoTuneTask(__attribute__((unused)) void *parameters) +{ + enum AUTOTUNE_STATE state = AT_INIT; + uint32_t lastUpdateTime = xTaskGetTickCount(); + 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); + + // 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 + InitSystemIdent(); + + while (1) { + static uint32_t updateCounter = 0; + uint32_t diffTime; + 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 + + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { + if (saveSiNeeded) { + // Save SystemIdent to permanent settings + UAVObjSave(SystemIdentHandle(), 0); + saveSiNeeded = false; +//so how to restart if it failed and both saves are false + } + if (savePidNeeded) { + // Save SystemIdent to permanent settings + UAVObjSave(SystemIdentHandle(), 0); + // Save PIDs to permanent settings + switch (systemIdentData.DestinationPidBank) { + case 1: + UAVObjSave(StabilizationSettingsBank1Handle(), 0); + break; + case 2: + UAVObjSave(StabilizationSettingsBank2Handle(), 0); + break; + case 3: + UAVObjSave(StabilizationSettingsBank3Handle(), 0); + break; + } + savePidNeeded = false; + } +//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)) { + 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 + ++flightModeSwitchTogglePosition; + if (flightModeSwitchTogglePosition > systemIdentData.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; + } + ProportionPidsSmoothToQuick(0.0f, (float) flightModeSwitchTogglePosition, (float) (systemIdentData.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE)); + savePidNeeded = true; + } + + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + // if accessory0-3 is configured as a PID vario 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) { + static AccessoryDesiredData accessoryValueOld = { 0.0f }; +// static float accessoryValueOld = 0.0f; + AccessoryDesiredData accessoryValue; +// float accessoryValue; + AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); + if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (1.0f/900.0f)) { + accessoryValueOld = accessoryValue; + ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f); + savePidNeeded = true; + } + } + state = AT_INIT; + vTaskDelay(50 / portTICK_RATE_MS); + lastUpdateTime = xTaskGetTickCount(); + continue; + } + + switch(state) { + case AT_INIT: + diffTime = xTaskGetTickCount() - lastUpdateTime; + // Spend the first block of time in normal rate mode to get stabilized + if (diffTime > INIT_TIME_DELAY_MS) { + StabilizationBankRollMaxGet(&rollMax); + StabilizationBankPitchMaxGet(&pitchMax); + StabilizationBankManualRateGet(&manualRate); + SystemSettingsAirframeTypeGet(&airframeType); + // Reset save status; only save if this tune completes. + saveSiNeeded = false; + savePidNeeded = false; + lastUpdateTime = xTaskGetTickCount(); + + // Only start when armed and flying + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { + InitSystemIdent(); + AfInit(gX, gP); + UpdateSystemIdent(gX, NULL, 0.0f, 0, 0, 0.0f); + measureTime = (uint32_t)systemIdentData.TuningDuration * (uint32_t)1000; + state = AT_START; + lastUpdateTime = xTaskGetTickCount(); + } + } + break; + + case AT_START: + diffTime = xTaskGetTickCount() - lastUpdateTime; + // Spend the first block of time in normal rate mode to get stabilized + if (diffTime > START_TIME_DELAY_MS) { + 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 -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; + xQueueReset(atQueue); #endif + /* And reset the point spill counter */ + updateCounter = 0; + atPointsSpilled = 0; + throttleAccumulator = 0; + state = AT_RUN; + lastUpdateTime = xTaskGetTickCount(); + } + break; - static bool last_sample_unpushed = 0; + case AT_RUN: + diffTime = xTaskGetTickCount() - lastUpdateTime; + doingIdent = true; + canSleep = false; + + for (int i=0; i 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)); + 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]); + } + //This will work up to 8kHz with an 89% throttle position before overflow + throttleAccumulator += 10000 * DEREFERENCE(pt,throttle); + // Update uavo every 256 cycles to avoid + // telemetry spam + if (!((updateCounter++) & 0xff)) { + float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f; + UpdateSystemIdent(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 + systemIdentData.Complete = true; + 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); + // data is bad if FC was disarmed at the time AT completed + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { + saveSiNeeded = true; + failureBits = CheckSettings(); + if (!failureBits) { + ComputeStabilizationAndSetPids(); + savePidNeeded = true; + } else { +//is this right + // default to disable PID changing with flight mode switch and accessory0-3 + accessoryToUse = -1; + flightModeSwitchTogglePosition = -1; + // raise a warning + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING, + SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); + } + } + state = AT_WAITING; +// break; +// fall through in the unlikely case that it is disarmed at AT_FINISHED +// and armed before it ever gets to AT_WAITING the first time + + case AT_WAITING: + default: + // maybe set an alarm to notify user that tuning is done + break; + } + + // Update based on ManualControlCommand + UpdateStabilizationDesired(doingIdent); + if (canSleep) { + vTaskDelay(YIELD_MS / portTICK_RATE_MS); + } + } +} + + +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; @@ -365,39 +648,113 @@ typedef struct { 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++; +#if defined(USE_CIRC_QUEUE) + if (circ_queue_advance_write(atQueue)) { +#else + if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) { +#endif + atPointsSpilled++; } } - struct at_queued_data *q_item = circ_queue_cur_write_pos(at_queue); +#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 (circ_queue_advance_write(at_queue) != 0) { +#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; } } + +static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) { + static uint32_t lastUpdateTime; + static uint8_t flightModePrev; + static uint8_t counter; + uint32_t updateTime; + + // only count transitions into and out of autotune + if ((flightModePrev == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ^ (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE)) { + flightModePrev = flightMode; + updateTime = xTaskGetTickCount(); + // if it has been over 2 seconds, reset the counter + if (updateTime - lastUpdateTime > 2000) { + counter = 0; + } + // if the counter is reset, start a new time period + if (counter == 0) { + lastUpdateTime = updateTime; + } + // if flight mode has toggled into autotune 3 times but is currently not autotune + if (++counter >= 5 && flightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + counter = 0; + return true; + } + } + + return false; +} + + +static void InitSystemIdent() { + SystemIdentGet(&systemIdentData); + // these are values that could be changed by the user + // save them through the following xSetDefaults() call + uint8_t dampRate = systemIdentData.DampRate; + uint8_t noiseRate = systemIdentData.NoiseRate; + bool calcYaw = systemIdentData.CalculateYaw; + uint8_t destBank = systemIdentData.DestinationPidBank; + uint8_t smoothQuick = systemIdentData.SmoothQuick; + + // 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.DampRate = dampRate; + systemIdentData.NoiseRate = noiseRate; + systemIdentData.CalculateYaw = calcYaw; + systemIdentData.DestinationPidBank = destBank; + // default to disable PID changing with flight mode switch and accessory0-3 + accessoryToUse = -1; + flightModeSwitchTogglePosition = -1; + systemIdentData.SmoothQuick = 0; + switch (smoothQuick) { + case 10: // use accessory0 + case 11: // use accessory1 + case 12: // use accessory2 + case 13: // use accessory3 + accessoryToUse = smoothQuick - 10; + systemIdentData.SmoothQuick = smoothQuick; + break; + case 23: // use flight mode switch toggle with 3 points + case 25: // 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; + break; + } +} + + static void UpdateSystemIdent(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle) { systemIdentData.Beta.Roll = X[6]; @@ -423,6 +780,7 @@ static void UpdateSystemIdent(const float *X, const float *noise, SystemIdentSet(&systemIdentData); } + static void UpdateStabilizationDesired(bool doingIdent) { StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); @@ -444,14 +802,14 @@ static void UpdateStabilizationDesired(bool doingIdent) { stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } - stabDesired.Thrust = (airframe_type == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle; + stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle; // is this a race // control feels very sluggish too StabilizationDesiredSet(&stabDesired); } -static uint8_t checkSettings() +static uint8_t CheckSettings() { uint8_t retVal = 0; @@ -478,8 +836,7 @@ static uint8_t checkSettings() } -#if 0 -void ComputeStabilization() +static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) { StabilizationSettingsBank1Data stabSettingsBank; @@ -502,97 +859,8 @@ void ComputeStabilization() // 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 -static 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; + const double ghf = (double) noiseRate / 1000.0d; + const double damp = (double) dampRate / 100.0d; double tau = exp(systemIdentData.Tau); double beta_roll = systemIdentData.Beta.Roll; @@ -657,256 +925,70 @@ static void ComputeStabilizationAndSetPids() //stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d); + // Save PIDs to permanent settings switch (systemIdentData.DestinationPidBank) { case 1: StabilizationSettingsBank1Set((void *)&stabSettingsBank); - UAVObjSave(StabilizationSettingsBank1Handle(), 0); break; case 2: StabilizationSettingsBank2Set((void *)&stabSettingsBank); - UAVObjSave(StabilizationSettingsBank2Handle(), 0); break; case 3: StabilizationSettingsBank3Set((void *)&stabSettingsBank); - UAVObjSave(StabilizationSettingsBank3Handle(), 0); break; } } -#endif -#define MAX_PTS_PER_CYCLE 4 - -/** - * Module thread, should not return. - */ -static void AutotuneTask(__attribute__((unused)) void *parameters) +static void ComputeStabilizationAndSetPids() { - enum AUTOTUNE_STATE state = AT_INIT; - - uint32_t last_update_time = xTaskGetTickCount(); - - float noise[3] = {0}; - -// is this needed? - af_init(gX,gP); - - uint32_t last_time = 0.0f; - const uint32_t YIELD_MS = 2; - -// should this be put in Init()? - 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 -// maybe it was only the first time when allocating new flash block? - if (save_needed) { - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { - uint8_t failureBits; - // Save the settings locally. - UAVObjSave(SystemIdentHandle(), 0); - failureBits = checkSettings(); - if (!failureBits) { - ComputeStabilizationAndSetPids(); - } else { - // raise a warning - ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING, - SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); - } - 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); - StabilizationBankManualRateGet(&manualRate); - 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) { -#if 1 - SystemIdentGet(&systemIdentData); - // these are values that could be changed by the user - // save them through the following xSetDefaults() call - uint8_t rateDamp = systemIdentData.RateDamp; - uint8_t rateNoise = systemIdentData.RateNoise; - bool calcYaw = systemIdentData.CalculateYaw; - uint8_t destBank = systemIdentData.DestinationPidBank; - - // 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.RateDamp = rateDamp; - systemIdentData.RateNoise = rateNoise; - systemIdentData.CalculateYaw = calcYaw; - systemIdentData.DestinationPidBank = destBank; -#endif -// 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; iraw_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); - } - } + ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentData.DampRate, systemIdentData.NoiseRate); } + +// scale damp and noise to generate PIDs according to how a slider or other ratio is set +// +// when val is half way between min and max, it generates the default PIDs +// when val is min, it generates the smoothest configured PIDs +// when val is max, it generates the quickest configured PIDs +// +// when val is between min and (min+max)/2, it scales val over the range [min, (min+max)/2] to generate PIDs between smoothest and default +// when val is between (min+max)/2 and max, it scales val over the range [(min+max)/2, max] to generate PIDs between default and quickest +// +// this is done piecewise because we are not guaranteed that default-min == max-default +// but we are given that [smoothDamp,smoothNoise] [defaultDamp,defaultNoise] [quickDamp,quickNoise] are all good parameterizations +#define dampMin systemIdentData.DampMin +#define dampDefault systemIdentData.DampRate +#define dampMax systemIdentData.DampMax +#define noiseMin systemIdentData.DampMin +#define noiseDefault systemIdentData.DampRate +#define noiseMax systemIdentData.DampMax +static void ProportionPidsSmoothToQuick(float min, float val, float max) +{ + float ratio, damp, noise; + + // translate from range [min, max] to range [0, max-min] + // takes care of min < 0 too + val -= min; + max -= min; + ratio = val / max; + + if (ratio <= 0.5f) { + // scale ratio in [0,0.5] to produce PIDs in [smoothest,default] + ratio *= 2.0f; + damp = (dampMax * (1.0f - ratio)) + (dampDefault * ratio); + noise = (noiseMin * (1.0f - ratio)) + (noiseDefault * 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); + } + + ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise); +} + + /** * Prediction step for EKF on control inputs to quad that * learns the system properties @@ -915,7 +997,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) * @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) +__attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in) { const float Ts = dT_s; const float Tsq = Ts * Ts; @@ -1027,11 +1109,8 @@ __attribute__((always_inline)) static inline void af_predict(float X[AF_NUMX], f 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]); @@ -1115,13 +1194,14 @@ __attribute__((always_inline)) static inline void af_predict(float X[AF_NUMX], f 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 void AfInit(float X[AF_NUMX], float P[AF_NUMP]) { - static const float q_init[AF_NUMX] = { + static const float qInit[AF_NUMX] = { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.05f, 0.05f, 0.005f, @@ -1141,8 +1221,8 @@ static void af_init(float X[AF_NUMX], float P[AF_NUMP]) #if 0 // these are values that could be changed by the user // save them through the following xSetDefaults() call - uint8_t damp = systemIdentData.RateDamp; - uint8_t noise = systemIdentData.RateNoise; + uint8_t damp = systemIdentData.DampRate; + uint8_t noise = systemIdentData.NoiseRate; bool yaw = systemIdentData.CalculateYaw; uint8_t bank = systemIdentData.DestinationPidBank; @@ -1154,8 +1234,8 @@ static void af_init(float X[AF_NUMX], float P[AF_NUMP]) SystemIdentTauGet(&X[9]); // restore the user changeable values - systemIdentData.RateDamp = damp; - systemIdentData.RateNoise = noise; + systemIdentData.DampRate = damp; + systemIdentData.NoiseRate = noise; systemIdentData.CalculateYaw = yaw; systemIdentData.DestinationPidBank = bank; #else @@ -1172,19 +1252,19 @@ static void af_init(float X[AF_NUMX], float P[AF_NUMP]) // 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]; + P[0] = qInit[0]; + P[1] = qInit[1]; + P[2] = qInit[2]; + P[4] = qInit[3]; + P[6] = qInit[4]; + P[8] = qInit[5]; + P[11] = qInit[6]; + P[14] = qInit[7]; + P[17] = qInit[8]; + P[27] = qInit[9]; + P[32] = qInit[10]; + P[37] = qInit[11]; + P[42] = qInit[12]; } /** diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 1f46a4f1a..3ba0b5cb8 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -57,6 +57,7 @@ MODULES += Telemetry OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge +OPTMODULES += AutoTune SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index afc1c82d1..02940b780 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -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) ) diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 67ad81a59..3fd174560 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -54,6 +54,7 @@ SRC += $(FLIGHTLIB)/notification.c OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge +OPTMODULES += AutoTune # Include all camera options CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index c98bc205e..6a98d3bd9 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -124,6 +124,9 @@ UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation +# this was missing when systemident was added +# UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemident UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 7aa0f3511..97080b1a7 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -50,6 +50,8 @@ MODULES += Airspeed SRC += $(FLIGHTLIB)/notification.c +OPTMODULES += AutoTune + # Paths OPSYSTEM = . BOARDINC = .. diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 928a1c0dc..a9e2b9872 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -119,6 +119,9 @@ UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += takeofflocation +# this was missing when systemident was added +# UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemident UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/shared/uavobjectdefinition/systemident.xml b/shared/uavobjectdefinition/systemident.xml index cecb94984..07520113f 100644 --- a/shared/uavobjectdefinition/systemident.xml +++ b/shared/uavobjectdefinition/systemident.xml @@ -12,18 +12,31 @@ - - - + + + - - + + + + + + + + + + + + + + + - - + + From faf2fffb5ab686fa625d844584384d7b722a4e44 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 4 Mar 2016 23:21:23 -0500 Subject: [PATCH 04/23] LP-76 smooth quick PIDs via accessory0-3 and mode switch toggle --- flight/modules/AutoTune/autotune.c | 232 +++++++++++++++++++++++++---- 1 file changed, 199 insertions(+), 33 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index a30611cba..548c2f2e0 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -317,7 +317,7 @@ 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 UpdateStabilizationDesired(bool doingIdent); static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); -static void InitSystemIdent(); +static void InitSystemIdent(bool loadDefaults); /** @@ -397,7 +397,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // 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 - InitSystemIdent(); + InitSystemIdent(false); while (1) { static uint32_t updateCounter = 0; @@ -415,14 +415,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (saveSiNeeded) { + saveSiNeeded = false; // Save SystemIdent to permanent settings UAVObjSave(SystemIdentHandle(), 0); - saveSiNeeded = false; //so how to restart if it failed and both saves are false } if (savePidNeeded) { - // Save SystemIdent to permanent settings - UAVObjSave(SystemIdentHandle(), 0); + savePidNeeded = false; // Save PIDs to permanent settings switch (systemIdentData.DestinationPidBank) { case 1: @@ -435,8 +434,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) UAVObjSave(StabilizationSettingsBank3Handle(), 0); break; } - savePidNeeded = false; } +// 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; } @@ -444,7 +443,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // 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)) { + if (flightModeSwitchTogglePosition!=-1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentData.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 ++flightModeSwitchTogglePosition; @@ -466,7 +465,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // 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) { + if (accessoryToUse != -1 && systemIdentData.Complete && !CheckSettings()) { static AccessoryDesiredData accessoryValueOld = { 0.0f }; // static float accessoryValueOld = 0.0f; AccessoryDesiredData accessoryValue; @@ -496,11 +495,12 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // Reset save status; only save if this tune completes. saveSiNeeded = false; savePidNeeded = false; +// systemIdentData.Complete = false; lastUpdateTime = xTaskGetTickCount(); // Only start when armed and flying if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { - InitSystemIdent(); + InitSystemIdent(true); AfInit(gX, gP); UpdateSystemIdent(gX, NULL, 0.0f, 0, 0, 0.0f); measureTime = (uint32_t)systemIdentData.TuningDuration * (uint32_t)1000; @@ -581,7 +581,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) } if (diffTime > measureTime) { // Move on to next state // permanent flag that AT is complete and PIDs can be calculated - systemIdentData.Complete = true; state = AT_FINISHED; lastUpdateTime = xTaskGetTickCount(); } @@ -599,20 +598,20 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (!failureBits) { 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 ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING, SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); } } state = AT_WAITING; -// break; -// fall through in the unlikely case that it is disarmed at AT_FINISHED -// and armed before it ever gets to AT_WAITING the first time + break; case AT_WAITING: default: @@ -713,26 +712,42 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) { } -static void InitSystemIdent() { +static void InitSystemIdent(bool loadDefaults) { SystemIdentGet(&systemIdentData); - // these are values that could be changed by the user - // save them through the following xSetDefaults() call - uint8_t dampRate = systemIdentData.DampRate; - uint8_t noiseRate = systemIdentData.NoiseRate; - bool calcYaw = systemIdentData.CalculateYaw; - uint8_t destBank = systemIdentData.DestinationPidBank; uint8_t smoothQuick = systemIdentData.SmoothQuick; - // 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); + 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; + } - // restore the user changeable values - systemIdentData.DampRate = dampRate; - systemIdentData.NoiseRate = noiseRate; - systemIdentData.CalculateYaw = calcYaw; - systemIdentData.DestinationPidBank = destBank; // default to disable PID changing with flight mode switch and accessory0-3 accessoryToUse = -1; flightModeSwitchTogglePosition = -1; @@ -791,6 +806,7 @@ static void UpdateStabilizationDesired(bool doingIdent) { 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; if (doingIdent) { stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; @@ -801,8 +817,8 @@ static void UpdateStabilizationDesired(bool doingIdent) { stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle; // is this a race // control feels very sluggish too StabilizationDesiredSet(&stabDesired); @@ -836,6 +852,7 @@ static uint8_t CheckSettings() } +#if 0 static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) { StabilizationSettingsBank1Data stabSettingsBank; @@ -863,6 +880,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float 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; @@ -871,7 +889,16 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float 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 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); @@ -938,6 +965,145 @@ 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 static void ComputeStabilizationAndSetPids() @@ -960,9 +1126,9 @@ static void ComputeStabilizationAndSetPids() #define dampMin systemIdentData.DampMin #define dampDefault systemIdentData.DampRate #define dampMax systemIdentData.DampMax -#define noiseMin systemIdentData.DampMin -#define noiseDefault systemIdentData.DampRate -#define noiseMax 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; From 78dffae5a19014109f3830bdd302ffdcde0a2aa1 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Mon, 7 Mar 2016 21:57:29 -0500 Subject: [PATCH 05/23] LP-76 disable stabHandler for AutoTune mode --- flight/modules/AutoTune/autotune.c | 8 ++++++-- flight/modules/ManualControl/stabilizedhandler.c | 9 +++++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 548c2f2e0..2e2e1c402 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -798,7 +798,8 @@ static void UpdateSystemIdent(const float *X, const float *noise, static void UpdateStabilizationDesired(bool doingIdent) { StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); + // unneeded since we are setting everything in this uavo + //StabilizationDesiredGet(&stabDesired); ManualControlCommandData manual_control_command; ManualControlCommandGet(&manual_control_command); @@ -806,7 +807,8 @@ static void UpdateStabilizationDesired(bool doingIdent) { 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 = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle; + stabDesired.Thrust = manual_control_command.Thrust; if (doingIdent) { stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; @@ -829,6 +831,7 @@ static uint8_t CheckSettings() { 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) { @@ -847,6 +850,7 @@ static uint8_t CheckSettings() else if (expf(systemIdentData.Tau) < 0.008f) { retVal |= 8; } +#endif return retVal; } diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 4ec0c5cc2..f7e4132f3 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -49,6 +49,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, @@ -65,6 +66,7 @@ static FlightModeSettingsStabilization1SettingsData attitudeSettings = { }; #endif #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ +#endif static float applyExpo(float value, float expo) { @@ -100,6 +102,7 @@ void stabilizedHandler(__attribute__((unused)) bool newinit) StabilizationDesiredInitialize(); StabilizationBankInitialize(); } + ManualControlCommandData cmd; ManualControlCommandGet(&cmd); @@ -157,12 +160,18 @@ 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 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 From ab50008e07a2a0d0092b801fcccfb4441a1f81c5 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 9 Mar 2016 02:06:37 -0500 Subject: [PATCH 06/23] LP-76 fix no control first time during 2 second startup --- flight/modules/AutoTune/autotune.c | 48 ++++++++++++++-------- shared/uavobjectdefinition/systemident.xml | 2 +- 2 files changed, 32 insertions(+), 18 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 2e2e1c402..fe6f2eba6 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -257,7 +257,7 @@ void circ_queue_read_completed(circ_queue_t q) { #endif #define MAX_PTS_PER_CYCLE 4 -#define START_TIME_DELAY_MS 2000 +//#define START_TIME_DELAY_MS 2000 #define INIT_TIME_DELAY_MS 2000 #define YIELD_MS 2 @@ -273,7 +273,7 @@ void circ_queue_read_completed(circ_queue_t q) { // Private types -enum AUTOTUNE_STATE { AT_INIT, AT_START, AT_RUN, AT_FINISHED, AT_WAITING }; +enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_START, AT_RUN, AT_FINISHED, AT_WAITING }; struct at_queued_data { float y[3]; /* Gyro measurements */ @@ -297,7 +297,7 @@ static volatile uint32_t atPointsSpilled; static uint32_t throttleAccumulator; static uint8_t rollMax, pitchMax; static StabilizationBankManualRateData manualRate; -static SystemSettingsAirframeTypeOptions airframeType; +//static SystemSettingsAirframeTypeOptions airframeType; static float gX[AF_NUMX] = {0}; static float gP[AF_NUMP] = {0}; SystemIdentData systemIdentData; @@ -383,7 +383,7 @@ MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart); static void AutoTuneTask(__attribute__((unused)) void *parameters) { enum AUTOTUNE_STATE state = AT_INIT; - uint32_t lastUpdateTime = xTaskGetTickCount(); + uint32_t lastUpdateTime = 0; // initialization is only for compiler warning float noise[3] = {0}; // is this needed? // AfInit(gX,gP); @@ -479,41 +479,55 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) } state = AT_INIT; vTaskDelay(50 / portTICK_RATE_MS); - lastUpdateTime = xTaskGetTickCount(); continue; } switch(state) { case AT_INIT: + // beware that control comes here every time the user toggles the flight mode switch into AutoTune + // and it isn't appropriate to reset the main state here + // 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); + 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 if (diffTime > INIT_TIME_DELAY_MS) { - StabilizationBankRollMaxGet(&rollMax); - StabilizationBankPitchMaxGet(&pitchMax); - StabilizationBankManualRateGet(&manualRate); - SystemSettingsAirframeTypeGet(&airframeType); - // Reset save status; only save if this tune completes. - saveSiNeeded = false; - savePidNeeded = false; -// systemIdentData.Complete = false; - lastUpdateTime = xTaskGetTickCount(); - // 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 } +//// 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) @@ -582,7 +596,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (diffTime > measureTime) { // Move on to next state // permanent flag that AT is complete and PIDs can be calculated state = AT_FINISHED; - lastUpdateTime = xTaskGetTickCount(); +// lastUpdateTime = xTaskGetTickCount(); } break; @@ -591,9 +605,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) 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 if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { - saveSiNeeded = true; failureBits = CheckSettings(); if (!failureBits) { ComputeStabilizationAndSetPids(); diff --git a/shared/uavobjectdefinition/systemident.xml b/shared/uavobjectdefinition/systemident.xml index 07520113f..5e05b9da3 100644 --- a/shared/uavobjectdefinition/systemident.xml +++ b/shared/uavobjectdefinition/systemident.xml @@ -23,7 +23,7 @@ - + From a217d0015fd6ef7763dd09317f255517fd462f65 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 11 Mar 2016 16:27:13 -0500 Subject: [PATCH 07/23] LP-76 better yaw-calc yaw by default-yaw overrides-cleanup for review --- flight/modules/AutoTune/autotune.c | 1108 +++++++---------- flight/modules/Autotune/autotune.c | 337 ----- flight/modules/Autotune/inc/autotune.h | 37 - flight/modules/ManualControl/manualcontrol.c | 57 - .../modules/ManualControl/stabilizedhandler.c | 29 +- flight/modules/Stabilization/innerloop.c | 442 ++++--- flight/modules/Stabilization/stabilization.c | 2 +- .../discoveryf4bare/firmware/UAVObjects.inc | 3 +- .../boards/revolution/firmware/UAVObjects.inc | 3 +- .../boards/revonano/firmware/UAVObjects.inc | 3 +- .../boards/revoproto/firmware/UAVObjects.inc | 3 +- .../boards/simposix/firmware/UAVObjects.inc | 3 +- .../gcs/src/plugins/uavobjects/uavobjects.pro | 3 +- ...ystemident.xml => systemidentsettings.xml} | 22 +- .../uavobjectdefinition/systemidentstate.xml | 19 + 15 files changed, 760 insertions(+), 1311 deletions(-) delete mode 100644 flight/modules/Autotune/autotune.c delete mode 100644 flight/modules/Autotune/inc/autotune.h rename shared/uavobjectdefinition/{systemident.xml => systemidentsettings.xml} (69%) create mode 100644 shared/uavobjectdefinition/systemidentstate.xml diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index fe6f2eba6..d78dadf69 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -43,11 +43,11 @@ #include "actuatordesired.h" #include "stabilizationdesired.h" #include "stabilizationsettings.h" -#include "systemident.h" +#include "systemidentsettings.h" +#include "systemidentstate.h" #include #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 - -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 -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 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]; diff --git a/flight/modules/Autotune/autotune.c b/flight/modules/Autotune/autotune.c deleted file mode 100644 index 60166dede..000000000 --- a/flight/modules/Autotune/autotune.c +++ /dev/null @@ -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 - -#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; - } -} - -/** - * @} - * @} - */ diff --git a/flight/modules/Autotune/inc/autotune.h b/flight/modules/Autotune/inc/autotune.h deleted file mode 100644 index 64c556a8e..000000000 --- a/flight/modules/Autotune/inc/autotune.h +++ /dev/null @@ -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 diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index ce94846d5..7f77eedf6 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -47,7 +47,6 @@ #include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include -#include #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) */ /** diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index f7e4132f3..560c0b827 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -36,7 +36,6 @@ #include #include #include -#include // 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 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 diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index f396bf551..2cd2385f6 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -52,7 +52,7 @@ #include #include #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) -#include +#include #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); } diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index ae6cb1663..d94da6385 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -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; diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 02940b780..4780f4453 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -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) ) diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index a10cbd5ec..191f50460 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -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) ) diff --git a/flight/targets/boards/revonano/firmware/UAVObjects.inc b/flight/targets/boards/revonano/firmware/UAVObjects.inc index a10cbd5ec..191f50460 100644 --- a/flight/targets/boards/revonano/firmware/UAVObjects.inc +++ b/flight/targets/boards/revonano/firmware/UAVObjects.inc @@ -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) ) diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 6a98d3bd9..af257283d 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -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) ) diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index a9e2b9872..e9d21d659 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -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) ) diff --git a/ground/gcs/src/plugins/uavobjects/uavobjects.pro b/ground/gcs/src/plugins/uavobjects/uavobjects.pro index 1553dfb1f..92be03832 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -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 \ diff --git a/shared/uavobjectdefinition/systemident.xml b/shared/uavobjectdefinition/systemidentsettings.xml similarity index 69% rename from shared/uavobjectdefinition/systemident.xml rename to shared/uavobjectdefinition/systemidentsettings.xml index 5e05b9da3..884606fec 100644 --- a/shared/uavobjectdefinition/systemident.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -1,15 +1,9 @@ - + The input to the PID tuning. - - - - - - @@ -23,20 +17,24 @@ - + + + - - - + + + + + - + diff --git a/shared/uavobjectdefinition/systemidentstate.xml b/shared/uavobjectdefinition/systemidentstate.xml new file mode 100644 index 000000000..457ef3515 --- /dev/null +++ b/shared/uavobjectdefinition/systemidentstate.xml @@ -0,0 +1,19 @@ + + + The input to the PID tuning. + + + + + + + + + + + + + + + + From b48ed29fc5a3e49a370f0089f1122e4e5af6ccf2 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 12 Mar 2016 18:19:09 -0500 Subject: [PATCH 08/23] LP-76 easier to understand yaw PID limiting --- flight/modules/AutoTune/autotune.c | 54 ++++++++++++++----- .../systemidentsettings.xml | 16 +++--- 2 files changed, 50 insertions(+), 20 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index d78dadf69..0ee0d522f 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -668,7 +668,7 @@ static uint8_t CheckSettingsRaw() if (systemIdentState.Beta.Pitch < 6) { retVal |= PITCH_BETA_LOW; } - if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) { + if (systemIdentState.Beta.Yaw < 4) { retVal |= YAW_BETA_LOW; } // Check the response speed @@ -693,9 +693,10 @@ static uint8_t CheckSettings() { uint8_t retVal = CheckSettingsRaw(); - if (systemIdentSettings.DisableSanityChecks - || ((retVal == YAW_BETA_LOW) && (!systemIdentSettings.CalculateYaw || systemIdentSettings.OverrideYawBeta))) { - retVal = 0; + if (systemIdentSettings.DisableSanityChecks) { + retVal = 0; + } else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) { + retVal &= ~YAW_BETA_LOW; } return retVal; @@ -758,19 +759,46 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d/wn); // 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]); - } - + float kpMax = 0.0f; + for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) { + double 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; double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d; + if (i<2) { + if (kpMax < (float) kp) { + kpMax = (float) kp; + } + } else { + // use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value + // use largest roll/pitch kp because it is the axis most slowed by rotational inertia + // and yaw is also slowed maximally by rotational inertia + // note that kp, ki, kd are all proportional in beta + // so reducing them all proportionally is the same as changing beta + float min = 0.0f; + float max = 0.0f; + switch (systemIdentSettings.YawPIDRatioFunction) { + case SYSTEMIDENTSETTINGS_YAWPIDRATIOFUNCTION_DISABLE: + max = 1000.0f; + min = 0.0f; + break; + case SYSTEMIDENTSETTINGS_YAWPIDRATIOFUNCTION_LIMIT: + max = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMax; + min = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMin; + break; + } + float ratio = 1.0f; + if (min > 0.0f && (float) kp < min) { + ratio = (float) kp / min; + } else if (max > 0.0f && (float) kp > max) { + ratio = (float) kp / max; + } + kp /= (double) ratio; + ki /= (double) ratio; + kd /= (double) ratio; + } + switch(i) { case 0: // Roll stabSettingsBank.RollRatePID.Kp = kp; diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 884606fec..8df05c0fc 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -17,18 +17,20 @@ - - - + + + + + + - + - - - + + From fd22011bfa13b8d3e1c39543fb937d6d6d047e55 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Mon, 14 Mar 2016 16:58:06 -0400 Subject: [PATCH 09/23] LP-76 get ready for PR uncrustify and comments --- flight/libraries/sanitycheck.c | 12 +- flight/modules/AutoTune/autotune.c | 857 +++++++++--------- flight/modules/ManualControl/manualcontrol.c | 10 +- .../modules/ManualControl/stabilizedhandler.c | 4 +- flight/modules/Notify/inc/sequences.h | 2 +- flight/modules/Stabilization/innerloop.c | 305 +------ flight/modules/Stabilization/stabilization.c | 8 +- flight/pios/stm32f4xx/pios_delay.c | 3 +- 8 files changed, 476 insertions(+), 725 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 844eb402e..51ae6b79f 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -274,12 +274,12 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter #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 + // running then set an alarm now that autotune module initializes the // appropriate objects - //if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) && - // (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))) { - // return false; - //} + // if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) && + // (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))) { + // return false; + // } #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ } #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) @@ -294,7 +294,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter // 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; } } diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 0ee0d522f..e4d2c6921 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -34,26 +34,26 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "openpilot.h" -#include "pios.h" -#include "flightstatus.h" -#include "manualcontrolcommand.h" -#include "manualcontrolsettings.h" -#include "gyrosensor.h" -#include "actuatordesired.h" -#include "stabilizationdesired.h" -#include "stabilizationsettings.h" -#include "systemidentsettings.h" -#include "systemidentstate.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include "systemsettings.h" -#include "taskinfo.h" -#include "stabilization.h" -#include "hwsettings.h" -#include "stabilizationsettingsbank1.h" -#include "stabilizationsettingsbank2.h" -#include "stabilizationsettingsbank3.h" -#include "accessorydesired.h" +#include +#include +#include +#include +#include +#include +#include +#include #if defined(PIOS_EXCLUDE_ADVANCED_FEATURES) #define powapprox fastpow @@ -68,43 +68,43 @@ #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 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 +#define AF_NUMX 13 +#define AF_NUMP 43 #if !defined(AT_QUEUE_NUMELEM) -#define AT_QUEUE_NUMELEM 18 +#define AT_QUEUE_NUMELEM 18 #endif -#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 */ +#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 */ -#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 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_DISABLED 0 #define SMOOTH_QUICK_ACCESSORY_BASE 10 #define SMOOTH_QUICK_TOGGLE_BASE 21 -// Private types +// Private types enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING }; struct at_queued_data { - float y[3]; /* Gyro measurements */ - float u[3]; /* Actuator desired */ - float throttle; /* Throttle desired */ + float y[3]; /* Gyro measurements */ + float u[3]; /* Actuator desired */ + float throttle; /* Throttle desired */ - uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */ + uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */ }; @@ -116,8 +116,8 @@ static volatile uint32_t atPointsSpilled; static uint32_t throttleAccumulator; static uint8_t rollMax, pitchMax; static StabilizationBankManualRateData manualRate; -static float gX[AF_NUMX] = {0}; -static float gP[AF_NUMP] = {0}; +static float gX[AF_NUMX] = { 0 }; +static float gP[AF_NUMP] = { 0 }; SystemIdentSettingsData systemIdentSettings; SystemIdentStateData systemIdentState; int8_t accessoryToUse; @@ -133,7 +133,7 @@ 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 AtNewGyroData(UAVObjEvent *ev); static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle); static void UpdateStabilizationDesired(bool doingIdent); static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); @@ -180,9 +180,9 @@ int32_t AutoTuneStart(void) { // Start main task if it is enabled if (moduleEnabled) { - //taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY); - //TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle); - //PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); + // taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY); + // TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle); + // PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); GyroSensorConnectCallback(AtNewGyroData); xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); @@ -200,10 +200,10 @@ MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart); static void AutoTuneTask(__attribute__((unused)) void *parameters) { enum AUTOTUNE_STATE state = AT_INIT; - uint32_t lastUpdateTime = 0; // initialization is only for compiler warning - float noise[3] = {0}; - uint32_t lastTime = 0.0f; - bool saveSiNeeded = false; + uint32_t lastUpdateTime = 0; // initialization is only for compiler warning + float noise[3] = { 0 }; + uint32_t lastTime = 0.0f; + bool saveSiNeeded = false; bool savePidNeeded = false; // get max attitude / max rate @@ -222,12 +222,12 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) uint32_t diffTime; uint32_t measureTime = 60000; bool doingIdent = false; - bool canSleep = true; + bool canSleep = true; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // I have never seen this module misbehave so not bothering making a watchdog - //PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); + // PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (saveSiNeeded) { @@ -258,7 +258,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // 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) + if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentSettings.Complete && !CheckSettings()) { if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // if user toggled while armed set PID's to next in sequence 2,3,4,0,1... or 1,2,0... @@ -272,8 +272,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2; } ProportionPidsSmoothToQuick(0.0f, - (float) flightModeSwitchTogglePosition, - (float) (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE)); + (float)flightModeSwitchTogglePosition, + (float)(systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE)); savePidNeeded = true; } @@ -293,10 +293,10 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) 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)) { + if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (1.0f / 900.0f)) { accessoryValueOld = accessoryValue; ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f); - savePidNeeded = true; + savePidNeeded = true; } } state = AT_INIT; @@ -304,7 +304,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) continue; } - switch(state) { + switch (state) { case AT_INIT: // beware that control comes here every time the user toggles the flight mode switch into AutoTune // and it isn't appropriate to reset the main state here @@ -350,7 +350,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // Reset save status // save SI data even if partial or bad, aids in diagnostics - saveSiNeeded = true; + saveSiNeeded = true; // don't save PIDs until data gathering is complete // and the complete data has been sanity checked savePidNeeded = false; @@ -365,27 +365,27 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) break; case AT_START: - lastTime = PIOS_DELAY_GetRaw(); + 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; + updateCounter = 0; + atPointsSpilled = 0; throttleAccumulator = 0; state = AT_RUN; - lastUpdateTime = xTaskGetTickCount(); + lastUpdateTime = xTaskGetTickCount(); break; case AT_RUN: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; doingIdent = true; - canSleep = false; + 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 0.010f) { dT_s = 0.010f; } lastTime = pt.raw_time; AfPredict(gX, gP, pt.u, pt.y, dT_s, pt.throttle); - for (int j=0; j<3; ++j) { - const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz - noise[j] = NOISE_ALPHA * noise[j] + (1-NOISE_ALPHA) * (pt.y[j] - gX[j]) * (pt.y[j] - gX[j]); + for (int j = 0; j < 3; ++j) { + const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz + noise[j] = NOISE_ALPHA * noise[j] + (1 - NOISE_ALPHA) * (pt.y[j] - gX[j]) * (pt.y[j] - gX[j]); } // This will work up to 8kHz with an 89% throttle position before overflow throttleAccumulator += 10000 * pt.throttle; // Update uavo every 256 cycles to avoid // telemetry spam if (((updateCounter++) & 0xff) == 0) { - float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f; + float hover_throttle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hover_throttle); } } @@ -431,7 +431,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // 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; + systemIdentState.Complete = true; } // always raise an alarm if sanity checks failed // even if disabling sanity checks @@ -443,7 +443,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); } } - float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f; + float hover_throttle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hover_throttle); SystemIdentSettingsSet(&systemIdentSettings); state = AT_WAITING; @@ -469,13 +469,14 @@ 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) { +static void AtNewGyroData(UAVObjEvent *ev) +{ static struct at_queued_data q_item; static bool last_sample_unpushed = false; GyroSensorData gyro; ActuatorDesiredData actuators; - if (!ev || !ev->obj || ev->instId!=0 || ev->event!=EV_UPDATED) { + if (!ev || !ev->obj || ev->instId != 0 || ev->event != EV_UPDATED) { return; } @@ -493,12 +494,12 @@ static void AtNewGyroData(UAVObjEvent * ev) { } 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.y[0] = gyro.x; + q_item.y[1] = gyro.y; + q_item.y[2] = gyro.z; + q_item.u[0] = actuators.Roll; + q_item.u[1] = actuators.Pitch; + q_item.u[2] = actuators.Yaw; q_item.throttle = actuators.Thrust; if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) { @@ -514,7 +515,8 @@ static void AtNewGyroData(UAVObjEvent * ev) { // 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 bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) +{ static uint32_t lastUpdateTime; static uint8_t flightModePrev; static uint8_t counter; @@ -523,7 +525,7 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) { // only count transitions into and out of autotune if ((flightModePrev == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ^ (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE)) { flightModePrev = flightMode; - updateTime = xTaskGetTickCount(); + updateTime = xTaskGetTickCount(); // if it has been over 2 seconds, reset the counter if (updateTime - lastUpdateTime > 2000) { counter = 0; @@ -548,7 +550,8 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) { // 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) { +static void InitSystemIdent(bool loadDefaults) +{ SystemIdentSettingsGet(&systemIdentSettings); uint8_t smoothQuick = systemIdentSettings.SmoothQuick; @@ -572,20 +575,20 @@ static void InitSystemIdent(bool loadDefaults) { // default to disable PID changing with flight mode switch and accessory0-3 accessoryToUse = -1; - flightModeSwitchTogglePosition = -1; + flightModeSwitchTogglePosition = -1; systemIdentSettings.SmoothQuick = SMOOTH_QUICK_DISABLED; switch (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 + 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 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 + 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; + flightModeSwitchTogglePosition = (smoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2; systemIdentSettings.SmoothQuick = smoothQuick; break; } @@ -596,30 +599,28 @@ static void InitSystemIdent(bool loadDefaults) { // these are stored in the settings for use with next battery // and also in the state for logging purposes static void UpdateSystemIdentState(const float *X, const float *noise, - float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle) { - systemIdentState.Beta.Roll = X[6]; - systemIdentState.Beta.Pitch = X[7]; - systemIdentState.Beta.Yaw = X[8]; - systemIdentState.Bias.Roll = X[10]; - systemIdentState.Bias.Pitch = X[11]; - systemIdentState.Bias.Yaw = X[12]; - systemIdentState.Tau = X[9]; + float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle) +{ + systemIdentState.Beta.Roll = X[6]; + systemIdentState.Beta.Pitch = X[7]; + systemIdentState.Beta.Yaw = X[8]; + systemIdentState.Bias.Roll = X[10]; + systemIdentState.Bias.Pitch = X[11]; + systemIdentState.Bias.Yaw = X[12]; + systemIdentState.Tau = X[9]; // 'settings' beta and tau have same value as state versions // the state version produces a GCS log // the settings version is remembered after power off/on - systemIdentSettings.Tau = systemIdentState.Tau; + systemIdentSettings.Tau = systemIdentState.Tau; memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); - if (noise) { systemIdentState.Noise.Roll = noise[0]; systemIdentState.Noise.Pitch = noise[1]; systemIdentState.Noise.Yaw = noise[2]; } systemIdentState.Period = dT_s * 1000.0f; - systemIdentState.NumAfPredicts = predicts; systemIdentState.NumSpilledPts = spills; - systemIdentState.HoverThrottle = hover_throttle; SystemIdentStateSet(&systemIdentState); @@ -628,14 +629,16 @@ static void UpdateSystemIdentState(const float *X, const float *noise, // 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) { +static void UpdateStabilizationDesired(bool doingIdent) +{ StabilizationDesiredData stabDesired; ManualControlCommandData manualControlCommand; + ManualControlCommandGet(&manualControlCommand); - stabDesired.Roll = manualControlCommand.Roll * rollMax; - stabDesired.Pitch = manualControlCommand.Pitch * pitchMax; - stabDesired.Yaw = manualControlCommand.Yaw * manualRate.Yaw; + stabDesired.Roll = manualControlCommand.Roll * rollMax; + stabDesired.Pitch = manualControlCommand.Pitch * pitchMax; + stabDesired.Yaw = manualControlCommand.Yaw * manualRate.Yaw; stabDesired.Thrust = manualControlCommand.Thrust; if (doingIdent) { @@ -647,7 +650,7 @@ static void UpdateStabilizationDesired(bool doingIdent) { stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); } @@ -694,8 +697,11 @@ static uint8_t CheckSettings() uint8_t retVal = CheckSettingsRaw(); if (systemIdentSettings.DisableSanityChecks) { - retVal = 0; - } else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) { + retVal = 0; + } + // if not calculating yaw, or if calculating yaw but ignoring errors + else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) { + // clear the yaw error bit retVal &= ~YAW_BETA_LOW; } @@ -724,51 +730,51 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // These three parameters define the desired response properties // - rate scale in the fraction of the natural speed of the system - // to strive for. + // to strive for. // - damp is the amount of damping in the system. higher values - // make oscillations less likely + // 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; + // of noise + const double ghf = (double)noiseRate / 1000.0d; + const double damp = (double)dampRate / 100.0d; double tau = exp(systemIdentState.Tau); - double exp_beta_roll_times_ghf = exp(systemIdentState.Beta.Roll)*ghf; - double exp_beta_pitch_times_ghf = exp(systemIdentState.Beta.Pitch)*ghf; + double 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 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); + 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); + 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); + const double a = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn) / 20.0d; + const double b = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn - a); // Calculate the gain for the outer loop by approximating the // inner loop as a single order lpf. Set the outer loop to be // critically damped; const double zeta_o = 1.3d; - const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d/wn); + const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d / wn); // dRonin simply uses default PID settings for yaw float kpMax = 0.0f; for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) { double 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; - double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d; + 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; - if (i<2) { - if (kpMax < (float) kp) { - kpMax = (float) kp; + if (i < 2) { + if (kpMax < (float)kp) { + kpMax = (float)kp; } } else { // use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value @@ -789,17 +795,17 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float break; } float ratio = 1.0f; - if (min > 0.0f && (float) kp < min) { - ratio = (float) kp / min; - } else if (max > 0.0f && (float) kp > max) { - ratio = (float) kp / max; + if (min > 0.0f && (float)kp < min) { + ratio = (float)kp / min; + } else if (max > 0.0f && (float)kp > max) { + ratio = (float)kp / max; } - kp /= (double) ratio; - ki /= (double) ratio; - kd /= (double) ratio; + kp /= (double)ratio; + ki /= (double)ratio; + kd /= (double)ratio; } - switch(i) { + switch (i) { case 0: // Roll stabSettingsBank.RollRatePID.Kp = kp; stabSettingsBank.RollRatePID.Ki = ki; @@ -811,8 +817,8 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float stabSettingsBank.PitchRatePID.Kp = kp; stabSettingsBank.PitchRatePID.Ki = ki; stabSettingsBank.PitchRatePID.Kd = kd; - stabSettingsBank.PitchPI.Kp = kp_o; - stabSettingsBank.PitchPI.Ki = 0; + stabSettingsBank.PitchPI.Kp = kp_o; + stabSettingsBank.PitchPI.Ki = 0; break; case 2: // Yaw stabSettingsBank.YawRatePID.Kp = kp; @@ -860,25 +866,26 @@ 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 +// this code guarantees that we will get those exact parameterizations at (val =) min, (max+min)/2, and max static void ProportionPidsSmoothToQuick(float min, float val, float max) { float ratio, damp, noise; // translate from range [min, max] to range [0, max-min] - // takes care of min < 0 too - val -= min; - max -= min; + // that takes care of min < 0 case too + val -= min; + max -= min; ratio = val / max; if (ratio <= 0.5f) { // scale ratio in [0,0.5] to produce PIDs in [smoothest,default] ratio *= 2.0f; - damp = (systemIdentSettings.DampMax * (1.0f - ratio)) + (systemIdentSettings.DampRate * ratio); - noise = (systemIdentSettings.NoiseMin * (1.0f - ratio)) + (systemIdentSettings.NoiseRate * ratio); + damp = (systemIdentSettings.DampMax * (1.0f - ratio)) + (systemIdentSettings.DampRate * ratio); + noise = (systemIdentSettings.NoiseMin * (1.0f - ratio)) + (systemIdentSettings.NoiseRate * ratio); } else { // scale ratio in [0.5,1.0] to produce PIDs in [default,quickest] ratio = (ratio - 0.5f) * 2.0f; - damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio); + damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio); noise = (systemIdentSettings.NoiseRate * (1.0f - ratio)) + (systemIdentSettings.NoiseMax * ratio); } @@ -896,35 +903,35 @@ static void ProportionPidsSmoothToQuick(float min, float val, float max) */ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in) { - const float Ts = dT_s; - const float Tsq = Ts * Ts; + const float 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 + 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]; + 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]; @@ -933,27 +940,28 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl // 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); + 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_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}; + 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++) + 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; @@ -961,304 +969,310 @@ __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; +// expanded with indentation for easier reading but uncrustify even damages comments #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[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[3] * ( + e_tau2 + Ts * e_tau + ) + Ts * e_b1 * e_tau2 * ( D[4] - D[29] - ) + Tsq*e_b1*e_tau*( + ) + Tsq * e_b1 * e_tau * ( D[4] - D[29] - ) + D[18]*Ts*e_tau*( + ) + 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*( + ) + 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*( + ) + D[31] * Tsq * e_b1 * e_tau * ( u1_in - u1 - ) + D[24]*Tsq*e_b1*e_tau*( - u1*( + ) + D[24] * Tsq * e_b1 * e_tau * ( + u1 * ( u1 - bias1 - ) + u1_in*( + ) + u1_in * ( bias1 - u1 + ) ) - ) - ) / Ts_e_tau2; + ) / Ts_e_tau2; P[4] = ( - Q[3]*Tsq4 + e_tau4*( + 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*( + ) + 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; + ) - 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[5] * ( + e_tau2 + Ts * e_tau + ) + Ts * e_b2 * e_tau2 * ( D[6] - D[34] - ) + Tsq*e_b2*e_tau*( + ) + Tsq * e_b2 * e_tau * ( D[6] - D[34] - ) + D[19]*Ts*e_tau*( + ) + 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*( + ) + 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*( + ) + D[36] * Tsq * e_b2 * e_tau * ( u2_in - u2 - ) + D[25]*Tsq*e_b2*e_tau*( - u2*( + ) + D[25] * Tsq * e_b2 * e_tau * ( + u2 * ( u2 - bias2 - ) + u2_in*( + ) + u2_in * ( bias2 - u2 + ) ) - ) - ) / Ts_e_tau2; + ) / Ts_e_tau2; P[6] = ( - Q[4]*Tsq4 + e_tau4*( + 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*( + ) + 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; + ) - 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[7] * ( + e_tau2 + Ts * e_tau + ) + Ts * e_b3 * e_tau2 * ( D[8] - D[39] - ) + Tsq*e_b3*e_tau*( + ) + Tsq * e_b3 * e_tau * ( D[8] - D[39] - ) + D[20]*Ts*e_tau*( + ) + 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*( + ) + 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*( + ) + D[41] * Tsq * e_b3 * e_tau * ( u3_in - u3 - ) + D[26]*Tsq*e_b3*e_tau*( - u3*( + ) + D[26] * Tsq * e_b3 * e_tau * ( + u3 * ( u3 - bias3 - ) + u3_in*( + ) + u3_in * ( bias3 - u3 + ) ) - ) - ) / Ts_e_tau2; + ) / Ts_e_tau2; P[8] = ( - Q[5]*Tsq4 + e_tau4*( + 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*( + ) + 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 + ) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2 + ) / Ts_e_tau4; +#endif /* 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[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[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[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[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[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[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[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[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]); + 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++) + 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]); + 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) + if (X[9] > -1.5f) { X[9] = -1.5f; - else if (X[9] < -5.5f) /* 4ms */ + } else if (X[9] < -5.5f) { /* 4ms */ X[9] = -5.5f; - if (X[10] > 0.5f) + } + if (X[10] > 0.5f) { X[10] = 0.5f; - else if (X[10] < -0.5f) + } else if (X[10] < -0.5f) { X[10] = -0.5f; - if (X[11] > 0.5f) + } + if (X[11] > 0.5f) { X[11] = 0.5f; - else if (X[11] < -0.5f) + } else if (X[11] < -0.5f) { X[11] = -0.5f; - if (X[12] > 0.5f) + } + if (X[12] > 0.5f) { X[12] = 0.5f; - else if (X[12] < -0.5f) + } else if (X[12] < -0.5f) { X[12] = -0.5f; + } } @@ -1269,8 +1283,8 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl static void AfInit(float X[AF_NUMX], float P[AF_NUMP]) { static const float qInit[AF_NUMX] = { - 1.0f, 1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, + 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 @@ -1278,28 +1292,25 @@ 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[6] = X[7] = 10.0f; // roll and pitch medium amount of strength // X[8] = 7.0f; // yaw strength // X[9] = -4.0f; // and 50 (18?) ms time scale // X[10] = X[11] = X[12] = 0.0f; // zero bias - memset(X, 0, AF_NUMX*sizeof(X[0])); + 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 - //SystemIdentSetDefaults(SystemIdentHandle(), 0); - //SystemIdentBetaArrayGet(&X[6]); memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta)); - //SystemIdentTauGet(&X[9]); X[9] = systemIdentState.Tau; // P initialization - memset(P, 0, AF_NUMP*sizeof(P[0])); - P[0] = qInit[0]; - P[1] = qInit[1]; - P[2] = qInit[2]; - P[4] = qInit[3]; - P[6] = qInit[4]; - P[8] = qInit[5]; + memset(P, 0, AF_NUMP * sizeof(P[0])); + P[0] = qInit[0]; + P[1] = qInit[1]; + P[2] = qInit[2]; + P[4] = qInit[3]; + P[6] = qInit[4]; + P[8] = qInit[5]; P[11] = qInit[6]; P[14] = qInit[7]; P[17] = qInit[8]; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 7f77eedf6..6931acb24 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -538,13 +538,12 @@ static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) */ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings) { - //uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE; StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM]; StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap); if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE || position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) { - return (FLIGHTSTATUS_FLIGHTMODEASSIST_NONE); + return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE; } switch (FlightModeAssistMap[position]) { @@ -594,22 +593,19 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD: case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO: // this is only for use with stabi mods with althold/vario. - //isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST; - //break; return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST; + case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL: case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL: default: // this is the default for non stabi modes also - //isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST; - //break; return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST; } } break; } - //return isAssistedFlag; + // return isAssistedFlag; return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE; } #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 560c0b827..1d42a2735 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -78,7 +78,8 @@ static float applyExpo(float value, float expo) */ void stabilizedHandler(__attribute__((unused)) bool newinit) { - static bool inited=false; + static bool inited = false; + if (!inited) { inited = true; StabilizationDesiredInitialize(); @@ -145,6 +146,7 @@ void stabilizedHandler(__attribute__((unused)) bool newinit) // let autotune.c handle it // because it must switch to Attitude after seconds return; + #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ default: // Major error, this should not occur because only enter this block when one of these is true diff --git a/flight/modules/Notify/inc/sequences.h b/flight/modules/Notify/inc/sequences.h index 2e9099895..a414171bc 100644 --- a/flight/modules/Notify/inc/sequences.h +++ b/flight/modules/Notify/inc/sequences.h @@ -185,7 +185,7 @@ const LedSequence_t *flightModeMap[] = { [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], + [FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL], #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ }; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 2cd2385f6..0505840e9 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -57,21 +57,21 @@ // Private constants -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL -#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) -#define UPDATE_MIN 1.0e-6f -#define UPDATE_MAX 1.0f -#define UPDATE_ALPHA 1.0e-2f +#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f -#define SYSTEM_IDENT_PERIOD ((uint32_t) 75) +#define SYSTEM_IDENT_PERIOD ((uint32_t)75) #if defined(PIOS_EXCLUDE_ADVANCED_FEATURES) -#define powapprox fastpow -#define expapprox fastexp +#define powapprox fastpow +#define expapprox fastexp #else -#define powapprox powf -#define expapprox expf +#define powapprox powf +#define expapprox expf #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ // Private variables @@ -319,7 +319,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; + break; case STABILIZATIONSTATUS_INNERLOOP_ACRO: { float stickinput[3]; @@ -340,112 +340,25 @@ static void stabilizationInnerloopTask() break; #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) - case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT: + case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT: { static int8_t identIteration = 0; - static float identOffsets[3] = {0}; + float identOffsets[3]; if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) { - systemIdentTimeVal = PIOS_DELAY_GetRaw(); - - 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++; + SystemIdentStateData systemIdentState; - 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; - - // yaw changes twice a cycle and roll/pitch changes once ? - switch(identIteration & 0x07) { - case 0: - identOffsets[0] = 0; - identOffsets[1] = 0; - identOffsets[2] = yaw_scale; - break; - case 1: - identOffsets[0] = roll_scale; - identOffsets[1] = 0; - identOffsets[2] = 0; - break; - case 2: - identOffsets[0] = 0; - identOffsets[1] = 0; - identOffsets[2] = -yaw_scale; - break; - case 3: - identOffsets[0] = -roll_scale; - identOffsets[1] = 0; - identOffsets[2] = 0; - break; - case 4: - identOffsets[0] = 0; - identOffsets[1] = 0; - identOffsets[2] = yaw_scale; - break; - case 5: - identOffsets[0] = 0; - identOffsets[1] = pitch_scale; - identOffsets[2] = 0; - break; - case 6: - identOffsets[0] = 0; - identOffsets[1] = 0; - identOffsets[2] = -yaw_scale; - break; - case 7: - identOffsets[0] = 0; - identOffsets[1] = -pitch_scale; - identOffsets[2] = 0; - 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 - systemIdentState.Beta.Roll), - expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch), - expapprox(SCALE_BIAS - systemIdentState.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? - 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 - -// same as stock -#if 1 - const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */ + SystemIdentStateGet(&systemIdentState); + systemIdentTimeVal = PIOS_DELAY_GetRaw(); + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration + 1) & 7; // why does yaw change twice a cycle and roll/pitch change only once? - 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]); + 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; } @@ -464,176 +377,6 @@ static void stabilizationInnerloopTask() // 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 */ - // 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], @@ -643,8 +386,6 @@ 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] += 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); } break; #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index d94da6385..ee46c8d72 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -144,7 +144,7 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT: #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) // roll or pitch - if (t<=1) { + if (t <= 1) { StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; } // else yaw (other modes don't worry about invalid thrust mode either) @@ -154,9 +154,9 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e 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 + // no break, do not reorder this code + // for low power FCs just fall through to Attitude mode + // that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ // do not reorder this code case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index 56c8afab7..062ffb878 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -173,7 +173,8 @@ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) * @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) { +uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) +{ return (later - raw) / us_ticks; } #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ From aa06b1793f9598436825a555443def07a6896b49 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Mon, 14 Mar 2016 18:02:07 -0400 Subject: [PATCH 10/23] LP-76 update copyrights --- flight/libraries/sanitycheck.c | 2 +- flight/modules/ManualControl/armhandler.c | 3 ++- flight/modules/ManualControl/manualcontrol.c | 3 ++- flight/modules/ManualControl/stabilizedhandler.c | 3 ++- flight/modules/Notify/inc/sequences.h | 3 ++- flight/modules/Stabilization/innerloop.c | 2 +- flight/modules/Stabilization/stabilization.c | 3 ++- flight/pios/inc/pios_delay.h | 3 ++- flight/pios/stm32f4xx/pios_delay.c | 3 ++- flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc | 1 + flight/targets/boards/revolution/firmware/Makefile | 2 +- flight/targets/boards/revolution/firmware/UAVObjects.inc | 1 + flight/targets/boards/revonano/firmware/Makefile | 2 +- flight/targets/boards/revonano/firmware/UAVObjects.inc | 1 + flight/targets/boards/revoproto/firmware/Makefile | 2 +- flight/targets/boards/revoproto/firmware/UAVObjects.inc | 1 + flight/targets/boards/simposix/firmware/Makefile | 2 +- flight/targets/boards/simposix/firmware/UAVObjects.inc | 1 + 18 files changed, 25 insertions(+), 13 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 51ae6b79f..6e1200ded 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -6,7 +6,7 @@ * @{ * @file sanitycheck.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. - * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Utilities to validate a flight configuration * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index e9d811c9e..2ccfcc7a6 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -7,7 +7,8 @@ * @{ * * @file armhandler.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 6931acb24..5a32e9deb 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -11,7 +11,8 @@ * AttitudeDesired object (stabilized mode) * * @file manualcontrol.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief ManualControl module. Handles safety R/C link and flight mode. * * @see The GNU Public License (GPL) Version 3 diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 1d42a2735..f57e665f5 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -7,7 +7,8 @@ * @{ * * @file stabilizedhandler.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/modules/Notify/inc/sequences.h b/flight/modules/Notify/inc/sequences.h index a414171bc..3298d5ff0 100644 --- a/flight/modules/Notify/inc/sequences.h +++ b/flight/modules/Notify/inc/sequences.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file sequences.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Notify module, sequences configuration. * * @see The GNU Public License (GPL) Version 3 diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 0505840e9..45d41a2a1 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -9,7 +9,7 @@ * @{ * * @file innerloop.c - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief Attitude stabilization module. * diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index ee46c8d72..24f257d12 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -9,7 +9,8 @@ * @{ * * @file stabilization.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Attitude stabilization module. * * @see The GNU Public License (GPL) Version 3 diff --git a/flight/pios/inc/pios_delay.h b/flight/pios/inc/pios_delay.h index 6bc298b21..63546d75d 100644 --- a/flight/pios/inc/pios_delay.h +++ b/flight/pios/inc/pios_delay.h @@ -7,7 +7,8 @@ * @{ * * @file pios_settings.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index 062ffb878..5b3a1af47 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -7,7 +7,8 @@ * @{ * * @file pios_delay.c - * @author Michael Smith Copyright (C) 2012 + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * Michael Smith Copyright (C) 2012 * @brief Delay Functions * - Provides a micro-second granular delay using the CPU * cycle counter. diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 4780f4453..ff28bf026 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -1,4 +1,5 @@ # +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 5ae61fbb3..4ff049d29 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -1,5 +1,5 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot # diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 191f50460..768949c99 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -1,4 +1,5 @@ # +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile index e7fa840cf..8dc8d2989 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -1,5 +1,5 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2014, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify diff --git a/flight/targets/boards/revonano/firmware/UAVObjects.inc b/flight/targets/boards/revonano/firmware/UAVObjects.inc index 191f50460..768949c99 100644 --- a/flight/targets/boards/revonano/firmware/UAVObjects.inc +++ b/flight/targets/boards/revonano/firmware/UAVObjects.inc @@ -1,4 +1,5 @@ # +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 3fd174560..147c8ec47 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -1,5 +1,5 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot # diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index af257283d..ebd14535c 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -1,4 +1,5 @@ # +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 97080b1a7..285a806e7 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -2,7 +2,7 @@ # # TODO: This file should be reworked. It will be done as a part of sim target refactoring. # -# Copyright (c) 2015 The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2015-2016 The LibrePilot Project, http://www.librepilot.org # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009. # # diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index e9d21d659..287be7d51 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -3,6 +3,7 @@ # # Makefile for OpenPilot UAVObject code # +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011. # # This program is free software; you can redistribute it and/or modify From d8e07b090e14c457431fca4a50cbb9317895f318 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 15 Mar 2016 13:12:14 -0400 Subject: [PATCH 11/23] LP-76 fix drops out of sky in AutoTune mode --- flight/modules/Stabilization/innerloop.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 45d41a2a1..f3aafcea1 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -343,7 +343,7 @@ static void stabilizationInnerloopTask() case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT: { static int8_t identIteration = 0; - float identOffsets[3]; + static float identOffsets[3] = { 0 }; if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) { const float SCALE_BIAS = 7.1f; From 4240c8ea5c5e0ae44f2126d2d598e8764fe3a514 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 18 Mar 2016 02:15:10 -0400 Subject: [PATCH 12/23] LP-76 implement new dRonin yaw calculation PR and fix a TL spelling error --- flight/modules/AutoTune/autotune.c | 99 ++++++++++++++----- flight/pios/stm32f4xx/pios_delay.c | 2 +- .../systemidentsettings.xml | 5 +- 3 files changed, 78 insertions(+), 28 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index e4d2c6921..edda57854 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include #include @@ -183,7 +183,7 @@ int32_t AutoTuneStart(void) // taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY); // TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle); // PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); - GyroSensorConnectCallback(AtNewGyroData); + GyroStateConnectCallback(AtNewGyroData); xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); } @@ -264,16 +264,16 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // 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 > systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) { + if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE) { flightModeSwitchTogglePosition = 0; } } else { // if they did it disarmed, then set PID's back to AutoTune default - flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2; + flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE) / 2; } ProportionPidsSmoothToQuick(0.0f, (float)flightModeSwitchTogglePosition, - (float)(systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE)); + (float)(systemIdentSettings.SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE)); savePidNeeded = true; } @@ -473,7 +473,7 @@ static void AtNewGyroData(UAVObjEvent *ev) { static struct at_queued_data q_item; static bool last_sample_unpushed = false; - GyroSensorData gyro; + GyroStateData gyro; ActuatorDesiredData actuators; if (!ev || !ev->obj || ev->instId != 0 || ev->event != EV_UPDATED) { @@ -482,7 +482,7 @@ static void AtNewGyroData(UAVObjEvent *ev) // object will at times change asynchronously so must copy data here, with locking // and do it as soon as possible - GyroSensorGet(&gyro); + GyroStateGet(&gyro); ActuatorDesiredGet(&actuators); if (last_sample_unpushed) { @@ -553,7 +553,7 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) static void InitSystemIdent(bool loadDefaults) { SystemIdentSettingsGet(&systemIdentSettings); - uint8_t smoothQuick = systemIdentSettings.SmoothQuick; + uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; if (loadDefaults) { // get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau) @@ -576,20 +576,20 @@ static void InitSystemIdent(bool loadDefaults) // default to disable PID changing with flight mode switch and accessory0-3 accessoryToUse = -1; flightModeSwitchTogglePosition = -1; - systemIdentSettings.SmoothQuick = SMOOTH_QUICK_DISABLED; - switch (smoothQuick) { + systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED; + switch (SmoothQuickSource) { case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0 case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3 - accessoryToUse = smoothQuick - SMOOTH_QUICK_ACCESSORY_BASE; - systemIdentSettings.SmoothQuick = smoothQuick; + accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; + systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; 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; - systemIdentSettings.SmoothQuick = smoothQuick; + flightModeSwitchTogglePosition = (SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE) / 2; + systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; } } @@ -671,8 +671,9 @@ static uint8_t CheckSettingsRaw() if (systemIdentState.Beta.Pitch < 6) { retVal |= PITCH_BETA_LOW; } - if (systemIdentState.Beta.Yaw < 4) { - retVal |= YAW_BETA_LOW; +// if (systemIdentState.Beta.Yaw < 4) { + 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. @@ -714,7 +715,8 @@ static uint8_t CheckSettings() // most of the doubles could be replaced with floats static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) { - StabilizationSettingsBank1Data stabSettingsBank; +// StabilizationSettingsBank1Data stabSettingsBank; + StabilizationBankData stabSettingsBank; switch (systemIdentSettings.DestinationPidBank) { case 1: @@ -763,14 +765,56 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // critically damped; const double zeta_o = 1.3d; const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d / wn); + const double ki_o = 0.75d * kp_o / (2.0d * M_PI_D * tau * 10.0d); - // dRonin simply uses default PID settings for yaw float kpMax = 0.0f; + double betaMinLn = 1000.0d; + StabilizationBankRollRatePIDData *rollPitchPid = NULL; // satisfy compiler warning only + for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) { - double 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; - double kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0d * damp * wn) - 1.0d) / beta - kp * tau_d; + double betaLn = SystemIdentStateBetaToArray(systemIdentState.Beta)[i]; + double beta = exp(betaLn); + double ki; + double kp; + double kd; + + switch (i) { + case 0: // Roll + case 1: // Pitch + ki = a * b * wn * wn * tau * tau_d / beta; + kp = tau * tau_d * ((a + b) * wn * wn + 2.0d * a * b * damp * wn) / beta - ki * tau_d; + kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0d * damp * wn) - 1.0d) / beta - kp * tau_d; + if (betaMinLn > betaLn) { + betaMinLn = betaLn; + // RollRatePID PitchRatePID YawRatePID + // form an array of structures + // point to one + rollPitchPid = &(&stabSettingsBank.RollRatePID)[i]; + } + break; + case 2: // Yaw + // yaw uses a mixture of yaw and the slowest axis (pitch) for it's beta and thus PID calculation + // calculate the ratio to use when converting from the slowest axis (pitch) to the yaw axis + // as (e^(betaMinLn-betaYawLn))^0.6 + // which is (e^betaMinLn / e^betaYawLn)^0.6 + // which is (betaMin / betaYaw)^0.6 + // which is betaMin^0.6 / betaYaw^0.6 + // now given that kp for each axis can be written as kpaxis = xp / betaaxis + // for xp that is constant across all axes + // then kpmin (probably kppitch) was xp / betamin (probably betapitch) + // which we multiply by betaMin^0.6 / betaYaw^0.6 to get the new Yaw kp + // so the new kpyaw is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6) + // which is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6) + // which is (xp * betaMin^0.6) / (betaMin * betaYaw^0.6) + // which is xp / (betaMin * betaYaw^0.6 / betaMin^0.6) + // which is xp / (betaMin^0.4 * betaYaw^0.6) + // hence the new effective betaYaw for Yaw P is (betaMin^0.4)*(betaYaw^0.6) + beta = exp(0.6d * (betaMinLn - (double) systemIdentState.Beta.Yaw)); + kp = (double) rollPitchPid->Kp * beta; + ki = 0.8d * (double) rollPitchPid->Ki * beta; + kd = 0.8d * (double) rollPitchPid->Kd * beta; + break; + } if (i < 2) { if (kpMax < (float)kp) { @@ -811,21 +855,26 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float stabSettingsBank.RollRatePID.Ki = ki; stabSettingsBank.RollRatePID.Kd = kd; stabSettingsBank.RollPI.Kp = kp_o; - stabSettingsBank.RollPI.Ki = 0; + stabSettingsBank.RollPI.Ki = ki_o; break; case 1: // Pitch stabSettingsBank.PitchRatePID.Kp = kp; stabSettingsBank.PitchRatePID.Ki = ki; stabSettingsBank.PitchRatePID.Kd = kd; stabSettingsBank.PitchPI.Kp = kp_o; - stabSettingsBank.PitchPI.Ki = 0; + stabSettingsBank.PitchPI.Ki = ki_o; break; case 2: // Yaw stabSettingsBank.YawRatePID.Kp = kp; stabSettingsBank.YawRatePID.Ki = ki; stabSettingsBank.YawRatePID.Kd = kd; +#if 0 + // if we ever choose to use these + // (e.g. mag yaw attitude) + // here they are stabSettingsBank.YawPI.Kp = kp_o; - stabSettingsBank.YawPI.Ki = 0; + stabSettingsBank.YawPI.Ki = ki_o; +#endif break; } } diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index 5b3a1af47..f9c5d62b1 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -171,7 +171,7 @@ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) /** - * @brief Subrtact two raw times and convert to us. + * @brief Subtract two raw times and convert to us. * @return Interval between raw times in microseconds */ uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 8df05c0fc..cdcbbab9b 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -17,7 +17,8 @@ - + + @@ -31,7 +32,7 @@ - + From f05b733434a93573a4c1a01a17bca1e9a82aeed1 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 27 Mar 2016 01:34:55 -0400 Subject: [PATCH 13/23] LP-76 minor cleanup --- flight/modules/AutoTune/autotune.c | 4 ++-- flight/modules/Stabilization/innerloop.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index edda57854..ed4650026 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -203,6 +203,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) uint32_t lastUpdateTime = 0; // initialization is only for compiler warning float noise[3] = { 0 }; uint32_t lastTime = 0.0f; + uint32_t measureTime = 0; + uint32_t updateCounter = 0; bool saveSiNeeded = false; bool savePidNeeded = false; @@ -218,9 +220,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) InitSystemIdent(false); while (1) { - static uint32_t updateCounter = 0; uint32_t diffTime; - uint32_t measureTime = 60000; bool doingIdent = false; bool canSleep = true; FlightStatusData flightStatus; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index f3aafcea1..6fd36b089 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -347,9 +347,9 @@ static void stabilizationInnerloopTask() if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) { const float SCALE_BIAS = 7.1f; - SystemIdentStateData systemIdentState; + SystemIdentStateBetaData systemIdentBeta; - SystemIdentStateGet(&systemIdentState); + SystemIdentStateBetaGet(&systemIdentBeta); systemIdentTimeVal = PIOS_DELAY_GetRaw(); identOffsets[0] = 0.0f; identOffsets[1] = 0.0f; @@ -358,7 +358,7 @@ static void stabilizationInnerloopTask() // why does yaw change twice a cycle and roll/pitch change only once? uint8_t index = ((uint8_t[]) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } )[identIteration]; - float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]); + float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentBeta)[index]); if (scale > 0.25f) { scale = 0.25f; } From 03b06daef63fb31303ad3b6692e359058cfd31eb Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 13 May 2016 13:08:46 -0400 Subject: [PATCH 14/23] LP-76 SmoothQuick fixes consilidate settings uncrustify --- flight/modules/AutoTune/autotune.c | 305 ++++++------------ flight/modules/Stabilization/innerloop.c | 13 +- .../systemidentsettings.xml | 36 ++- 3 files changed, 135 insertions(+), 219 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index ed4650026..6c21beadb 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -79,21 +79,28 @@ #define AT_QUEUE_NUMELEM 18 #endif -#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 NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */ +#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */ +#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */ +#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */ + +#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */ +#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */ #define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */ #define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */ -#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */ +#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */ +// CheckSettings() returned error bits #define ROLL_BETA_LOW 1 #define PITCH_BETA_LOW 2 #define YAW_BETA_LOW 4 #define TAU_TOO_LONG 8 #define TAU_TOO_SHORT 16 +// smooth-quick modes #define SMOOTH_QUICK_DISABLED 0 #define SMOOTH_QUICK_ACCESSORY_BASE 10 -#define SMOOTH_QUICK_TOGGLE_BASE 21 +#define SMOOTH_QUICK_TOGGLE_BASE 20 // Private types @@ -118,10 +125,10 @@ static uint8_t rollMax, pitchMax; static StabilizationBankManualRateData manualRate; static float gX[AF_NUMX] = { 0 }; static float gP[AF_NUMP] = { 0 }; -SystemIdentSettingsData systemIdentSettings; -SystemIdentStateData systemIdentState; -int8_t accessoryToUse; -int8_t flightModeSwitchTogglePosition; +static SystemIdentSettingsData systemIdentSettings; +static SystemIdentStateData systemIdentState; +static int8_t accessoryToUse; +static int8_t flightModeSwitchTogglePosition; // Private functions @@ -138,6 +145,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise, float dT_ static void UpdateStabilizationDesired(bool doingIdent); static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); static void InitSystemIdent(bool loadDefaults); +static void InitSmoothQuick(); /** @@ -201,12 +209,12 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) { enum AUTOTUNE_STATE state = AT_INIT; uint32_t lastUpdateTime = 0; // initialization is only for compiler warning - float noise[3] = { 0 }; - uint32_t lastTime = 0.0f; - uint32_t measureTime = 0; + float noise[3] = { 0 }; + uint32_t lastTime = 0.0f; + uint32_t measureTime = 0; uint32_t updateCounter = 0; - bool saveSiNeeded = false; - bool savePidNeeded = false; + bool saveSiNeeded = false; + bool savePidNeeded = false; // get max attitude / max rate // for use in generating Attitude mode commands from this module @@ -218,6 +226,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // based on what is in SystemIdent // so that the user can use the PID smooth->quick slider in following flights InitSystemIdent(false); + InitSmoothQuick(); while (1) { uint32_t diffTime; @@ -226,7 +235,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) FlightStatusData flightStatus; FlightStatusGet(&flightStatus); - // I have never seen this module misbehave so not bothering making a watchdog + // I have never seen this module misbehave in a way that requires a watchdog, so not bothering making one // PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { @@ -252,28 +261,29 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) } } - // 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 - // and the data gathering is complete - // and the data gathered is good - // note: CheckFlightModeSwitchForPidRequest(mode) only returns true if mode is not autotune + // if using flight mode switch "quick toggle 3x" to "try smooth -> quick PIDs" is enabled + // and user toggled into and back out of AutoTune three times in the last two seconds + // and the autotune data gathering is complete + // and the autotune data gathered is good + // note: CheckFlightModeSwitchForPidRequest(mode) only returns true if current mode is not autotune if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentSettings.Complete && !CheckSettings()) { if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { - // if user toggled while armed set PID's to next in sequence 2,3,4,0,1... or 1,2,0... - // if smoothest is -100 and quickest is +100 this corresponds to 0,+50,+100,-100,-50... or 0,+100,-100 + // if user toggled while armed set PID's to next in sequence + // 2,3,4,0,1 (5 positions) or 1,2,0 (3 positions) or 3,4,5,6,0,1,2 (7 positions) + // if you assume that smoothest is -100 and quickest is +100 + // this corresponds to 0,+50,+100,-100,-50 (for 5 position toggle) ++flightModeSwitchTogglePosition; - if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE) { + if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) { flightModeSwitchTogglePosition = 0; } } else { // if they did it disarmed, then set PID's back to AutoTune default - flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE) / 2; + flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; } ProportionPidsSmoothToQuick(0.0f, (float)flightModeSwitchTogglePosition, - (float)(systemIdentSettings.SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE)); + (float)(systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE)); savePidNeeded = true; } @@ -282,25 +292,40 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // - the state machine needs to be reset // - the local version of Attitude mode gets skipped if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + // we don't want it saving to permanent storage many times + // while the user is moving the knob once, so wait till the knob stops moving + static uint8_t savePidDelay; // 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 + // and accessory0-3 changed by at least 1/160 of full range (2) // (don't bother checking to see if the requested accessory# is configured properly // if it isn't, the value will be 0 which is the center of [-1,1] anyway) if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) { static AccessoryDesiredData accessoryValueOld = { 0.0f }; AccessoryDesiredData accessoryValue; AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); - // if the accessory changed more than 1/900 + // if the accessory changed more than 1/80 // (this test is intended to remove one unit jitter) - if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (1.0f / 900.0f)) { - accessoryValueOld = accessoryValue; + // some old PPM receivers use a low resolution chip which only allows about 180 steps + // what we are doing here does not need any higher precision than that + if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) { + accessoryValueOld = accessoryValue; + // this copies the PIDs to memory and makes them active + // but does not write them to permanent storage ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f); - savePidNeeded = true; + // this schedules the first possible write of the PIDs to occur later (a fraction of a second) + savePidDelay = SMOOTH_QUICK_FLUSH_TICKS; + } else if (savePidDelay && --savePidDelay == 0) { + // this flags that the PIDs can be written to permanent storage right now + // but they will only be written when the FC is disarmed + // so this means immediate or wait till FC is disarmed + savePidNeeded = true; } + } else { + savePidDelay = 0; } state = AT_INIT; - vTaskDelay(50 / portTICK_RATE_MS); + vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS); continue; } @@ -320,7 +345,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) case AT_INIT_DELAY: diffTime = xTaskGetTickCount() - lastUpdateTime; // after a small delay, get the stab bank values and SystemIdentSettings in case they changed - // this is a very small delay, so fms toggle gets in here + // this is a very small delay (100ms), so "quick 3x fms toggle" gets in here if (diffTime > INIT_TIME_DELAY_MS) { // do these here so the user has at most a 1/10th second // with controls that use the previous bank's rates @@ -330,6 +355,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // load SystemIdentSettings so that they can change it // and do smooth-quick on changed values InitSystemIdent(false); + // no InitSmoothQuick() here or the toggle switch gets reset even in a "quick toggle" that should increment it state = AT_INIT_DELAY2; lastUpdateTime = xTaskGetTickCount(); } @@ -340,6 +366,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // that allows the user to get his fingers on the sticks // and avoids starting the AutoTune if the user is toggling the flight mode switch // to select other PIDs on the "simulated Smooth Quick slider". + // or simply "passing through" this flight mode to get to another flight mode diffTime = xTaskGetTickCount() - lastUpdateTime; // after 2 seconds start systemident flight mode if (diffTime > SYSTEMIDENT_TIME_DELAY_MS) { @@ -355,6 +382,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // and the complete data has been sanity checked savePidNeeded = false; InitSystemIdent(true); + InitSmoothQuick(); AfInit(gX, gP); UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; @@ -411,8 +439,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // Update uavo every 256 cycles to avoid // telemetry spam if (((updateCounter++) & 0xff) == 0) { - float hover_throttle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; - UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hover_throttle); + float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; + UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle); } } if (diffTime > measureTime) { // Move on to next state @@ -443,8 +471,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); } } - float hover_throttle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; - UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hover_throttle); + float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; + UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hoverThrottle); SystemIdentSettingsSet(&systemIdentSettings); state = AT_WAITING; break; @@ -553,7 +581,6 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) static void InitSystemIdent(bool loadDefaults) { SystemIdentSettingsGet(&systemIdentSettings); - uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; if (loadDefaults) { // get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau) @@ -572,10 +599,16 @@ static void InitSystemIdent(bool loadDefaults) memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData)); systemIdentState.Complete = systemIdentSettings.Complete; } +} + + +static void InitSmoothQuick() +{ + uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; // default to disable PID changing with flight mode switch and accessory0-3 accessoryToUse = -1; - flightModeSwitchTogglePosition = -1; + flightModeSwitchTogglePosition = -1; systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED; switch (SmoothQuickSource) { case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0 @@ -585,10 +618,11 @@ static void InitSystemIdent(bool loadDefaults) accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; - 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 + case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points + case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points + case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points // first test PID is in the middle of the smooth -> quick range - flightModeSwitchTogglePosition = (SmoothQuickSource - SMOOTH_QUICK_TOGGLE_BASE) / 2; + flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; } @@ -671,10 +705,13 @@ static uint8_t CheckSettingsRaw() if (systemIdentState.Beta.Pitch < 6) { retVal |= PITCH_BETA_LOW; } -// if (systemIdentState.Beta.Yaw < 4) { +// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default +#if 0 + // if (systemIdentState.Beta.Yaw < 4) { if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) { - retVal |= YAW_BETA_LOW; + retVal |= YAW_BETA_LOW; } +#endif // Check the response speed // Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values. if (expf(systemIdentState.Tau) > 0.1f) { @@ -700,11 +737,14 @@ static uint8_t CheckSettings() if (systemIdentSettings.DisableSanityChecks) { retVal = 0; } +// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default +#if 0 // if not calculating yaw, or if calculating yaw but ignoring errors else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) { // clear the yaw error bit retVal &= ~YAW_BETA_LOW; } +#endif return retVal; } @@ -715,7 +755,7 @@ static uint8_t CheckSettings() // most of the doubles could be replaced with floats static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) { -// StabilizationSettingsBank1Data stabSettingsBank; + _Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)"); StabilizationBankData stabSettingsBank; switch (systemIdentSettings.DestinationPidBank) { @@ -768,12 +808,12 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float const double ki_o = 0.75d * kp_o / (2.0d * M_PI_D * tau * 10.0d); float kpMax = 0.0f; - double betaMinLn = 1000.0d; + double betaMinLn = 1000.0d; StabilizationBankRollRatePIDData *rollPitchPid = NULL; // satisfy compiler warning only for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) { double betaLn = SystemIdentStateBetaToArray(systemIdentState.Beta)[i]; - double beta = exp(betaLn); + double beta = exp(betaLn); double ki; double kp; double kd; @@ -809,10 +849,10 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // which is xp / (betaMin * betaYaw^0.6 / betaMin^0.6) // which is xp / (betaMin^0.4 * betaYaw^0.6) // hence the new effective betaYaw for Yaw P is (betaMin^0.4)*(betaYaw^0.6) - beta = exp(0.6d * (betaMinLn - (double) systemIdentState.Beta.Yaw)); - kp = (double) rollPitchPid->Kp * beta; - ki = 0.8d * (double) rollPitchPid->Ki * beta; - kd = 0.8d * (double) rollPitchPid->Kd * beta; + beta = exp(0.6d * (betaMinLn - (double)systemIdentState.Beta.Yaw)); + kp = (double)rollPitchPid->Kp * beta; + ki = 0.8d * (double)rollPitchPid->Ki * beta; + kd = 0.8d * (double)rollPitchPid->Kd * beta; break; } @@ -828,16 +868,18 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // so reducing them all proportionally is the same as changing beta float min = 0.0f; float max = 0.0f; - switch (systemIdentSettings.YawPIDRatioFunction) { - case SYSTEMIDENTSETTINGS_YAWPIDRATIOFUNCTION_DISABLE: - max = 1000.0f; - min = 0.0f; - break; - case SYSTEMIDENTSETTINGS_YAWPIDRATIOFUNCTION_LIMIT: + switch (systemIdentSettings.CalculateYaw) { + case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUELIMITTORATIO: max = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMax; min = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMin; break; + case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUEIGNORELIMIT: + default: + max = 1000.0f; + min = 0.0f; + break; } + float ratio = 1.0f; if (min > 0.0f && (float)kp < min) { ratio = (float)kp / min; @@ -879,8 +921,9 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float } } - // Librepilot might do something with this some time + // Librepilot might do something more with this some time // stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d); + // SystemIdentSettingsDerivativeCutoffSet(&systemIdentSettings.DerivativeCutoff); // Save PIDs to UAVO RAM (not permanently yet) switch (systemIdentSettings.DestinationPidBank) { @@ -1018,154 +1061,6 @@ __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; -// expanded with indentation for easier reading but uncrustify even damages comments -#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; -#endif /* 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 diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 6fd36b089..3d515244e 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -359,8 +359,17 @@ static void stabilizationInnerloopTask() uint8_t index = ((uint8_t[]) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } )[identIteration]; float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentBeta)[index]); - if (scale > 0.25f) { - scale = 0.25f; + // if roll or pitch limit to 25% of range + if (identIteration & 1) { + if (scale > 0.25f) { + scale = 0.25f; + } + } + // else it is yaw that can be a little more radical + else { + if (scale > 0.45f) { + scale = 0.45f; + } } if (identIteration & 2) { scale = -scale; diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index cdcbbab9b..1484daa51 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -11,27 +11,39 @@ - + + + + + + + + + + - - + + - - - + + + + - + - - + + - - - + + + + + From 68c3d5030ab6f04864bf7ca65dd681a503fbfda2 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 14 May 2016 00:03:26 -0400 Subject: [PATCH 15/23] LP-76 auto module enable, more sanity checks, SmoothQuick tweaks --- flight/libraries/sanitycheck.c | 3 + flight/modules/AutoTune/autotune.c | 134 +++++++++++++----- .../systemidentsettings.xml | 18 ++- 3 files changed, 110 insertions(+), 45 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 6e1200ded..f60a3015b 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -167,6 +167,9 @@ int32_t configuration_check() #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: ADDSEVERITY(!gps_assisted); + // it would be fun to try autotune on a fixed wing + // but that should only be attempted by devs at first + ADDSEVERITY(multirotor); break; #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ default: diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 6c21beadb..a55ab50e1 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -145,7 +146,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise, float dT_ static void UpdateStabilizationDesired(bool doingIdent); static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); static void InitSystemIdent(bool loadDefaults); -static void InitSmoothQuick(); +static void InitSmoothQuick(bool loadToggle); /** @@ -161,11 +162,26 @@ int32_t AutoTuneInitialize(void) HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) { + // even though the AutoTune module is automatically enabled + // (below, when the flight mode switch is configured to use autotune) + // there are use cases where the user may even want it enabled without being on the FMS + // that allows PIDs to be adjusted in flight moduleEnabled = true; } else { + // if the user did not enable the autotune module + // do it for them if they have autotune on their flight mode switch + FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; moduleEnabled = false; + FlightModeSettingsInitialize(); + FlightModeSettingsFlightModePositionGet(fms); + for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) { + if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) { + moduleEnabled = true; + break; + } + } } -#endif +#endif /* ifdef MODULE_AutoTune_BUILTIN */ if (moduleEnabled) { SystemIdentSettingsInitialize(); @@ -226,7 +242,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // based on what is in SystemIdent // so that the user can use the PID smooth->quick slider in following flights InitSystemIdent(false); - InitSmoothQuick(); + InitSmoothQuick(true); while (1) { uint32_t diffTime; @@ -287,14 +303,27 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) savePidNeeded = true; } + ////////////////////////////////////////////////////////////////////////////////////// + // if configured to use a slider for smooth-quick and the autotune module is running + // (note that the module can be automatically or manually enabled) + // then the smooth-quick slider is always active (when not actually in autotune mode) + // + // when the slider is active it will immediately change the PIDs + // and it will schedule the PIDs to be written to permanent storage + // + // if the FC is disarmed, the perm write will happen on next loop + // but if the FC is armed, the perm write will only occur when the FC goes disarmed + ////////////////////////////////////////////////////////////////////////////////////// + + // we don't want it saving to permanent storage many times + // while the user is moving the knob once, so wait till the knob stops moving + static uint8_t savePidDelay; // any time we are not in AutoTune mode: // - the user may be using the accessory0-3 knob/slider to request PID changes // - the state machine needs to be reset // - the local version of Attitude mode gets skipped if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { - // we don't want it saving to permanent storage many times - // while the user is moving the knob once, so wait till the knob stops moving - static uint8_t savePidDelay; + static bool savePidActive = false; // if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range // and FC is not currently running autotune // and accessory0-3 changed by at least 1/160 of full range (2) @@ -309,16 +338,27 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // some old PPM receivers use a low resolution chip which only allows about 180 steps // what we are doing here does not need any higher precision than that if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) { - accessoryValueOld = accessoryValue; + accessoryValueOld = accessoryValue; // this copies the PIDs to memory and makes them active // but does not write them to permanent storage ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f); - // this schedules the first possible write of the PIDs to occur later (a fraction of a second) - savePidDelay = SMOOTH_QUICK_FLUSH_TICKS; + // don't save PID to perm storage the first time + // that usually means at power up + // + // that keeps it from writing the same value at each boot + // but means that it won't be permanent if they move the slider before FC power on + // (note that the PIDs will be changed, just not saved permanently) + if (savePidActive) { + // this schedules the first possible write of the PIDs to occur a fraction of a second or so from now + // and moves the scheduled time if it is already scheduled + savePidDelay = SMOOTH_QUICK_FLUSH_TICKS; + } else { + savePidActive = true; + } } else if (savePidDelay && --savePidDelay == 0) { // this flags that the PIDs can be written to permanent storage right now // but they will only be written when the FC is disarmed - // so this means immediate or wait till FC is disarmed + // so this means immediate (after NOT_AT_MODE_DELAY_MS) or wait till FC is disarmed savePidNeeded = true; } } else { @@ -327,6 +367,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) state = AT_INIT; vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS); continue; + } else { + savePidDelay = 0; } switch (state) { @@ -355,9 +397,15 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // load SystemIdentSettings so that they can change it // and do smooth-quick on changed values InitSystemIdent(false); - // no InitSmoothQuick() here or the toggle switch gets reset even in a "quick toggle" that should increment it - state = AT_INIT_DELAY2; - lastUpdateTime = xTaskGetTickCount(); + InitSmoothQuick(false); + // wait for FC to arm in case they are doing this without a flight mode switch + // that causes the 2+ second delay that follows to happen after arming + // which gives them a chance to take off before the shakes start + // the FC must be armed and if we check here it also allows switchless setup to use autotune + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { + state = AT_INIT_DELAY2; + lastUpdateTime = xTaskGetTickCount(); + } } break; @@ -373,21 +421,18 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) 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); - InitSmoothQuick(); - AfInit(gX, gP); - UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); - measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; - state = AT_START; - } + // Reset save status + // save SI data even if partial or bad, aids in diagnostics + saveSiNeeded = true; + // don't save PIDs until data gathering is complete + // and the complete data has been sanity checked + savePidNeeded = false; + InitSystemIdent(true); + InitSmoothQuick(true); + AfInit(gX, gP); + UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); + measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; + state = AT_START; } } break; @@ -587,13 +632,13 @@ static void InitSystemIdent(bool loadDefaults) // so that if they are changed there (mainly for future code changes), they will be changed here too SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0); SystemIdentStateGet(&systemIdentState); - // Tau Beta and the Complete flag get default values + // 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 + // 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)); @@ -602,29 +647,42 @@ static void InitSystemIdent(bool loadDefaults) } -static void InitSmoothQuick() +static void InitSmoothQuick(bool loadToggle) { uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; - // default to disable PID changing with flight mode switch and accessory0-3 - accessoryToUse = -1; - flightModeSwitchTogglePosition = -1; - systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED; switch (SmoothQuickSource) { case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0 case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3 + // disable PID changing with flight mode switch + // ignore loadToggle if user is also switching to use knob as source + flightModeSwitchTogglePosition = -1; accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points - // first test PID is in the middle of the smooth -> quick range - flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; + // disable PID changing with accessory0-3 + accessoryToUse = -1; + // don't allow init of current toggle position in the middle of 3x fms toggle + if (loadToggle) { + // first test PID is in the middle of the smooth -> quick range + flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; + } systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; + case SMOOTH_QUICK_DISABLED: + default: + // disable PID changing with flight mode switch + // ignore loadToggle since user is disabling toggle + flightModeSwitchTogglePosition = -1; + // disable PID changing with accessory0-3 + accessoryToUse = -1; + systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED; + break; } } @@ -642,7 +700,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise, systemIdentState.Bias.Pitch = X[11]; systemIdentState.Bias.Yaw = X[12]; systemIdentState.Tau = X[9]; - // 'settings' beta and tau have same value as state versions + // '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; diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 1484daa51..9a2d2f7be 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -36,14 +36,18 @@ - - + + - - - - - + + + + + + + + + From 039e95ac4fd178200f977d1a4c4c463ab5297e22 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 29 May 2016 13:02:11 -0400 Subject: [PATCH 16/23] LP-76 fix MSP merge conflict sort OPTMODULES in alphabetical order --- flight/targets/boards/discoveryf4bare/firmware/Makefile | 2 +- flight/targets/boards/revolution/firmware/Makefile | 2 +- flight/targets/boards/revonano/firmware/Makefile | 2 +- flight/targets/boards/revoproto/firmware/Makefile | 2 +- flight/targets/boards/sparky2/firmware/Makefile | 3 ++- flight/targets/boards/sparky2/firmware/UAVObjects.inc | 4 +++- 6 files changed, 9 insertions(+), 6 deletions(-) diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 3ba0b5cb8..20ad2966a 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -55,9 +55,9 @@ MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge -OPTMODULES += AutoTune SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 4ff049d29..19173a486 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -52,9 +52,9 @@ MODULES += Logging MODULES += Telemetry MODULES += Notify +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge -OPTMODULES += AutoTune SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile index 8dc8d2989..14157ef7e 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -51,9 +51,9 @@ MODULES += PathFollower MODULES += Telemetry MODULES += Notify +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge -OPTMODULES += AutoTune SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 147c8ec47..9d4fb0f8f 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -52,9 +52,9 @@ MODULES += Telemetry SRC += $(FLIGHTLIB)/notification.c +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge -OPTMODULES += AutoTune # Include all camera options CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF diff --git a/flight/targets/boards/sparky2/firmware/Makefile b/flight/targets/boards/sparky2/firmware/Makefile index 160081566..b16807dda 100644 --- a/flight/targets/boards/sparky2/firmware/Makefile +++ b/flight/targets/boards/sparky2/firmware/Makefile @@ -1,5 +1,5 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot # @@ -52,6 +52,7 @@ MODULES += Logging MODULES += Telemetry MODULES += Notify +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge diff --git a/flight/targets/boards/sparky2/firmware/UAVObjects.inc b/flight/targets/boards/sparky2/firmware/UAVObjects.inc index 7e7a70f8b..89659c31a 100644 --- a/flight/targets/boards/sparky2/firmware/UAVObjects.inc +++ b/flight/targets/boards/sparky2/firmware/UAVObjects.inc @@ -126,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter - +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate + UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) From 4ab9264015b182cbc75135715524ff09894845ca Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 3 Jun 2016 12:14:14 -0400 Subject: [PATCH 17/23] LP-76 disallow SystemIdent stabilization mode to be selected in GCS --- .../flightmodesettings.xml | 48 +++++++++---------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 8228649d9..5ee84a544 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -9,55 +9,55 @@ elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl,SystemIdent" defaultvalue="Attitude,Attitude,AxisLock,Manual" - limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \ - %NE:AltitudeHold:AltitudeVario:CruiseControl; \ - %NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \ - %NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;" + limits="%NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \ + %NE:AltitudeHold:AltitudeVario:CruiseControl:SystemIdent; \ + %NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar:SystemIdent; \ + %NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:SystemIdent;" /> From e880ce68226fa99e1347db282447d9ee18c488c2 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 3 Jun 2016 12:34:36 -0400 Subject: [PATCH 18/23] LP-76 remove unneeded and commented out code --- flight/libraries/sanitycheck.c | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index f60a3015b..56ec85335 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -274,23 +274,7 @@ 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 autotune 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) { From 63452e45ec57bf7936c37d6f7a0c4aca306be976 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 3 Jun 2016 12:42:18 -0400 Subject: [PATCH 19/23] LP-76 add blank lines in switch --- flight/modules/AutoTune/autotune.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index a55ab50e1..3ae12cd75 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -662,6 +662,7 @@ static void InitSmoothQuick(bool loadToggle) accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; + case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points @@ -674,6 +675,7 @@ static void InitSmoothQuick(bool loadToggle) } systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; + case SMOOTH_QUICK_DISABLED: default: // disable PID changing with flight mode switch From f17f44982fffc03c2e38e6c1c00f98dadbdd1990 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 8 Jun 2016 14:17:20 -0400 Subject: [PATCH 20/23] LP-76 remove comment about a Nano issue that was probably caused by bad Nano hardware --- flight/modules/AutoTune/autotune.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 3ae12cd75..a48eb646c 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -67,10 +67,8 @@ // 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 +// Pull Request version tested on Nano. 120 bytes of stack left when configured with 1340 #define STACK_SIZE_BYTES 1340 -// #define TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define AF_NUMX 13 From 9b6d7b263fc94063a30d6829c6462fb82da44cdc Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Thu, 9 Jun 2016 09:58:01 -0400 Subject: [PATCH 21/23] LP-76 change (c) to (C) in copyright comments --- flight/targets/boards/discoveryf4bare/firmware/Makefile | 6 +++--- .../targets/boards/discoveryf4bare/firmware/UAVObjects.inc | 2 +- flight/targets/boards/revolution/firmware/Makefile | 6 +++--- flight/targets/boards/revolution/firmware/UAVObjects.inc | 2 +- flight/targets/boards/revonano/firmware/Makefile | 4 ++-- flight/targets/boards/revonano/firmware/UAVObjects.inc | 2 +- flight/targets/boards/revoproto/firmware/Makefile | 6 +++--- flight/targets/boards/revoproto/firmware/UAVObjects.inc | 2 +- flight/targets/boards/simposix/firmware/Makefile | 2 +- flight/targets/boards/sparky2/firmware/Makefile | 6 +++--- flight/targets/boards/sparky2/firmware/UAVObjects.inc | 4 ++-- 11 files changed, 21 insertions(+), 21 deletions(-) diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 20ad2966a..ee1113e44 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index ff28bf026..b704f4ab0 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -1,6 +1,6 @@ # # Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 19173a486..ec760e0a4 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 768949c99..6100c4750 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -1,6 +1,6 @@ # # Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile index 14157ef7e..3a20a18d4 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -1,6 +1,6 @@ # -# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2014, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2014, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/revonano/firmware/UAVObjects.inc b/flight/targets/boards/revonano/firmware/UAVObjects.inc index 768949c99..6100c4750 100644 --- a/flight/targets/boards/revonano/firmware/UAVObjects.inc +++ b/flight/targets/boards/revonano/firmware/UAVObjects.inc @@ -1,6 +1,6 @@ # # Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 9d4fb0f8f..736978d5e 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index ebd14535c..2b5646e6a 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -1,6 +1,6 @@ # # Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 285a806e7..1849b9740 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -2,7 +2,7 @@ # # TODO: This file should be reworked. It will be done as a part of sim target refactoring. # -# Copyright (c) 2015-2016 The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2015-2016 The LibrePilot Project, http://www.librepilot.org # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009. # # diff --git a/flight/targets/boards/sparky2/firmware/Makefile b/flight/targets/boards/sparky2/firmware/Makefile index b16807dda..e84a4adb4 100644 --- a/flight/targets/boards/sparky2/firmware/Makefile +++ b/flight/targets/boards/sparky2/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by diff --git a/flight/targets/boards/sparky2/firmware/UAVObjects.inc b/flight/targets/boards/sparky2/firmware/UAVObjects.inc index 89659c31a..f9ff52a9f 100644 --- a/flight/targets/boards/sparky2/firmware/UAVObjects.inc +++ b/flight/targets/boards/sparky2/firmware/UAVObjects.inc @@ -1,6 +1,6 @@ # -# Copyright (c) 2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by From ec80904cb27e2543555622184d102696e4bb9e9c Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Thu, 9 Jun 2016 10:18:51 -0400 Subject: [PATCH 22/23] LP-76 remove a comment from 2 UAVObjects.inc files --- flight/targets/boards/revoproto/firmware/UAVObjects.inc | 1 - flight/targets/boards/simposix/firmware/UAVObjects.inc | 1 - 2 files changed, 2 deletions(-) diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 2b5646e6a..cba723f6b 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -125,7 +125,6 @@ UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation -# this was missing when systemident was added # UAVOBJSRCFILENAMES += perfcounter UAVOBJSRCFILENAMES += systemidentsettings UAVOBJSRCFILENAMES += systemidentstate diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 287be7d51..52093b2e9 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -120,7 +120,6 @@ UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += takeofflocation -# this was missing when systemident was added # UAVOBJSRCFILENAMES += perfcounter UAVOBJSRCFILENAMES += systemidentsettings UAVOBJSRCFILENAMES += systemidentstate From 0d0adae6f7de6bbbd92265f7272e39ce82aa4fb5 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 18 Jun 2016 11:34:10 -0400 Subject: [PATCH 23/23] LP-76 uncrustify all --- flight/modules/Stabilization/innerloop.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 3d515244e..a50e6e6ec 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -123,7 +123,7 @@ void stabilizationInnerloopInit() measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) // Settings for system identification - systemIdentTimeVal = PIOS_DELAY_GetRaw(); + systemIdentTimeVal = PIOS_DELAY_GetRaw(); #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ } @@ -393,7 +393,7 @@ static void stabilizationInnerloopTask() 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] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled); actuatorDesiredAxis[t] += identOffsets[t]; } break;