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

Merge branch 'next' into corvuscorax/OP-1161_magnetometer-alarm

Browse Source 
Conflicts:
	ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
This commit is contained in:
Corvus Corax 2014-05-06 17:30:00 +02:00
commit 2bd7609de9
66 changed files with 2294 additions and 1815 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/libraries/inc/insgps.h
View File

@ -67,6 +67,7 @@ void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetGyroBiasVar(float gyro_bias_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetG(float g_e);
void INSSetMagVar(float scaled_mag_var[3]);
void INSSetBaroVar(float baro_var);
void INSPosVelReset(float pos[3], float vel[3]);

9
flight/libraries/insgps13state.c
View File

@ -91,6 +91,8 @@ static struct EKFData {
float H[NUMV][NUMX];
// local magnetic unit vector in NED frame
float Be[3];
// local gravity vector
float g_e;
// covariance matrix and state vector
float P[NUMX][NUMX];
float X[NUMX];
@ -286,6 +288,11 @@ void INSSetMagNorth(float B[3])
ekf.Be[2] = B[2] / mag;
}
void INSSetG(float g_e)
{
ekf.g_e = g_e;
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
@ -641,7 +648,7 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
az;
Xdot[5] =
2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + ekf.g_e;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;

17
flight/modules/AltitudeHold/inc/altitudehold.h → flight/libraries/math/mathmisc.c
View File

@ -1,9 +1,13 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Reuseable math functions
* @{
*
* @file examplemodperiodic.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Example module to be used as a template for actual modules.
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @see The GNU Public License (GPL) Version 3
*
@ -23,9 +27,6 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef EXAMPLEMODPERIODIC_H
#define EXAMPLEMODPERIODIC_H
int32_t ExampleModPeriodicInitialize();
int32_t GuidanceInitialize(void);
#endif // EXAMPLEMODPERIODIC_H
// space deliberately left empty, any non inline misc math functions can go here

55
flight/libraries/math/mathmisc.h Normal file
View File

@ -0,0 +1,55 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Reuseable math functions
* @{
*
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @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 MATHMISC_H
#define MATHMISC_H
// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
static inline float boundf(float val, float boundary1, float boundary2)
{
if (boundary1 > boundary2) {
if (!(val >= boundary2)) {
return boundary2;
} else if (!(val <= boundary1)) {
return boundary1;
}
} else {
if (!(val >= boundary1)) {
return boundary1;
} else if (!(val <= boundary2)) {
return boundary2;
}
}
return val;
}
#endif /* MATHMISC_H */

21
flight/libraries/math/pid.c
View File

@ -30,11 +30,9 @@
#include "openpilot.h"
#include "pid.h"
#include <mathmisc.h>
#include <pios_math.h>
// ! Private method
static float bound(float val, float range);
// ! Store the shared time constant for the derivative cutoff.
static float deriv_tau = 7.9577e-3f;
@ -52,7 +50,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
{
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term
float diff = (err - pid->lastErr);
@ -84,7 +82,7 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term,
float dterm = 0;
@ -142,16 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
pid->d = d;
pid->iLim = iLim;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
} else if (val > range) {
val = range;
}
return val;
}

86
flight/libraries/sanitycheck.c
View File

@ -46,7 +46,7 @@
****************************/
// ! Check a stabilization mode switch position for safety
static int32_t check_stabilization_settings(int index, bool multirotor);
static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
/**
* Run a preflight check over the hardware configuration
@ -98,34 +98,28 @@ int32_t configuration_check()
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor, coptercontrol) : severity;
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor, coptercontrol) : severity;
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor, coptercontrol) : severity;
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(4, multirotor, coptercontrol) : severity;
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(5, multirotor, coptercontrol) : severity;
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(6, multirotor, coptercontrol) : severity;
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
@ -199,14 +193,8 @@ int32_t configuration_check()
* @param[in] index Which stabilization mode to check
* @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR
*/
static int32_t check_stabilization_settings(int index, bool multirotor)
static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
{
// Make sure the modes have identical sizes
if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
return SYSTEMALARMS_ALARM_ERROR;
}
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
@ -220,22 +208,58 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
case 3:
FlightModeSettingsStabilization3SettingsArrayGet(modes);
break;
case 4:
FlightModeSettingsStabilization4SettingsArrayGet(modes);
break;
case 5:
FlightModeSettingsStabilization5SettingsArrayGet(modes);
break;
case 6:
FlightModeSettingsStabilization6SettingsArrayGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;
}
// For multirotors verify that nothing is set to "none"
// For multirotors verify that roll/pitch/yaw are not set to "none"
// (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
if (multirotor) {
for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) {
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
return SYSTEMALARMS_ALARM_ERROR;
}
}
}
// coptercontrol cannot do altitude holding
if (coptercontrol) {
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) {
return SYSTEMALARMS_ALARM_ERROR;
}
}
// check that thrust modes are only set to thrust axis
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) {
return SYSTEMALARMS_ALARM_ERROR;
}
}
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
)) {
return SYSTEMALARMS_ALARM_ERROR;
}
// Warning: This assumes that certain conditions in the XML file are met. That
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
// (this is checked at compile time by static constraint manualcontrol.h)
return SYSTEMALARMS_ALARM_OK;
}

5
flight/modules/Airspeed/airspeed.c
View File

@ -45,12 +45,11 @@
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
#include "airspeedalarm.h"
#include "taskinfo.h"
// Private constants
#define STACK_SIZE_BYTES 500
#define STACK_SIZE_BYTES 650
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
@ -159,7 +158,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// if sensor type changed reset Airspeed alarm
if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT);
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
}

7
flight/modules/Airspeed/baro_airspeed_etasv3.c
View File

@ -40,7 +40,6 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_ETASV3)
@ -65,13 +64,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
if (airspeedSensor->SensorValue == (uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -96,7 +95,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
// Compute airspeed
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}

9
flight/modules/Airspeed/baro_airspeed_mpxv.c
View File

@ -40,7 +40,6 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_MPXV)
@ -64,7 +63,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
// Ensure that the ADC pin is properly configured
if (airspeedADCPin < 0) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
if (sensor.type == PIOS_MPXV_UNKNOWN) {
@ -77,7 +76,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
break;
default:
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
}
@ -85,7 +84,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
if (!airspeedSettings->ZeroPoint) {
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize.
calibrationCount++;
@ -110,7 +109,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MPXV) */

9
flight/modules/Airspeed/baro_airspeed_ms4525do.c
View File

@ -40,7 +40,6 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#include "taskinfo.h"
#if defined(PIOS_INCLUDE_MS4525DO)
@ -75,7 +74,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
airspeedSensor->SensorValueTemperature = -1;
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
@ -88,7 +87,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT);
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -102,7 +101,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
calibrationCount = 0;
}
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
return;
}
}
@ -129,7 +128,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
// everything is fine so set ALARM_OK
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MS4525DO) */

29
flight/modules/Airspeed/gps_airspeed.c
View File

@ -37,7 +37,6 @@
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include "airspeedalarm.h"
#include <pios_math.h>
@ -46,6 +45,7 @@
#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO
#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO
#define COSINE_OF_5_DEG 0.9961947f
// Private types
struct GPSGlobals {
@ -60,6 +60,12 @@ struct GPSGlobals {
static struct GPSGlobals *gps;
// Private functions
// a simple square inline function based on multiplication faster than powf(x,2.0f)
static inline float Sq(float x)
{
return x * x;
}
/*
* Initialize function loads first data sets, and allocates memory for structure.
@ -103,6 +109,11 @@ void gps_airspeedInitialize()
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
/* Remark regarding "IMU Wind Estimation": The approach includes errors when |V| is
* not constant, i.e. when the change in V_gps does not solely come from a reorientation
* this error depends strongly on the time scale considered. Is the time between t1 and t2 too
* small, "spikes" absorving unconsidred acceleration will arise
*/
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
{
float Rbe[3][3];
@ -121,35 +132,39 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]);
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
if (fabsf(cosDiff) < COSINE_OF_5_DEG) {
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
return; // do not calculate if gps velocity is insufficient...
}
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
float normDiffGPS2 = Sq(gpsVelData.North - gps->gpsVelOld_N) + Sq(gpsVelData.East - gps->gpsVelOld_E) + Sq(gpsVelData.Down - gps->gpsVelOld_D);
// Calculate the norm^2 of the difference between the two fuselage vectors
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
float normDiffAttitude2 = Sq(Rbe[0][0] - gps->RbeCol1_old[0]) + Sq(Rbe[0][1] - gps->RbeCol1_old[1]) + Sq(Rbe[0][2] - gps->RbeCol1_old[2]);
// Airspeed magnitude is the ratio between the two difference norms
float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
} else if (!IS_REAL(airspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
} else {
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}
// Save old variables for next pass

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

@ -181,8 +181,9 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
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_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) {

15
flight/modules/CameraStab/camerastab.c
View File

@ -79,7 +79,6 @@ static struct CameraStab_data {
// Private functions
static void attitudeUpdated(UAVObjEvent *ev);
static float bound(float val, float limit);
#ifdef USE_GIMBAL_FF
static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab);
@ -186,8 +185,9 @@ static void attitudeUpdated(UAVObjEvent *ev)
input_rate = accessory.AccessoryVal *
cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i];
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis,
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
-cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i],
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
}
break;
default:
@ -229,7 +229,7 @@ static void attitudeUpdated(UAVObjEvent *ev)
// bounding for elevon mixing occurs on the unmixed output
// to limit the range of the mixed output you must limit the range
// of both the unmixed pitch and unmixed roll
float output = bound((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], 1.0f);
float output = boundf((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], -1.0f, 1.0f);
// set output channels
switch (i) {
@ -282,13 +282,6 @@ static void attitudeUpdated(UAVObjEvent *ev)
}
}
float bound(float val, float limit)
{
return (val > limit) ? limit :
(val < -limit) ? -limit :
val;
}
#ifdef USE_GIMBAL_FF
void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab)
{

91
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -85,7 +85,6 @@ static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude();
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static float bound(float val, float min, float max);
/**
* Initialise the module, called on startup
@ -277,9 +276,9 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress, 0, 1);
boundf(progress.fractional_progress, 0, 1);
altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
bound(progress.fractional_progress, 0, 1);
boundf(progress.fractional_progress, 0, 1);
break;
}
// make sure groundspeed is not zero
@ -356,9 +355,10 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
@ -427,15 +427,15 @@ static uint8_t updateFixedDesiredAttitude()
// Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias,
fixedwingpathfollowerSettings.HorizontalVelMin,
fixedwingpathfollowerSettings.HorizontalVelMax);
indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias,
fixedwingpathfollowerSettings.HorizontalVelMin,
fixedwingpathfollowerSettings.HorizontalVelMax);
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = bound(
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedwingpathfollowerSettings.VerticalVelMax,
fixedwingpathfollowerSettings.VerticalVelMax);
@ -473,14 +473,14 @@ static uint8_t updateFixedDesiredAttitude()
*/
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else { powerIntegral = 0; }
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = bound(
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
@ -497,9 +497,9 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
@ -529,16 +529,16 @@ static uint8_t updateFixedDesiredAttitude()
if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
// Integrate with saturation
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
}
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
);
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp
@ -549,9 +549,9 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
@ -606,9 +606,9 @@ static uint8_t updateFixedDesiredAttitude()
courseError -= 360.0f;
}
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
courseIntegral);
@ -616,10 +616,10 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
fixedwingpathfollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.Max);
stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
@ -631,9 +631,10 @@ static uint8_t updateFixedDesiredAttitude()
stabDesired.Yaw = 0;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
@ -642,20 +643,6 @@ static uint8_t updateFixedDesiredAttitude()
return result;
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);

133
flight/modules/ManualControl/altitudehandler.c
View File

@ -1,133 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file altitudehandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @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 "inc/manualcontrol.h"
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <altitudeholddesired.h>
#include <altitudeholdsettings.h>
#include <positionstate.h>
#if defined(REVOLUTION)
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
* @input: ManualControlCommand
* @output: AltitudeHoldDesired
*/
void altitudeHandler(bool newinit)
{
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
if (newinit) {
StabilizationBankInitialize();
AltitudeHoldDesiredInitialize();
AltitudeHoldSettingsInitialize();
PositionStateInitialize();
}
// this is the max speed in m/s at the extents of thrust
float thrustRate;
uint8_t thrustExp;
static uint8_t flightMode;
static bool newaltitude = true;
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
PositionStateData posState;
PositionStateGet(&posState);
altitudeHoldDesiredData.Roll = cmd.Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
if (newinit) {
newaltitude = true;
}
uint8_t cutOff;
AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff);
if (cutOff && cmd.Thrust < 0) {
// Cut thrust if desired
altitudeHoldDesiredData.SetPoint = cmd.Thrust;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd.Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd.Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust < DEADBAND_LOW) {
altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd.Thrust < 0 ? 0 : cmd.Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd.Thrust) / DEADBAND_LOW) / 255 * thrustRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (newaltitude == true) {
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false;
}
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
}
#else /* if defined(REVOLUTION) */
void altitudeHandler(__attribute__((unused)) bool newinit)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
}
#endif // REVOLUTION
/**
* @}
* @}
*/

129
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -61,13 +61,6 @@ void manualHandler(bool newinit);
*/
void stabilizedHandler(bool newinit);
/**
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
* @input: ManualControlCommand
* @output: AltitudeHoldDesired
*/
void altitudeHandler(bool newinit);
/**
* @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
@ -88,29 +81,114 @@ void pathPlannerHandler(bool newinit);
*/
#define assumptions1 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions2 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions3 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions4 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions5 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions6 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions7 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_NUMELEM) && \
1 \
)
#define assumptions_flightmode \
@ -119,15 +197,16 @@ void pathPlannerHandler(bool newinit);
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED4) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED5) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED6) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) && \
1 \
)
#endif // MANUALCONTROL_H

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

@ -85,16 +85,6 @@ static const controlHandler handler_AUTOTUNE = {
};
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
// TODO: move the altitude handling into stabi
static const controlHandler handler_ALTITUDE = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &altitudeHandler,
};
static const controlHandler handler_PATHFOLLOWER = {
.controlChain = {
.Stabilization = true,
@ -123,7 +113,7 @@ static void commandUpdatedCb(UAVObjEvent *ev);
static void manualControlTask(void);
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode)
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
/**
* Module starting
@ -203,10 +193,12 @@ static void manualControlTask(void)
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
handler = &handler_STABILIZED;
break;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
@ -216,10 +208,6 @@ static void manualControlTask(void)
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
handler = &handler_PATHPLANNER;
break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
handler = &handler_ALTITUDE;
break;
#endif
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
handler = &handler_AUTOTUNE;

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

@ -78,6 +78,15 @@ void stabilizedHandler(bool newinit)
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
stab_settings = cast_struct_to_array(settings.Stabilization4Settings, settings.Stabilization4Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
stab_settings = cast_struct_to_array(settings.Stabilization5Settings, settings.Stabilization5Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
stab_settings = cast_struct_to_array(settings.Stabilization6Settings, settings.Stabilization6Settings.Roll);
break;
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);
@ -86,25 +95,25 @@ void stabilizedHandler(bool newinit)
}
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax :
0; // this is an invalid mode
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
@ -120,19 +129,20 @@ void stabilizedHandler(bool newinit)
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
0; // this is an invalid mode
}
stabilization.Thrust = cmd.Thrust;
stabilization.StabilizationMode.Thrust = stab_settings[3];
StabilizationDesiredSet(&stabilization);
}

187
flight/modules/AltitudeHold/altitudehold.c → flight/modules/Stabilization/altitudeloop.c
View File

@ -1,8 +1,8 @@
/**
******************************************************************************
*
* @file guidance.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file altitudeloop.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
@ -26,89 +26,122 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input object: ActiveWaypoint
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
* This module will periodically update the value of the AttitudeDesired object.
*
* The module executes in its own thread in this example.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <altitudeloop.h>
#include <CoordinateConversions.h>
#include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module
#include <altitudeholdstatus.h>
#include <flightstatus.h>
#include <stabilizationdesired.h>
#include <accelstate.h>
#include <pios_constants.h>
#include <velocitystate.h>
#include <positionstate.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 1024
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
#ifdef REVOLUTION
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 512
// Private types
// Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
static ThrustModeType thrustMode;
static PiOSDeltatimeConfig timeval;
static float thrustSetpoint = 0.0f;
static float thrustDemand = 0.0f;
static float startThrust = 0.5f;
// Private functions
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void VelocityStateUpdatedCb(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
* Setup mode and setpoint
*/
int32_t AltitudeHoldStart()
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
{
// Start main task
SettingsUpdatedCb(NULL);
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
static bool newaltitude = true;
return 0;
if (reinit) {
startThrust = setpoint;
pid_zero(&pid0);
pid_zero(&pid1);
newaltitude = true;
}
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// this is the max speed in m/s at the extents of thrust
float thrustRate;
uint8_t thrustExp;
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
PositionStateData posState;
PositionStateGet(&posState);
if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
// Cut thrust if desired
thrustSetpoint = 0.0f;
thrustDemand = 0.0f;
thrustMode = DIRECT;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (newaltitude == true) {
thrustSetpoint = posState.Down;
thrustMode = ALTITUDEHOLD;
newaltitude = false;
}
return thrustDemand;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldInitialize()
void stabilizationAltitudeloopInit()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize();
AltitudeHoldStatusInitialize();
PositionStateInitialize();
VelocityStateInitialize();
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Create object queue
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VelocityStateConnectCallback(&VelocityStateUpdatedCb);
return 0;
// Start main task
SettingsUpdatedCb(NULL);
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
/**
@ -116,44 +149,25 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
*/
static void altitudeHoldTask(void)
{
static float startThrust = 0.5f;
// make sure we run only when we are supposed to run
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
break;
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrustGet(&startThrust);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
break;
}
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
// do the actual control loop(s)
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
switch (thrustMode) {
case ALTITUDEHOLD:
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT);
break;
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
@ -162,37 +176,16 @@ static void altitudeHoldTask(void)
AltitudeHoldStatusSet(&altitudeHoldStatus);
float thrust;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
thrust = altitudeHoldDesired.SetPoint;
switch (thrustMode) {
case DIRECT:
thrustDemand = thrustSetpoint;
break;
default:
// velocity control loop
thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
if (thrust >= 1.0f) {
thrust = 1.0f;
}
if (thrust <= 0.0f) {
thrust = 0.0f;
}
break;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Thrust = thrust;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
@ -203,3 +196,11 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
}
#endif /* ifdef REVOLUTION */

293
flight/modules/Stabilization/cruisecontrol.c Normal file
View File

@ -0,0 +1,293 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief cruisecontrol mode
* @note This file implements the logic for a cruisecontrol
* @{
*
* @file cruisecontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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 <stabilization.h>
#include <attitudestate.h>
#include <sin_lookup.h>
static float cruisecontrol_factor = 1.0f;
static inline float CruiseControlLimitThrust(float thrust)
{
// limit to user specified absolute max thrust
return boundf(thrust, stabSettings.cruiseControl.min_thrust, stabSettings.cruiseControl.max_thrust);
}
// assumes 1.0 <= factor <= 100.0
// a factor of less than 1.0 could make it return a value less than stabSettings.cruiseControl.min_thrust
// CP helis need to have min_thrust=-1
//
// multicopters need to have min_thrust=0.05 or so
// values below that will not be subject to max / min limiting
// that means thrust can be less than min
// that means multicopter motors stop spinning at low stick
static inline float CruiseControlFactorToThrust(float factor, float thrust)
{
// don't touch thrust if it's less than min_thrust
// without that test, quadcopter props will spin up
// to min thrust even at zero throttle stick
// if Cruise Control is enabled on this flight switch position
if (thrust > stabSettings.cruiseControl.min_thrust) {
return CruiseControlLimitThrust(thrust * factor);
}
return thrust;
}
static float CruiseControlAngleToFactor(float angle)
{
float factor;
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = stabSettings.settings.CruiseControlMaxPowerFactor;
} else {
// the simple bank angle boost calculation that Cruise Control revolves around
factor = 1.0f / fabsf(cos_lookup_deg(angle));
// factor in the power trim, no effect at 1.0, linear effect increases with factor
factor = (factor - 1.0f) * stabSettings.cruiseControl.power_trim + 1.0f;
// limit to user specified max power multiplier
if (factor > stabSettings.settings.CruiseControlMaxPowerFactor) {
factor = stabSettings.settings.CruiseControlMaxPowerFactor;
}
}
return factor;
}
void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand)
{
static float previous_angle;
static uint32_t previous_time = 0;
static bool previous_time_valid = false;
// For multiple, speedy flips this mainly strives to address the
// fact that (due to thrust delay) thrust didn't average straight
// down, but at an angle. For less speedy flips it acts like it
// used to. It can be turned off by setting power delay to 0.
// It takes significant time for the motors of a multi-copter to
// spin up. It takes significant time for the collective servo of
// a CP heli to move from one end to the other. Both of those are
// modeled here as linear, i.e. twice as much change takes twice
// as long. Given a correctly configured maximum delay time this
// code calculates how far in advance to start the control
// transition so that half way through the physical transition it
// is just crossing the transition angle.
// Example: Rotation rate = 360. Full stroke delay = 0.2
// Transition angle 90 degrees. Start the transition 0.1 second
// before 90 degrees (36 degrees at 360 deg/sec) and it will be
// complete 0.1 seconds after 90 degrees.
// Note that this code only handles the transition to/from inverted
// thrust. It doesn't handle the case where thrust is changed a
// lot in a small angle range when that range is close to 90 degrees.
// It doesn't handle the small constant "system delay" caused by the
// delay between reading sensors and actuators beginning to respond.
// It also assumes that the pilot is holding the throttle constant;
// when the pilot does change the throttle, the compensation is
// simply recalculated.
// This implementation of future thrust isn't perfect. That would
// probably require an iterative procedure for solving a
// transcendental equation of the form linear(x) = 1/cos(x). It's
// shortcomings generally don't hurt anything and work better than
// without it. It is designed to work perfectly if the pilot is
// using full thrust during flips and it is only activated if 70% or
// greater thrust is used.
uint32_t time = PIOS_DELAY_GetuS();
// Get roll and pitch angles, calculate combined angle, and begin
// the general algorithm.
// Example: 45 degrees roll plus 45 degrees pitch = 60 degrees
// Do it every 8th iteration to save CPU.
if (time != previous_time || previous_time_valid == false) {
float angle, angle_unmodified;
// spherical right triangle
// 0.0 <= angle <= 180.0
angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitude->Roll)
* cos_lookup_deg(attitude->Pitch)));
// Calculate rate as a combined (roll and pitch) bank angle
// change; in degrees per second. Rate is calculated over the
// most recent 8 loops through stabilization. We could have
// asked the gyros. This is probably cheaper.
if (previous_time_valid) {
float rate;
// rate can be negative.
rate = (angle - previous_angle) / ((float)(time - previous_time) / 1000000.0f);
// Define "within range" to be those transitions that should
// be executing now. Recall that each impulse transition is
// spread out over a range of time / angle.
// There is only one transition and the high power level for
// it is either:
// 1/fabsf(cos(angle)) * current thrust
// or max power factor * current thrust
// or full thrust
// You can cross the transition with angle either increasing
// or decreasing (rate positive or negative).
// Thrust is never boosted for negative values of
// thrustDemand (negative stick values)
//
// When the aircraft is upright, thrust is always boosted
// . for positive values of thrustDemand
// When the aircraft is inverted, thrust is sometimes
// . boosted or reversed (or combinations thereof) or zeroed
// . for positive values of thrustDemand
// It depends on the inverted power settings.
// Of course, you can set MaxPowerFactor to 1.0 to
// . effectively disable boost.
if (thrustDemand > 0.0f) {
// to enable the future thrust calculations, make sure
// there is a large enough transition that the result
// will be roughly on vs. off; without that, it can
// exaggerate the length of time the inverted to upright
// transition holds full throttle and reduce the length
// of time for full throttle when going upright to inverted.
if (thrustDemand > 0.7f) {
float thrust;
thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand);
// determine if we are in range of the transition
// given the thrust at max_angle and thrustDemand
// (typically close to 1.0), change variable 'thrust' to
// be the proportion of the largest thrust change possible
// that occurs when going into inverted mode.
// Example: 'thrust' is 0.8 A quad has min_thrust set
// to 0.05 The difference is 0.75. The largest possible
// difference with this setup is 0.9 - 0.05 = 0.85, so
// the proportion is 0.75/0.85
// That is nearly a full throttle stroke.
// the 'thrust' variable is non-negative here
switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
// normal multi-copter case, stroke is max to zero
// technically max to constant min_thrust
// can be used by CP
thrust = (thrust - CruiseControlLimitThrust(0.0f)) / stabSettings.cruiseControl.thrust_difference;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
// reversed but not boosted
// : CP heli case, stroke is max to -stick
// : thrust = (thrust - CruiseControlLimitThrust(-thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
// else it is both unreversed and unboosted
// : simply turn off boost, stroke is max to +stick
// : thrust = (thrust - CruiseControlLimitThrust(thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
thrust = (thrust - CruiseControlLimitThrust(
(stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED)
? -thrustDemand
: thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
// if boosted and reversed
if (stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
// CP heli case, stroke is max to min
thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
}
// else it is boosted and unreversed so the throttle doesn't change
else {
// CP heli case, no transition, so stroke is zero
thrust = 0.0f;
}
break;
}
// 'thrust' is now the proportion of max stroke
// multiply this proportion of max stroke,
// times the max stroke time, to get this stroke time
// we only want half of this time before the transition
// (and half after the transition)
thrust *= stabSettings.cruiseControl.half_power_delay;
// 'thrust' is now the length of time for this stroke
// multiply that times angular rate to get the lead angle
thrust *= fabsf(rate);
// if the transition is within range we use it,
// else we just use the current calculated thrust
if ((float)stabSettings.settings.CruiseControlMaxAngle - thrust <= angle
&& angle <= (float)stabSettings.settings.CruiseControlMaxAngle + thrust) {
// default to a little above max angle
angle = (float)stabSettings.settings.CruiseControlMaxAngle + 0.01f;
// if roll direction is downward
// then thrust value is taken from below max angle
// by the code that knows about the transition angle
if (rate < 0.0f) {
angle -= 0.02f;
}
}
} // if thrust > 0.7; else just use the angle we already calculated
cruisecontrol_factor = CruiseControlAngleToFactor(angle);
} else { // if thrust > 0 set factor from angle; else
cruisecontrol_factor = 1.0f;
}
if (angle >= (float)stabSettings.settings.CruiseControlMaxAngle) {
switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
cruisecontrol_factor = 0.0f;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
cruisecontrol_factor = 1.0f;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
// no change, leave factor >= 1.0 alone
break;
}
if (stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
cruisecontrol_factor = -cruisecontrol_factor;
}
}
} // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone
previous_time = time;
previous_time_valid = true;
previous_angle = angle_unmodified;
} // every 8th time
}
float cruisecontrol_apply_factor(float raw)
{
if (stabSettings.settings.CruiseControlMaxPowerFactor > 0.0001f) {
raw = CruiseControlFactorToThrust(cruisecontrol_factor, raw);
}
return raw;
}

46
flight/modules/Airspeed/airspeedalarm.c → flight/modules/Stabilization/inc/altitudeloop.h
View File

@ -2,13 +2,14 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @addtogroup StabilizationModule Stabilization Module
* @brief altitudeloop mode
* @note This file implements the logic for a altitudeloop
* @{
*
* @file airspeedalarm.c
* @file altitudeloop.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
@ -29,37 +30,12 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: none
*
* Handle locally airspeed alarms issue changes to PIOS only when necessary
*
*/
#ifndef ALTITUDELOOP_H
#define ALTITUDELOOP_H
#include "airspeedalarm.h"
typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
// local variable
void stabilizationAltitudeloopInit();
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
static SystemAlarmsAlarmOptions severitySet = SYSTEMALARMS_ALARM_UNINITIALISED;
// functions
/**
* Handle airspeed alarms and isuue an Alarm to PIOS only if necessary
*/
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity)
{
if (severity == severitySet) {
return false;
}
severitySet = severity;
return AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, severity) == 0;
}
/**
* @}
* @}
*/
#endif /* ALTITUDELOOP_H */

42
flight/modules/Stabilization/inc/cruisecontrol.h Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief cruisecontrol mode
* @note This file implements the logic for a cruisecontrol
* @{
*
* @file cruisecontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#ifndef CRUISECONTROL_H
#define CRUISECONTROL_H
#include <openpilot.h>
#include <attitudestate.h>
void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand);
float cruisecontrol_apply_factor(float raw);
#endif /* CRUISECONTROL_H */

25
flight/modules/Airspeed/inc/airspeedalarm.h → flight/modules/Stabilization/inc/innerloop.h
View File

@ -2,13 +2,14 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @addtogroup StabilizationModule Stabilization Module
* @brief innerloop mode
* @note This file implements the logic for a innerloop
* @{
*
* @file airspeedalarm.h
* @file innerloop.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, reads temperature and pressure from MS4525DO
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
@ -28,18 +29,10 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AIRSPEEDALARM_H
#define AIRSPEEDALARM_H
#include <openpilot.h>
#include "alarms.h"
#ifndef INNERLOOP_H
#define INNERLOOP_H
void stabilizationInnerloopInit();
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity);
#endif // AIRSPEEDALARM_H
/**
* @}
* @}
*/
#endif /* INNERLOOP_H */

38
flight/modules/Stabilization/inc/outerloop.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief outerloop mode
* @note This file implements the logic for a outerloop
* @{
*
* @file outerloop.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#ifndef OUTERLOOP_H
#define OUTERLOOP_H
void stabilizationOuterloopInit();
#endif /* OUTERLOOP_H */

44
flight/modules/Stabilization/inc/stabilization.h
View File

@ -33,10 +33,52 @@
#ifndef STABILIZATION_H
#define STABILIZATION_H
enum { ROLL, PITCH, YAW, MAX_AXES };
#include <openpilot.h>
#include <pid.h>
#include <stabilizationsettings.h>
#include <stabilizationbank.h>
int32_t StabilizationInitialize();
typedef struct {
StabilizationSettingsData settings;
StabilizationBankData stabBank;
float gyro_alpha;
struct {
float min_thrust;
float max_thrust;
float thrust_difference;
float power_trim;
float half_power_delay;
float max_power_factor_angle;
} cruiseControl;
struct {
int8_t gyroupdates;
int8_t rateupdates;
} monitor;
float rattitude_mode_transition_stick_position;
struct pid innerPids[3], outerPids[3];
} StabilizationData;
extern StabilizationData stabSettings;
#define AXES 4
#define FAILSAFE_TIMEOUT_MS 30
#ifndef PIOS_STABILIZATION_STACK_SIZE
#define STACK_SIZE_BYTES 800
#else
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#endif
// must be same as eventdispatcher to avoid needing additional mutexes
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
// outer loop only executes every 4th uavobject update to safe CPU
#define OUTERLOOP_SKIPCOUNT 4
#endif // STABILIZATION_H
/**

289
flight/modules/Stabilization/innerloop.c Normal file
View File

@ -0,0 +1,289 @@
/**
******************************************************************************
* @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 innerloop.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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_struct_helper.h>
#include <pid.h>
#include <callbackinfo.h>
#include <ratedesired.h>
#include <actuatordesired.h>
#include <gyrostate.h>
#include <airspeedstate.h>
#include <stabilizationstatus.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <stabilization.h>
#include <relay_tuning.h>
#include <virtualflybar.h>
#include <cruisecontrol.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
// Private variables
static DelayedCallbackInfo *callbackHandle;
static float gyro_filtered[3] = { 0, 0, 0 };
static float axis_lock_accum[3] = { 0, 0, 0 };
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval;
static float speedScaleFactor = 1.0f;
// Private functions
static void stabilizationInnerloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#ifdef REVOLUTION
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#endif
void stabilizationInnerloopInit()
{
RateDesiredInitialize();
ActuatorDesiredInitialize();
GyroStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
#ifdef REVOLUTION
AirspeedStateInitialize();
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
#endif
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
GyroStateConnectCallback(GyroStateUpdatedCb);
// schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationInnerloopTask()
{
// watchdog and error handling
{
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
bool warn = false;
bool error = false;
bool crit = false;
// check if outer loop keeps executing
if (stabSettings.monitor.rateupdates > -64) {
stabSettings.monitor.rateupdates--;
}
if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) {
// warning if rate loop skipped more than 2 execution
warn = true;
}
if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
// error if rate loop skipped more than 4 executions
error = true;
}
// check if gyro keeps updating
if (stabSettings.monitor.gyroupdates < 1) {
// critical if gyro didn't update at all!
crit = true;
}
if (stabSettings.monitor.gyroupdates > 1) {
// warning if we missed a gyro update
warn = true;
}
if (stabSettings.monitor.gyroupdates > 3) {
// error if we missed 3 gyro updates
error = true;
}
stabSettings.monitor.gyroupdates = 0;
if (crit) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
} else if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else if (warn) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
RateDesiredData rateDesired;
ActuatorDesiredData actuator;
StabilizationStatusInnerLoopData enabled;
FlightStatusControlChainData cchain;
RateDesiredGet(&rateDesired);
ActuatorDesiredGet(&actuator);
StabilizationStatusInnerLoopGet(&enabled);
FlightStatusControlChainGet(&cchain);
float *rate = &rateDesired.Roll;
float *actuatorDesiredAxis = &actuator.Roll;
int t;
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break;
case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK:
if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) {
// While getting strong commands act like rate mode
axis_lock_accum[t] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT;
axis_lock_accum[t] = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock);
rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp;
}
// 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],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
}
actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
actuator.UpdateTime = dT * 1000;
if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuator);
} else {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
{
uint8_t armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
GyroStateData gyroState;
GyroStateGet(&gyroState);
gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha);
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
stabSettings.monitor.gyroupdates++;
}
#ifdef REVOLUTION
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
AirspeedStateData airspeedState;
AirspeedStateGet(&airspeedState);
if (stabSettings.settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
// feature has been turned off
speedScaleFactor = 1.0f;
} else {
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
speedScaleFactor = boundf((stabSettings.settings.ScaleToAirspeed * stabSettings.settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed),
stabSettings.settings.ScaleToAirspeedLimits.Min,
stabSettings.settings.ScaleToAirspeedLimits.Max);
}
}
#endif
/**
* @}
* @}
*/

288
flight/modules/Stabilization/outerloop.c Normal file
View File

@ -0,0 +1,288 @@
/**
******************************************************************************
* @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 outerloop.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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_struct_helper.h>
#include <pid.h>
#include <callbackinfo.h>
#include <ratedesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <stabilizationstatus.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <stabilization.h>
#include <cruisecontrol.h>
#include <altitudeloop.h>
#include <CoordinateConversions.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
// Private variables
static DelayedCallbackInfo *callbackHandle;
static AttitudeStateData attitude;
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval;
// Private functions
static void stabilizationOuterloopTask();
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
void stabilizationOuterloopInit()
{
RateDesiredInitialize();
StabilizationDesiredInitialize();
AttitudeStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationOuterloopTask()
{
AttitudeStateData attitudeState;
RateDesiredData rateDesired;
StabilizationDesiredData stabilizationDesired;
StabilizationStatusOuterLoopData enabled;
AttitudeStateGet(&attitudeState);
StabilizationDesiredGet(&stabilizationDesired);
RateDesiredGet(&rateDesired);
StabilizationStatusOuterLoopGet(&enabled);
float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
int t;
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
float local_error[3];
{
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
rpy_desired[0] = stabilizationDesiredAxis[0];
rpy_desired[1] = stabilizationDesiredAxis[1];
rpy_desired[2] = stabilizationDesiredAxis[2];
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeState.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory
local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll;
local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch;
local_error[2] = stabilizationDesiredAxis[2] - attitudeState.Yaw;
// find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if (modulo < 0) {
local_error[2] = modulo + 180.0f;
} else {
local_error[2] = modulo - 180.0f;
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break;
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
float magnitude = fabsf(stickinput[t]);
if (t < 2) {
magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
}
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
float rate_input = cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabSettings.stabBank, stabSettings.stabBank.RollMax)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[t] = rate_input + weak_leveling;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
}
}
RateDesiredSet(&rateDesired);
{
uint8_t armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
// update cruisecontrol based on attitude
cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
stabSettings.monitor.rateupdates = 0;
}
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// to reduce CPU utilisation, outer loop is not executed every state update
static uint8_t cpusafer = 0;
if ((cpusafer++ % OUTERLOOP_SKIPCOUNT) == 0) {
// this does not need mutex protection as both eventdispatcher and stabi run in same callback task!
AttitudeStateGet(&attitude);
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
}
}
/**
* @}
* @}
*/

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

@ -63,7 +63,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
portTickType thisTime = xTaskGetTickCount();
static bool rateRelayRunning[MAX_AXES];
static bool rateRelayRunning[3];
// This indicates the current estimate of the smoothed error. So when it is high
// we are waiting for it to go low.

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

@ -33,121 +33,55 @@
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "stabilization.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
#include "stabilizationsettingsbank3.h"
#include "actuatordesired.h"
#include "ratedesired.h"
#include "relaytuning.h"
#include "relaytuningsettings.h"
#include "stabilizationdesired.h"
#include "attitudestate.h"
#include "airspeedstate.h"
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
#include "flightmodesettings.h"
#include "taskinfo.h"
#include <pid.h>
#include <manualcontrolcommand.h>
#include <flightmodesettings.h>
#include <stabilizationsettings.h>
#include <stabilizationdesired.h>
#include <stabilizationstatus.h>
#include <stabilizationbank.h>
#include <stabilizationsettingsbank1.h>
#include <stabilizationsettingsbank2.h>
#include <stabilizationsettingsbank3.h>
#include <relaytuning.h>
#include <relaytuningsettings.h>
#include <ratedesired.h>
#include <sin_lookup.h>
#include <stabilization.h>
#include <innerloop.h>
#include <outerloop.h>
#include <altitudeloop.h>
// Math libraries
#include "CoordinateConversions.h"
#include "pid.h"
#include "sin_lookup.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
#include "virtualflybar.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
// Private constants
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define MAX_QUEUE_SIZE 1
#if defined(PIOS_STABILIZATION_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#else
#define STACK_SIZE_BYTES 860
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
#define FAILSAFE_TIMEOUT_MS 30
// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
// The PID_RATE set is used by the attitude portion of Attitude mode
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
// Public variables
StabilizationData stabSettings;
// Private variables
static xTaskHandle taskHandle;
static StabilizationSettingsData settings;
static xQueueHandle queue;
float gyro_alpha = 0;
float axis_lock_accum[3] = { 0, 0, 0 };
uint8_t max_axis_lock = 0;
uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
int cur_flight_mode = -1;
static float rattitude_mode_transition_stick_position;
static float cruise_control_min_thrust;
static float cruise_control_max_thrust;
static uint8_t cruise_control_max_angle;
static float cruise_control_max_power_factor;
static float cruise_control_power_trim;
static int8_t cruise_control_inverted_power_switch;
static float cruise_control_neutral_thrust;
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
static int cur_flight_mode = -1;
// Private functions
static void stabilizationTask(void *parameters);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void BankUpdatedCb(UAVObjEvent *ev);
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
/**
* Module initialization
*/
int32_t StabilizationStart()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeStateConnectQueue(queue);
GyroStateConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
// Start main task
xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
#endif
@ -160,650 +94,189 @@ int32_t StabilizationStart()
int32_t StabilizationInitialize()
{
// Initialize variables
ManualControlCommandInitialize();
ManualControlSettingsInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
StabilizationSettingsInitialize();
StabilizationStatusInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
ActuatorDesiredInitialize();
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
#endif
#ifdef REVOLUTION
AirspeedStateInitialize();
#endif
ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
// Code required for relay tuning
sin_lookup_initalize();
RelayTuningSettingsInitialize();
RelayTuningInitialize();
stabilizationOuterloopInit();
stabilizationInnerloopInit();
#ifdef REVOLUTION
stabilizationAltitudeloopInit();
#endif
pid_zero(&stabSettings.outerPids[0]);
pid_zero(&stabSettings.outerPids[1]);
pid_zero(&stabSettings.outerPids[2]);
pid_zero(&stabSettings.innerPids[0]);
pid_zero(&stabSettings.innerPids[1]);
pid_zero(&stabSettings.innerPids[2]);
return 0;
}
MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
/**
* Module task
*/
static void stabilizationTask(__attribute__((unused)) void *parameters)
static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
UAVObjEvent ev;
PiOSDeltatimeConfig timeval;
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
float throttleDesired;
RateDesiredData rateDesired;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
FlightStatusData flightStatus;
StabilizationBankData stabBank;
#ifdef REVOLUTION
AirspeedStateData airspeedState;
#endif
SettingsUpdatedCb((UAVObjEvent *)NULL);
// Main task loop
ZeroPids();
while (1) {
float dT;
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
// Wait until the Gyro object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
continue;
}
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
ManualControlCommandThrottleGet(&throttleDesired);
AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroStateData);
StabilizationBankGet(&stabBank);
#ifdef DIAG_RATEDESIRED
RateDesiredGet(&rateDesired);
#endif
uint8_t flight_mode_switch_position;
ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position);
if (cur_flight_mode != flight_mode_switch_position) {
cur_flight_mode = flight_mode_switch_position;
SettingsBankUpdatedCb(NULL);
}
#ifdef REVOLUTION
float speedScaleFactor;
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
AirspeedStateGet(&airspeedState);
if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
// feature has been turned off
speedScaleFactor = 1.0f;
} else {
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) {
speedScaleFactor = settings.ScaleToAirspeedLimits.Min;
}
if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) {
speedScaleFactor = settings.ScaleToAirspeedLimits.Max;
}
}
#else
const float speedScaleFactor = 1.0f;
#endif
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
float local_error[3];
// Essentially zero errors for anything in rate or none
if (stabDesired.StabilizationMode.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[0] = stabDesired.Roll;
} else {
rpy_desired[0] = attitudeState.Roll;
}
if (stabDesired.StabilizationMode.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[1] = stabDesired.Pitch;
} else {
rpy_desired[1] = attitudeState.Pitch;
}
if (stabDesired.StabilizationMode.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[2] = stabDesired.Yaw;
} else {
rpy_desired[2] = attitudeState.Yaw;
}
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeState.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory
float local_error[3] = { stabDesired.Roll - attitudeState.Roll,
stabDesired.Pitch - attitudeState.Pitch,
stabDesired.Yaw - attitudeState.Yaw };
// find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if (modulo < 0) {
local_error[2] = modulo + 180.0f;
} else {
local_error[2] = modulo - 180.0f;
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
float gyro_filtered[3];
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyroStateData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha);
float *stabDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
ActuatorDesiredGet(&actuatorDesired);
// A flag to track which stabilization mode each axis is in
static uint8_t previous_mode[MAX_AXES] = { 255, 255, 255 };
bool error = false;
// Run the selected stabilization algorithm on each axis:
for (uint8_t i = 0; i < MAX_AXES; i++) {
// Check whether this axis mode needs to be reinitialized
bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]);
previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i];
// Apply the selected control law
switch (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] =
bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
// A parameterization from Attitude mode at center stick
// - to Rate mode at full stick
// This is done by parameterizing to use the rotation rate that Attitude mode
// - would use at center stick to using the rotation rate that Rate mode
// - would use at full stick in a weighted average sort of way.
{
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// Compute what Rate mode would give for this stick angle's rate
// Save Rate's rate in a temp for later merging with Attitude's rate
float rateDesiredAxisRate;
rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f)
* cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
// Compute what Attitude mode would give for this stick angle's rate
// stabDesired for this mode is [-1.0f,+1.0f]
// - multiply by Attitude mode max angle to get desired angle
// - subtract off the actual angle to get the angle error
// This is what local_error[] holds for Attitude mode
float attitude_error = stabDesiredAxis[i]
* cast_struct_to_array(stabBank.RollMax, stabBank.RollMax)[i]
- cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i];
// Compute the outer loop just like Attitude mode does
float rateDesiredAxisAttitude;
rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude,
cast_struct_to_array(stabBank.ManualRate,
stabBank.ManualRate.Roll)[i]);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
float magnitude;
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
+ magnitude * rateDesiredAxisRate;
// Compute the inner loop for the averaged rate
// actuatorDesiredAxis[i] is the weighted average
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = stabDesiredAxis[i];
// Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
float weak_leveling = local_error[i] * weak_leveling_kp;
weak_leveling = bound(weak_leveling, weak_leveling_max);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = stabDesiredAxis[i];
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
}
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(stabDesiredAxis[i],
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
}
// Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f);
break;
default:
error = true;
break;
}
}
if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) {
stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
}
#ifdef DIAG_RATEDESIRED
RateDesiredSet(&rateDesired);
#endif
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Thrust = stabDesired.Thrust;
// modify thrust according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// but without manually adjusting thrust
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
&& cruise_control_max_power_factor > 0.0001f) {
static uint8_t toggle;
static float factor;
float angle;
// get attitude state and calculate angle
// do it every 8th iteration to save CPU
if ((toggle++ & 7) == 0) {
// spherical right triangle
// 0 <= acosf() <= Pi
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
// if past the cutoff angle (60 to 180 (180 means never))
if (angle > cruise_control_max_angle) {
// -1 reversed collective, 0 zero power, or 1 normal power
// these are all unboosted
factor = cruise_control_inverted_power_switch;
} else {
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = cruise_control_max_power_factor;
} else {
factor = 1.0f / fabsf(cos_lookup_deg(angle));
if (factor > cruise_control_max_power_factor) {
factor = cruise_control_max_power_factor;
}
}
// factor in the power trim, no effect at 1.0, linear effect increases with factor
factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
// if inverted and they want negative boost
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
factor = -factor;
// as long as thrust is getting reversed
// we may as well do pitch and yaw for a complete "invert switch"
actuatorDesired.Pitch = -actuatorDesired.Pitch;
actuatorDesired.Yaw = -actuatorDesired.Yaw;
}
}
}
// also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
if (actuatorDesired.Thrust > cruise_control_min_thrust) {
// quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
// CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
actuatorDesired.Thrust = (actuatorDesired.Thrust - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Thrust > cruise_control_max_thrust) {
actuatorDesired.Thrust = cruise_control_max_thrust;
} else if (actuatorDesired.Thrust < cruise_control_min_thrust) {
actuatorDesired.Thrust = cruise_control_min_thrust;
}
}
}
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
for (uint8_t i = 0; i < MAX_AXES; i++) {
previous_mode[i] = 255;
}
}
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (uint8_t i = 0; i < MAX_AXES; i++) {
previous_mode[i] = 255;
}
}
// Clear or set alarms. Done like this to prevent toggline each cycle
// and hammering system alarms
if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
StabilizationStatusData status;
StabilizationDesiredStabilizationModeData mode;
int t;
StabilizationDesiredStabilizationModeGet(&mode);
for (t = 0; t < AXES; t++) {
switch (cast_struct_to_array(mode, mode.Roll)[t]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
}
}
StabilizationStatusSet(&status);
}
/**
* Clear the accumulators and derivatives for all the axes
*/
static void ZeroPids(void)
static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
for (uint32_t i = 0; i < PID_MAX; i++) {
pid_zero(&pids[i]);
}
uint8_t fm;
ManualControlCommandFlightModeSwitchPositionGet(&fm);
for (uint8_t i = 0; i < 3; i++) {
axis_lock_accum[i] = 0.0f;
if (fm == cur_flight_mode) {
return;
}
cur_flight_mode = fm;
SettingsBankUpdatedCb(NULL);
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
return -range;
} else if (val > range) {
return range;
}
return val;
}
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
return;
}
if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
(settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
(settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
settings.FlightModeMap[cur_flight_mode] > 2)) {
if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
return;
}
StabilizationBankData bank;
switch (settings.FlightModeMap[cur_flight_mode]) {
switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
break;
}
StabilizationBankSet(&bank);
StabilizationBankSet(&stabSettings.stabBank);
}
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankData bank;
StabilizationBankGet(&bank);
// this code will be needed if any other modules alter stabilizationbank
/*
StabilizationBankData curBank;
if(flight_mode < 0) return;
switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
{
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank);
}
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank);
}
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank);
}
break;
default:
return; //invalid bank number
}
*/
StabilizationBankGet(&stabSettings.stabBank);
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
stabSettings.stabBank.RollRatePID.Ki,
stabSettings.stabBank.RollRatePID.Kd,
stabSettings.stabBank.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], bank.PitchRatePID.Kp,
bank.PitchRatePID.Ki,
bank.PitchRatePID.Kd,
bank.PitchRatePID.ILimit);
pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
stabSettings.stabBank.PitchRatePID.Ki,
stabSettings.stabBank.PitchRatePID.Kd,
stabSettings.stabBank.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], bank.YawRatePID.Kp,
bank.YawRatePID.Ki,
bank.YawRatePID.Kd,
bank.YawRatePID.ILimit);
pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
stabSettings.stabBank.YawRatePID.Ki,
stabSettings.stabBank.YawRatePID.Kd,
stabSettings.stabBank.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], bank.RollPI.Kp,
bank.RollPI.Ki,
pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
stabSettings.stabBank.RollPI.Ki,
0,
bank.RollPI.ILimit);
stabSettings.stabBank.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp,
bank.PitchPI.Ki,
pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
stabSettings.stabBank.PitchPI.Ki,
0,
bank.PitchPI.ILimit);
stabSettings.stabBank.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], bank.YawPI.Kp,
bank.YawPI.Ki,
pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
stabSettings.stabBank.YawPI.Ki,
0,
bank.YawPI.ILimit);
stabSettings.stabBank.YawPI.ILimit);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationSettingsGet(&settings);
// needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
StabilizationSettingsGet(&stabSettings.settings);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while thrust is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
@ -811,37 +284,29 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation
const float fakeDt = 0.0025f;
if (settings.GyroTau < 0.0001f) {
gyro_alpha = 0; // not trusting this to resolve to 0
if (stabSettings.settings.GyroTau < 0.0001f) {
stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
} else {
gyro_alpha = expf(-fakeDt / settings.GyroTau);
stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
}
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
// force flight mode update
cur_flight_mode = -1;
// Rattitude stick angle where the attitude to rate transition happens
if (settings.RattitudeModeTransition < (uint8_t)10) {
rattitude_mode_transition_stick_position = 10.0f / 100.0f;
if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
} else {
rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f;
stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
}
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f;
cruise_control_max_angle = settings.CruiseControlMaxAngle;
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
cruise_control_neutral_thrust = (float)settings.CruiseControlNeutralThrust / 100.0f;
stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust;
memcpy(
cruise_control_flight_mode_switch_pos_enable,
settings.CruiseControlFlightModeSwitchPosEnable,
sizeof(cruise_control_flight_mode_switch_pos_enable));
stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f;
stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor));
}
/**

26
flight/modules/Stabilization/virtualflybar.c
View File

@ -37,12 +37,9 @@
#include "stabilizationsettings.h"
// ! Private variables
static float vbar_integral[MAX_AXES];
static float vbar_integral[3];
static float vbar_decay = 0.991f;
// ! Private methods
static float bound(float val, float range);
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
{
float gyro_gain = 1.0f;
@ -54,7 +51,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Track the angle of the virtual flybar which includes a slow decay
vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle);
vbar_integral[axis] = boundf(vbar_integral[axis], -settings->VbarMaxAngle, settings->VbarMaxAngle);
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
if (settings->VbarGyroSuppress > 0) {
@ -64,15 +61,15 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Get the settings for the correct axis
switch (axis) {
case ROLL:
case 0:
kp = settings->VbarRollPI.Kp;
ki = settings->VbarRollPI.Ki;
break;
case PITCH:
case 1:
kp = settings->VbarPitchPI.Kp;
ki = settings->VbarPitchPI.Ki;;
break;
case YAW:
case 2:
kp = settings->VbarYawPI.Kp;
ki = settings->VbarYawPI.Ki;
break;
@ -105,16 +102,3 @@ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
return 0;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
} else if (val > range) {
val = range;
}
return val;
}

6
flight/modules/StateEstimation/filteraltitude.c
View File

@ -33,6 +33,7 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <altitudefiltersettings.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
@ -65,6 +66,7 @@ struct data {
bool first_run;
portTickType initTimer;
AltitudeFilterSettingsData settings;
float gravity;
};
// Private variables
@ -81,6 +83,7 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
HomeLocationInitialize();
AttitudeStateInitialize();
AltitudeFilterSettingsInitialize();
AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
@ -107,6 +110,7 @@ static int32_t init(stateFilter *self)
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
HomeLocationg_eGet(&this->gravity);
return 0;
}
@ -145,7 +149,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
AttitudeStateGet(&att);
float Rbe[3][3];
Quaternion2R(&att.q1, Rbe);
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity);
// low pass filter accelerometers
this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;

1
flight/modules/StateEstimation/filterekf.c
View File

@ -361,6 +361,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
}
INSSetMagNorth(this->homeLocation.Be);
INSSetG(this->homeLocation.g_e);
if (!this->usePos) {
// position and velocity variance used in indoor mode

24
flight/modules/StateEstimation/stateestimation.c
View File

@ -155,6 +155,10 @@ static stateFilter cfmFilter;
static stateFilter ekf13iFilter;
static stateFilter ekf13Filter;
// this is a hack to provide a computational shortcut for faster gyro state progression
static float gyroRaw[3];
static float gyroDelta[3];
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static const filterPipeline *cfQueue = &(filterPipeline) {
.filter = &magFilter,
@ -415,6 +419,11 @@ static void StateEstimationCb(void)
// fetch sensors, check values, and load into state struct
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroRaw[0] = states.gyro[0];
gyroRaw[1] = states.gyro[1];
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
@ -453,7 +462,12 @@ static void StateEstimationCb(void)
case RUNSTATE_SAVE:
// the final output of filters is saved in state variables
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z);
// EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroDelta[0] = states.gyro[0] - gyroRaw[0];
gyroDelta[1] = states.gyro[1] - gyroRaw[1];
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
@ -540,6 +554,14 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
if (ev->obj == GyroSensorHandle()) {
updatedSensors |= SENSORUPDATES_gyro;
// shortcut - update GyroState right away
GyroSensorData s;
GyroStateData t;
GyroSensorGet(&s);
t.x = s.x + gyroDelta[0];
t.y = s.y + gyroDelta[1];
t.z = s.z + gyroDelta[2];
GyroStateSet(&t);
}
if (ev->obj == AccelSensorHandle()) {

93
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -101,7 +101,6 @@ static void updatePathVelocity();
static void updateEndpointVelocity();
static void updateFixedAttitude(float *attitude);
static void updateVtolDesiredAttitude(bool yaw_attitude);
static float bound(float val, float min, float max);
static bool vtolpathfollower_enabled;
static void accessoryUpdated(UAVObjEvent *ev);
@ -140,6 +139,7 @@ int32_t VtolPathFollowerInitialize()
AccessoryDesiredInitialize();
PoiLearnSettingsInitialize();
PoiLocationInitialize();
HomeLocationInitialize();
vtolpathfollower_enabled = true;
} else {
vtolpathfollower_enabled = false;
@ -159,6 +159,7 @@ static float eastPosIntegral = 0;
static float downPosIntegral = 0;
static float thrustOffset = 0;
static float gravity;
/**
* Module thread, should not return.
*/
@ -182,6 +183,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
HomeLocationg_eGet(&gravity);
SystemSettingsGet(&systemSettings);
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
@ -394,7 +396,7 @@ static void updatePathVelocity()
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
@ -403,7 +405,7 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
@ -427,14 +429,14 @@ static void updatePathVelocity()
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1);
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus
pathStatus.error = progress.error;
@ -472,15 +474,15 @@ void updateEndpointVelocity()
// Compute desired north command
northError = pathDesired.End.North - positionState.North;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
eastError = pathDesired.End.East - positionState.East;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral);
// Limit the maximum velocity
@ -494,11 +496,11 @@ void updateEndpointVelocity()
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End.Down - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
@ -516,9 +518,10 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
@ -595,18 +598,18 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
@ -615,22 +618,22 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1);
stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
@ -647,6 +650,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
}
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
@ -682,7 +686,7 @@ static void updateNedAccel()
accel_ned[i] += Rbe[j][i] * accel[j];
}
}
accel_ned[2] += 9.81f;
accel_ned[2] += gravity;
NedAccelData accelData;
NedAccelGet(&accelData);
@ -692,19 +696,6 @@ static void updateNedAccel()
NedAccelSet(&accelData);
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);

1
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -87,6 +87,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c
SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c

1
flight/targets/boards/coptercontrol/firmware/inc/openpilot.h
View File

@ -39,6 +39,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

2
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -160,7 +160,7 @@
#define PIOS_ACTUATOR_STACK_SIZE 820
#define PIOS_MANUAL_STACK_SIZE 635
#define PIOS_RECEIVER_STACK_SIZE 620
#define PIOS_STABILIZATION_STACK_SIZE 780
#define PIOS_STABILIZATION_STACK_SIZE 400
#ifdef DIAG_TASKS
#define PIOS_SYSTEM_STACK_SIZE 740

1
flight/targets/boards/oplinkmini/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

1
flight/targets/boards/osd/firmware/inc/openpilot.h
View File

@ -39,6 +39,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

2
flight/targets/boards/revolution/firmware/Makefile
View File

@ -33,7 +33,7 @@ MODULES += Sensors
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed
MODULES += AltitudeHold
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization
MODULES += VtolPathFollower
MODULES += ManualControl

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

@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationstatus
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
@ -105,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive

1
flight/targets/boards/revolution/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

2
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -144,7 +144,7 @@
#define PIOS_GPS_SETS_HOMELOCATION
/* Stabilization options */
/* #define PIOS_QUATERNION_STABILIZATION */
#define PIOS_QUATERNION_STABILIZATION
/* Performance counters */
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692

2
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -33,7 +33,7 @@ MODULES += Sensors
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed
MODULES += AltitudeHold
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization
MODULES += VtolPathFollower
MODULES += ManualControl

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

@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationstatus
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
@ -105,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive

1
flight/targets/boards/revoproto/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

3
flight/targets/boards/simposix/firmware/Makefile
View File

@ -42,7 +42,7 @@ MODULES += FirmwareIAP
MODULES += StateEstimation
#MODULES += Sensors/simulated/Sensors
MODULES += Airspeed
MODULES += AltitudeHold
#MODULES += AltitudeHold # now integrated in Stabilization
#MODULES += OveroSync
# Paths
@ -96,6 +96,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c
SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c

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

@ -86,6 +86,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationstatus
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
@ -107,7 +108,6 @@ UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance

1
flight/targets/boards/simposix/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

27
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
View File

@ -146,12 +146,27 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Thrust, "Thrust", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Thrust, "Thrust", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Thrust, "Thrust", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization4Settings", ui->fmsSsPos4Thrust, "Thrust", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization5Settings", ui->fmsSsPos5Thrust, "Thrust", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization6Settings", ui->fmsSsPos6Thrust, "Thrust", 1, true);
addWidgetBinding("FlightModeSettings", "Arming", ui->armControl);
addWidgetBinding("FlightModeSettings", "ArmedTimeout", ui->armTimeout, 0, 1000);
@ -1324,32 +1339,26 @@ void ConfigInputWidget::updatePositionSlider()
default:
case 6:
ui->fmsModePos6->setEnabled(true);
ui->cc_box_5->setEnabled(true);
ui->pidBankSs1_5->setEnabled(true);
// pass through
case 5:
ui->fmsModePos5->setEnabled(true);
ui->cc_box_4->setEnabled(true);
ui->pidBankSs1_4->setEnabled(true);
// pass through
case 4:
ui->fmsModePos4->setEnabled(true);
ui->cc_box_3->setEnabled(true);
ui->pidBankSs1_3->setEnabled(true);
// pass through
case 3:
ui->fmsModePos3->setEnabled(true);
ui->cc_box_2->setEnabled(true);
ui->pidBankSs1_2->setEnabled(true);
// pass through
case 2:
ui->fmsModePos2->setEnabled(true);
ui->cc_box_1->setEnabled(true);
ui->pidBankSs1_1->setEnabled(true);
// pass through
case 1:
ui->fmsModePos1->setEnabled(true);
ui->cc_box_0->setEnabled(true);
ui->pidBankSs1_0->setEnabled(true);
// pass through
case 0:
@ -1359,32 +1368,26 @@ void ConfigInputWidget::updatePositionSlider()
switch (manualSettingsDataPriv.FlightModeNumber) {
case 0:
ui->fmsModePos1->setEnabled(false);
ui->cc_box_0->setEnabled(false);
ui->pidBankSs1_0->setEnabled(false);
// pass through
case 1:
ui->fmsModePos2->setEnabled(false);
ui->cc_box_1->setEnabled(false);
ui->pidBankSs1_1->setEnabled(false);
// pass through
case 2:
ui->fmsModePos3->setEnabled(false);
ui->cc_box_2->setEnabled(false);
ui->pidBankSs1_2->setEnabled(false);
// pass through
case 3:
ui->fmsModePos4->setEnabled(false);
ui->cc_box_3->setEnabled(false);
ui->pidBankSs1_3->setEnabled(false);
// pass through
case 4:
ui->fmsModePos5->setEnabled(false);
ui->cc_box_4->setEnabled(false);
ui->pidBankSs1_4->setEnabled(false);
// pass through
case 5:
ui->fmsModePos6->setEnabled(false);
ui->cc_box_5->setEnabled(false);
ui->pidBankSs1_5->setEnabled(false);
// pass through
case 6:

579
ground/openpilotgcs/src/plugins/config/input.ui
View File

@ -116,8 +116,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>774</width>
<height>748</height>
<width>772</width>
<height>751</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout">
@ -546,8 +546,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>768</width>
<height>742</height>
<width>772</width>
<height>751</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_7" rowstretch="1,0,0,0">
@ -587,7 +587,7 @@
<property name="title">
<string>Stabilization Modes Configuration</string>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0,0">
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0,0,0,0">
<property name="leftMargin">
<number>9</number>
</property>
@ -626,6 +626,51 @@
</property>
</spacer>
</item>
<item row="1" column="10">
<spacer name="horizontalSpacer_15">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="11">
<widget class="QLabel" name="label_11">
<property name="minimumSize">
<size>
<width>120</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Thrust</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="9">
<widget class="QLabel" name="label_10">
<property name="minimumSize">
@ -684,7 +729,7 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="1" column="10">
<item row="1" column="12">
<spacer name="horizontalSpacer_11">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@ -750,6 +795,18 @@ margin:1px;</string>
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item>
<widget class="QLabel" name="label_14">
<property name="text">
@ -780,6 +837,36 @@ margin:1px;</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_114">
<property name="text">
<string>Stabilized 4</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_121">
<property name="text">
<string>Stabilized 5</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_122">
<property name="text">
<string>Stabilized 6</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@ -831,6 +918,27 @@ margin:1px;</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos4Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos5Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos6Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@ -882,6 +990,27 @@ margin:1px;</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos4Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos5Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos6Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@ -933,6 +1062,99 @@ margin:1px;</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos4Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos5Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos6Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="11">
<widget class="QFrame" name="frame_7">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item>
<widget class="QComboBox" name="fmsSsPos1Thrust">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos2Thrust">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos3Thrust">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos4Thrust">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos5Thrust">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos6Thrust">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@ -989,7 +1211,7 @@ margin:1px;</string>
<number>6</number>
</property>
<item row="0" column="6">
<widget class="QLabel" name="label_11">
<widget class="QLabel" name="label_111">
<property name="minimumSize">
<size>
<width>80</width>
@ -1078,7 +1300,7 @@ channel value for each flight mode.</string>
<item row="2" column="10">
<widget class="QLabel" name="label_15">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<sizepolicy hsizetype="Ignored" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@ -1090,7 +1312,7 @@ channel value for each flight mode.</string>
</font>
</property>
<property name="text">
<string>Avoid &quot;Manual&quot; for multirotors!</string>
<string>Avoid &quot;Manual&quot; for multirotors! Never select &quot;Altitude&quot;, &quot;VelocityControl&quot; or &quot;CruiseControl&quot; on a fixed wing!</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@ -1116,22 +1338,6 @@ channel value for each flight mode.</string>
</property>
</spacer>
</item>
<item row="1" column="5">
<spacer name="horizontalSpacer_15">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="7">
<spacer name="horizontalSpacer_16">
<property name="orientation">
@ -1164,35 +1370,6 @@ channel value for each flight mode.</string>
</property>
</spacer>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_12">
<property name="minimumSize">
<size>
<width>105</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Cruise Control</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="8" rowspan="3">
<widget class="Line" name="line">
<property name="orientation">
@ -1229,294 +1406,6 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="1" column="4" rowspan="2">
<widget class="QFrame" name="frame_4">
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<property name="spacing">
<number>6</number>
</property>
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_0">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:0</string>
<string>haslimits:no</string>
<string>scale:1</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_1">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #2.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:1</string>
<string>haslimits:no</string>
<string>scale:1</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_2">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #3.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:2</string>
<string>haslimits:no</string>
<string>scale:1</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_3">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #4.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:3</string>
<string>haslimits:no</string>
<string>scale:1</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_4">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #5.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:4</string>
<string>haslimits:no</string>
<string>scale:1</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_5">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #6.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:5</string>
<string>haslimits:no</string>
<string>scale:1</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="0" rowspan="2">
<widget class="QFrame" name="frame">
<property name="minimumSize">
@ -1867,7 +1756,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
</widget>
</item>
<item row="1" column="6" rowspan="2">
<widget class="QFrame" name="frame_7">
<widget class="QFrame" name="frame_8">
<property name="minimumSize">
<size>
<width>75</width>
@ -2164,8 +2053,8 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
<rect>
<x>0</x>
<y>0</y>
<width>768</width>
<height>742</height>
<width>407</width>
<height>138</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">

355
ground/openpilotgcs/src/plugins/config/stabilization.ui
View File

@ -8444,7 +8444,7 @@ border-radius: 5;</string>
<string notr="true"/>
</property>
<property name="text">
<string>Max rate attitude (deg/s)</string>
<string>Max rate limit (all modes) (deg/s)</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -13377,8 +13377,6 @@ border-radius: 5;</string>
</property>
<property name="decimals">
<number>5</number>
</property>
<property name="singleStep">
<double>0.000100000000000</double>
@ -15943,7 +15941,6 @@ border-radius: 5;</string>
<string>element:Kp</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:5,20</string>
</stringlist>
</property>
@ -17355,7 +17352,6 @@ border-radius: 5;</string>
<red>26</red>
<green>26</green>
<blue>26</blue>
</color>
</brush>
</colorrole>
@ -19421,7 +19417,6 @@ border-radius: 5;</string>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
@ -19946,7 +19941,6 @@ border-radius: 5;</string>
<red>39</red>
<green>39</green>
<blue>39</blue>
</color>
</brush>
</colorrole>
@ -21981,7 +21975,6 @@ border-radius: 5;</string>
</size>
</property>
</spacer>
</item>
<item row="1" column="3">
<widget class="QDoubleSpinBox" name="AccelKp">
@ -24186,6 +24179,12 @@ border-radius: 5;</string>
</item>
<item row="1" column="0" colspan="2">
<widget class="QFrame" name="gridFrame">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<layout class="QGridLayout" name="gridLayout_25">
<property name="leftMargin">
<number>9</number>
@ -24200,30 +24199,23 @@ border-radius: 5;</string>
<number>9</number>
</property>
<item row="1" column="4">
<widget class="QDoubleSpinBox" name="doubleSpinBox_6">
<widget class="QComboBox" name="comboBox">
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;-1, 0, or 1. Cruise Control multiplies the throttle/collective stick by this value if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrust) when inverted. 1 says to use the unboosted stick value. -1 (DON'T USE, INCOMPLETE, UNTESTED, for use by CP helis using idle up) says to reverse the collective stick when inverted.
&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="accelerated">
<bool>true</bool>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-1.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;CP helis can set this to Reversed to automatically reverse the direction of thrust when inverted. Fixed pitch copters, including multicopters, should leave this set at Unreversed. Unreversed direction with Boosted power may be dangerous because it adds power and the thrust direction moves the aircraft towards the ground.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlInvertedPowerSwitch</string>
<string>fieldname:CruiseControlInvertedThrustReversing</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
@ -24231,6 +24223,39 @@ border-radius: 5;</string>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_10">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>145</width>
<height>16</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>InvrtdThrustRev</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="5">
<spacer name="horizontalSpacer_59">
<property name="orientation">
@ -24247,7 +24272,7 @@ border-radius: 5;</string>
<item row="1" column="2">
<widget class="QDoubleSpinBox" name="doubleSpinBox_3">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This is the bank angle that CruiseControl goes into inverted / power disabled mode. The power for inverted mode is controlled by CruiseControlInvertedPowerSwitch&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The bank angle where CruiseControl goes into inverted power mode. InvertedThrustReverse and InvertedPower control the direction and amount of power when in inverted mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -24259,7 +24284,7 @@ border-radius: 5;</string>
<number>0</number>
</property>
<property name="maximum">
<double>180.000000000000000</double>
<double>255.000000000000000</double>
</property>
<property name="value">
<double>105.000000000000000</double>
@ -24278,7 +24303,7 @@ border-radius: 5;</string>
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Really just a safety limit. 4.0 means it will not use more than 4 times the power the throttle/collective stick is requesting.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Really just a safety limit. 3.0 means it will not use more than 3 times the power the throttle/collective stick is requesting.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -24290,13 +24315,13 @@ border-radius: 5;</string>
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
<double>1.000000000000000</double>
</property>
<property name="maximum">
<double>50.000000000000000</double>
</property>
<property name="singleStep">
<double>0.250000000000000</double>
<double>0.100000000000000</double>
</property>
<property name="value">
<double>3.000000000000000</double>
@ -24310,11 +24335,10 @@ border-radius: 5;</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_6">
<widget class="QLabel" name="label_11">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
@ -24323,7 +24347,7 @@ border-radius: 5;</string>
</property>
<property name="minimumSize">
<size>
<width>140</width>
<width>155</width>
<height>16</height>
</size>
</property>
@ -24339,75 +24363,13 @@ color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>PowerTrim</string>
<string>PowerDelayComp</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="4">
<widget class="QDoubleSpinBox" name="doubleSpinBox_7">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This needs to be 0 for all copters except CP helis that are using idle up.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="accelerated">
<bool>true</bool>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlNeutralThrust</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_2">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;If you find that banging the stick around a lot makes the copter climb a bit, adjust this number down a little.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="accelerated">
<bool>true</bool>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>80.000000000000000</double>
</property>
<property name="maximum">
<double>120.000000000000000</double>
</property>
<property name="singleStep">
<double>0.250000000000000</double>
</property>
<property name="value">
<double>100.000000000000000</double>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlPowerTrim</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_4">
<property name="sizePolicy">
@ -24451,7 +24413,7 @@ border-radius: 5;</string>
</property>
<property name="minimumSize">
<size>
<width>140</width>
<width>90</width>
<height>16</height>
</size>
</property>
@ -24484,7 +24446,7 @@ border-radius: 5;</string>
</property>
<property name="minimumSize">
<size>
<width>140</width>
<width>92</width>
<height>16</height>
</size>
</property>
@ -24510,7 +24472,7 @@ border-radius: 5;</string>
<item row="1" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBox_4">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Throttle/Collective stick below this disables Cruise Control. Also, by default Cruise Control forces the use of this value for thrust when the copter is inverted. For safety, never set this so low that the trimmed throttle/collective stick cannot get below it.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Throttle/Collective stick below this amount disables Cruise Control. Also, by default Cruise Control forces the use of this value for thrust when InvertedPower setting is Zero and the copter is inverted. CP helis probably want this set to -100%. For safety with fixed pitch copters (including multicopters), never set this so low that the trimmed throttle stick cannot get below it or your motor(s) will still run with the throttle stick all the way down. Fixed pitch throttle sticks go from -100 to 0 in the first tiny bit of throttle stick (and then up to 100 using the rest of the throttle range), so for example, a lot of &amp;quot;high throttle trim&amp;quot; will keep the stick from ever commanding less than 5% so it won't be possible to stop the motors with the throttle stick. Banking the copter in your hand in this state will make the motors speed up.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -24521,6 +24483,12 @@ border-radius: 5;</string>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-100.000000000000000</double>
</property>
<property name="maximum">
<double>49.000000000000000</double>
</property>
<property name="value">
<double>5.000000000000000</double>
</property>
@ -24535,43 +24503,10 @@ border-radius: 5;</string>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QLabel" name="label_11">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>140</width>
<height>16</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>NeutralThrust</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBox_5">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Multi-copters should probably use 90% to 95% to leave some headroom for stabilization. CP helis can set this to 100%.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Multi-copters should probably use 80% to 90% to leave some headroom for stabilization. CP helis can set this to 100%.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -24582,6 +24517,9 @@ border-radius: 5;</string>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>51.000000000000000</double>
</property>
<property name="value">
<double>90.000000000000000</double>
</property>
@ -24606,7 +24544,7 @@ border-radius: 5;</string>
</property>
<property name="minimumSize">
<size>
<width>140</width>
<width>97</width>
<height>16</height>
</size>
</property>
@ -24629,8 +24567,24 @@ border-radius: 5;</string>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_10">
<item row="1" column="0">
<spacer name="horizontalSpacer_60">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>90</width>
<height>11</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_6">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
@ -24639,7 +24593,68 @@ border-radius: 5;</string>
</property>
<property name="minimumSize">
<size>
<width>140</width>
<width>97</width>
<height>16</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>PowerTrim</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_7">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Moves the inverted mode power switching this fraction of a second into the future to compensate for slow responding power systems. Used to more accurately time the changing of power when entering/exiting inverted mode. Set this to equal the fraction of a second it takes for your vehicle to go from full negative thrust (10% positive thrust for multis, full negative thrust (-90%) for CP helis) to full positive thrust (+90%). Increase this if for example continuous left flips &amp;quot;walk off&amp;quot; to the left because power is changed too late.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="accelerated">
<bool>true</bool>
</property>
<property name="decimals">
<number>5</number>
</property>
<property name="maximum">
<double>2.000000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.250000000000000</double>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlPowerDelayComp</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QLabel" name="label_12">
<property name="minimumSize">
<size>
<width>120</width>
<height>16</height>
</size>
</property>
@ -24662,21 +24677,58 @@ border-radius: 5;</string>
</property>
</widget>
</item>
<item row="1" column="0">
<spacer name="horizontalSpacer_60">
<property name="orientation">
<enum>Qt::Horizontal</enum>
<item row="3" column="2">
<widget class="QDoubleSpinBox" name="doubleSpinBox_2">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;If you find that quickly moving the stick around a lot makes the copter climb a bit, adjust this number down a little. It will be a compromise between climbing a little with lots of stick motion and descending a little with minimal stick motion.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>90</width>
<height>11</height>
</size>
<property name="accelerated">
<bool>true</bool>
</property>
</spacer>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>50.000000000000000</double>
</property>
<property name="maximum">
<double>200.000000000000000</double>
</property>
<property name="singleStep">
<double>0.250000000000000</double>
</property>
<property name="value">
<double>100.000000000000000</double>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlPowerTrim</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item row="3" column="4">
<widget class="QComboBox" name="comboBox_2">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The amount of power used when in inverted mode. Zero (min throttle stick for fixed pitch copters includding multicopters, neutral collective for CP), Normal (uses stick value), or Boosted (boosted according to bank angle). Beginning multicopter pilots should leave this set to Zero to automatically reduce throttle during flips. Boosted power with Unreversed direction may be dangerous because it adds power and the thrust direction moves the aircraft towards the ground.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlInvertedPowerOutput</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
@ -26166,7 +26218,6 @@ border-radius: 5;</string>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="styleSheet">
<string notr="true"/>

21
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -318,19 +318,34 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration()
data.Stabilization1Settings[0] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
data.Stabilization1Settings[1] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
data.Stabilization1Settings[2] = FlightModeSettings::STABILIZATION1SETTINGS_AXISLOCK;
data.Stabilization1Settings[3] = FlightModeSettings::STABILIZATION1SETTINGS_MANUAL;
data.Stabilization2Settings[0] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
data.Stabilization2Settings[1] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
data.Stabilization2Settings[2] = FlightModeSettings::STABILIZATION2SETTINGS_RATE;
data.Stabilization2Settings[3] = FlightModeSettings::STABILIZATION2SETTINGS_MANUAL;
data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
data.Stabilization3Settings[3] = FlightModeSettings::STABILIZATION3SETTINGS_MANUAL;
data.Stabilization4Settings[0] = FlightModeSettings::STABILIZATION4SETTINGS_ATTITUDE;
data.Stabilization4Settings[1] = FlightModeSettings::STABILIZATION4SETTINGS_ATTITUDE;
data.Stabilization4Settings[2] = FlightModeSettings::STABILIZATION4SETTINGS_AXISLOCK;
data.Stabilization4Settings[3] = FlightModeSettings::STABILIZATION4SETTINGS_CRUISECONTROL;
data.Stabilization5Settings[0] = FlightModeSettings::STABILIZATION5SETTINGS_ATTITUDE;
data.Stabilization5Settings[1] = FlightModeSettings::STABILIZATION5SETTINGS_ATTITUDE;
data.Stabilization5Settings[2] = FlightModeSettings::STABILIZATION5SETTINGS_RATE;
data.Stabilization5Settings[3] = FlightModeSettings::STABILIZATION5SETTINGS_CRUISECONTROL;
data.Stabilization6Settings[0] = FlightModeSettings::STABILIZATION6SETTINGS_RATE;
data.Stabilization6Settings[1] = FlightModeSettings::STABILIZATION6SETTINGS_RATE;
data.Stabilization6Settings[2] = FlightModeSettings::STABILIZATION6SETTINGS_RATE;
data.Stabilization6Settings[3] = FlightModeSettings::STABILIZATION6SETTINGS_CRUISECONTROL;
data2.FlightModeNumber = 3;
data.FlightModePosition[0] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1;
data.FlightModePosition[1] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2;
data.FlightModePosition[2] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED3;
data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD;
data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_POSITIONHOLD;
data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED4;
data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED5;
data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED6;
modeSettings->setData(data);
addModifiedObject(modeSettings, tr("Writing flight mode settings 1/2"));
controlSettings->setData(data2);

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -35,7 +35,6 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \
$$UAVOBJECT_SYNTHETICS/attitudestate.h \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
@ -62,6 +61,7 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/overosyncstats.h \
$$UAVOBJECT_SYNTHETICS/overosyncsettings.h \
$$UAVOBJECT_SYNTHETICS/systemsettings.h \
$$UAVOBJECT_SYNTHETICS/stabilizationstatus.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettings.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.h \
@ -136,7 +136,6 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
$$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
$$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \
@ -163,6 +162,7 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/overosyncstats.cpp \
$$UAVOBJECT_SYNTHETICS/overosyncsettings.cpp \
$$UAVOBJECT_SYNTHETICS/systemsettings.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationstatus.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettings.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.cpp \

1
make/apps-defs.mk
View File

@ -106,6 +106,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c
SRC += $(FLIGHTLIB)/printf-stdarg.c
## Modules

14
shared/uavobjectdefinition/altitudeholddesired.xml
View File

@ -1,14 +0,0 @@
<xml>
<object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
<description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
<field name="SetPoint" units="" type="float" elements="1"/>
<field name="ControlMode" units="" type="enum" elements="1" options="Altitude,Velocity,Thrust" />
<field name="Roll" units="deg" type="float" elements="1"/>
<field name="Pitch" units="deg" type="float" elements="1"/>
<field name="Yaw" units="deg/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

6
shared/uavobjectdefinition/callbackinfo.xml
View File

@ -6,6 +6,8 @@
<elementname>EventDispatcher</elementname>
<elementname>StateEstimation</elementname>
<elementname>AltitudeHold</elementname>
<elementname>Stabilization0</elementname>
<elementname>Stabilization1</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
@ -16,6 +18,8 @@
<elementname>EventDispatcher</elementname>
<elementname>StateEstimation</elementname>
<elementname>AltitudeHold</elementname>
<elementname>Stabilization0</elementname>
<elementname>Stabilization1</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
@ -30,6 +34,8 @@
<elementname>EventDispatcher</elementname>
<elementname>StateEstimation</elementname>
<elementname>AltitudeHold</elementname>
<elementname>Stabilization0</elementname>
<elementname>Stabilization1</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>

115
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -6,20 +6,71 @@
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
<field name="Stabilization1Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw"
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Attitude,Attitude,AxisLock"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
elementnames="Roll,Pitch,Yaw,Thrust"
options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
defaultvalue="Attitude,Attitude,AxisLock,Manual"
limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
/>
<field name="Stabilization2Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw"
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Attitude,Attitude,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
elementnames="Roll,Pitch,Yaw,Thrust"
options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
defaultvalue="Attitude,Attitude,Rate,Manual"
limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
/>
<field name="Stabilization3Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw"
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Rate,Rate,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
elementnames="Roll,Pitch,Yaw,Thrust"
options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
defaultvalue="Rate,Rate,Rate,Manual"
limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
/>
<field name="Stabilization4Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
defaultvalue="Attitude,Attitude,AxisLock,CruiseControl"
limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
/>
<field name="Stabilization5Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
defaultvalue="Attitude,Attitude,Rate,CruiseControl"
limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
/>
<field name="Stabilization6Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"
defaultvalue="Rate,Rate,Rate,CruiseControl"
limits="%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity:CruiseControl; \
%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude,\
%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity,\
%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Rattitude:RelayRate:RelayAttitude:AltitudeHold:VerticalVelocity;"
/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<!-- Currently only some modes are enabled for UI using limits attribute per board. Update when more modes will be operational -->
@ -27,32 +78,32 @@
units=""
type="enum"
elements="6"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
defaultvalue="Stabilized1,Stabilized2,Stabilized3,AltitudeHold,AltitudeVario,Manual"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"
defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6"
limits="\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
%0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>

2
shared/uavobjectdefinition/flightstatus.xml
View File

@ -4,7 +4,7 @@
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
<field name="ControlChain" units="bool" type="enum" options="false,true">
<elementnames>

1
shared/uavobjectdefinition/ratedesired.xml
View File

@ -4,6 +4,7 @@
<field name="Roll" units="deg/s" type="float" elements="1"/>
<field name="Pitch" units="deg/s" type="float" elements="1"/>
<field name="Yaw" units="deg/s" type="float" elements="1"/>
<field name="Thrust" units="%" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

5
shared/uavobjectdefinition/stabilizationdesired.xml
View File

@ -5,9 +5,8 @@
<field name="Pitch" units="degrees" type="float" elements="1"/>
<field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="Thrust" units="%" type="float" elements="1"/>
<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings -->
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"/>
<field name="ThrustStabilizationMode" units="" type="enum" elements="1" options="None"/>
<!-- These values should match those in FlightModeSettings.Stabilization{1,2,3}Settings -->
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude,AltitudeHold,VerticalVelocity,CruiseControl"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

11
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -22,6 +22,7 @@
<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
<field name="AxisLockKp" units="" type="float" elements="1" defaultvalue="2.5"/>
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="30"/>
<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>
@ -30,15 +31,17 @@
<field name="RattitudeModeTransition" units="%" type="uint8" elements="1" defaultvalue="80"/>
<field name="CruiseControlMinThrust" units="%" type="uint8" elements="1" defaultvalue="5"/>
<field name="CruiseControlMaxThrust" units="%" type="uint8" elements="1" defaultvalue="90"/>
<field name="CruiseControlMinThrust" units="%" type="int8" elements="1" defaultvalue="5"/>
<field name="CruiseControlMaxThrust" units="%" type="uint8" elements="1" defaultvalue="90"/>
<field name="CruiseControlMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="105"/>
<field name="CruiseControlMaxPowerFactor" units="x" type="float" elements="1" defaultvalue="3.0"/>
<field name="CruiseControlPowerTrim" units="%" type="float" elements="1" defaultvalue="100.0"/>
<field name="CruiseControlInvertedPowerSwitch" units="" type="int8" elements="1" defaultvalue="0"/>
<field name="CruiseControlNeutralThrust" units="%" type="uint8" elements="1" defaultvalue="0"/>
<field name="CruiseControlPowerDelayComp" units="sec" type="float" elements="1" defaultvalue="0.25"/>
<field name="CruiseControlFlightModeSwitchPosEnable" units="" type="enum" elements="6" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="CruiseControlInvertedThrustReversing" units="" type="enum" elements="1" options="Unreversed,Reversed" defaultvalue="Unchanged"/>
<field name="CruiseControlInvertedPowerOutput" units="" type="enum" elements="1" options="Zero,Normal,Boosted" defaultvalue="Zero"/>
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>

2
shared/uavobjectdefinition/stabilizationsettingsbank1.xml
View File

@ -6,7 +6,7 @@
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

2
shared/uavobjectdefinition/stabilizationsettingsbank2.xml
View File

@ -6,7 +6,7 @@
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

2
shared/uavobjectdefinition/stabilizationsettingsbank3.xml
View File

@ -6,7 +6,7 @@
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

28
shared/uavobjectdefinition/stabilizationstatus.xml Normal file
View File

@ -0,0 +1,28 @@
<xml>
<object name="StabilizationStatus" singleinstance="true" settings="false" category="Control">
<description>Contains status information to control submodules for stabilization.</description>
<field name="OuterLoop" units="" type="enum" options="Direct,Attitude,Rattitude,Weakleveling,Altitude,VerticalVelocity">
<elementnames>
<elementname>Roll</elementname>
<elementname>Pitch</elementname>
<elementname>Yaw</elementname>
<elementname>Thrust</elementname>
</elementnames>
</field>
<field name="InnerLoop" units="" type="enum" options="Direct,VirtualFlyBar,RelayTuning,AxisLock,Rate,CruiseControl">
<elementnames>
<elementname>Roll</elementname>
<elementname>Pitch</elementname>
<elementname>Yaw</elementname>
<elementname>Thrust</elementname>
</elementnames>
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="5000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>