rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into next

Browse Source
This commit is contained in:
Philippe Renon 2014-01-16 19:02:39 +01:00
commit 9229da9c89
24 changed files with 260 additions and 419 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/libraries/sanitycheck.c
View File

@ -114,9 +114,9 @@ int32_t configuration_check()
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) { if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR; severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
} }
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break; break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) { if (coptercontrol) {

21
flight/modules/Altitude/revolution/altitude.c
View File

@ -40,22 +40,24 @@
#include "altitude.h" #include "altitude.h"
#include "barosensor.h" // object that will be updated by the module #include "barosensor.h" // object that will be updated by the module
#include "revosettings.h"
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module #include "sonaraltitude.h" // object that will be updated by the module
#endif #endif
#include "taskinfo.h" #include "taskinfo.h"
// Private constants // Private constants
#define STACK_SIZE_BYTES 500 #define STACK_SIZE_BYTES 550
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// Private types // Private types
// Private variables // Private variables
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
// Private functions // Private functions
static void altitudeTask(void *parameters); static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
/** /**
* Initialise the module, called on startup * Initialise the module, called on startup
@ -77,6 +79,9 @@ int32_t AltitudeStart()
int32_t AltitudeInitialize() int32_t AltitudeInitialize()
{ {
BaroSensorInitialize(); BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize(); SonarAltitudeInitialize();
#endif #endif
@ -103,6 +108,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
// Undef for normal operation // Undef for normal operation
// #define PIOS_MS5611_SLOW_TEMP_RATE 20 // #define PIOS_MS5611_SLOW_TEMP_RATE 20
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
#ifdef PIOS_MS5611_SLOW_TEMP_RATE #ifdef PIOS_MS5611_SLOW_TEMP_RATE
uint8_t temp_press_interleave_count = 1; uint8_t temp_press_interleave_count = 1;
#endif #endif
@ -154,12 +161,12 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
vTaskDelay(PIOS_MS5611_GetDelay()); vTaskDelay(PIOS_MS5611_GetDelay());
PIOS_MS5611_ReadADC(); PIOS_MS5611_ReadADC();
temp = PIOS_MS5611_GetTemperature(); temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure(); press = PIOS_MS5611_GetPressure();
float temp2 = temp * temp;
press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d;
float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) { if (!isnan(altitude)) {
data.Altitude = altitude; data.Altitude = altitude;
@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
} }
} }
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
}
/** /**
* @} * @}
* @} * @}

417
flight/modules/AltitudeHold/altitudehold.c
View File

@ -46,33 +46,35 @@
#include <openpilot.h> #include <openpilot.h>
#include <math.h> #include <math.h>
#include <pid.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <altholdsmoothed.h>
#include <attitudestate.h> #include <attitudestate.h>
#include <altitudeholdsettings.h> #include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module #include <altitudeholddesired.h> // object that will be updated by the module
#include <barosensor.h> #include <altitudeholdstatus.h>
#include <positionstate.h>
#include <flightstatus.h> #include <flightstatus.h>
#include <stabilizationdesired.h> #include <stabilizationdesired.h>
#include <accelstate.h> #include <accelstate.h>
#include <taskinfo.h>
#include <pios_constants.h> #include <pios_constants.h>
#include <velocitystate.h>
#include <positionstate.h>
// Private constants // Private constants
#define MAX_QUEUE_SIZE 2
#define STACK_SIZE_BYTES 1024 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define ACCEL_DOWNSAMPLE 4
#define TIMEOUT_TRESHOLD 200000 #define STACK_SIZE_BYTES 1024
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
// Private types // Private types
// Private variables // Private variables
static xTaskHandle altitudeHoldTaskHandle; static DelayedCallbackInfo *altitudeHoldCBInfo;
static xQueueHandle queue;
static AltitudeHoldSettingsData altitudeHoldSettings; static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
// Private functions // Private functions
static void altitudeHoldTask(void *parameters); static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev);
/** /**
@ -82,8 +84,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
int32_t AltitudeHoldStart() int32_t AltitudeHoldStart()
{ {
// Start main task // Start main task
xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); SettingsUpdatedCb(NULL);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); DelayedCallbackDispatch(altitudeHoldCBInfo);
return 0; return 0;
} }
@ -96,319 +98,122 @@ int32_t AltitudeHoldInitialize()
{ {
AltitudeHoldSettingsInitialize(); AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize(); AltitudeHoldDesiredInitialize();
AltHoldSmoothedInitialize(); AltitudeHoldStatusInitialize();
// Create object queue // Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
return 0; return 0;
} }
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
float tau;
float throttleIntegral;
float velocity;
float decay;
float velocity_decay;
bool running = false;
float error;
float switchThrottle;
float smoothed_altitude;
float starting_altitude;
/** /**
* Module thread, should not return. * Module thread, should not return.
*/ */
static void altitudeHoldTask(__attribute__((unused)) void *parameters) static void altitudeHoldTask(void)
{ {
AltitudeHoldDesiredData altitudeHoldDesired; static float startThrottle = 0.5f;
StabilizationDesiredData stabilizationDesired;
portTickType thisTime, lastUpdateTime; // make sure we run only when we are supposed to run
UAVObjEvent ev; FlightStatusData flightStatus;
// Force update of the settings FlightStatusGet(&flightStatus);
SettingsUpdatedCb(&ev); switch (flightStatus.FlightMode) {
// Failsafe handling case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
uint32_t lastAltitudeHoldDesiredUpdate = 0; case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
bool enterFailSafe = false; break;
// Listen for updates. default:
AltitudeHoldDesiredConnectQueue(queue); pid_zero(&pid0);
BaroSensorConnectQueue(queue); pid_zero(&pid1);
FlightStatusConnectQueue(queue); StabilizationDesiredThrottleGet(&startThrottle);
AccelStateConnectQueue(queue); DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
bool altitudeHoldFlightMode = false; return;
BaroSensorAltitudeGet(&smoothed_altitude);
running = false;
enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
uint8_t flightMode; break;
FlightStatusFlightModeGet(&flightMode);
// initialize enable flag
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
// Main task loop
bool baro_updated = false;
while (1) {
enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
if (!running) {
throttleIntegral = 0;
}
// Todo: Add alarm if it should be running
continue;
} else if (ev.obj == BaroSensorHandle()) {
baro_updated = true;
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
} else if (ev.obj == FlightStatusHandle()) {
FlightStatusFlightModeGet(&flightMode);
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
if (altitudeHoldFlightMode && !running) {
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&throttleIntegral);
switchThrottle = throttleIntegral;
error = 0;
velocity = 0;
running = true;
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
starting_altitude = altHold.Altitude;
} else if (!altitudeHoldFlightMode) {
running = false;
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
}
} else if (ev.obj == AccelStateHandle()) {
static uint32_t timeval;
static float z[4] = { 0, 0, 0, 0 };
float z_new[4];
float P[4][4], K[4][2], x[2];
float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f };
static float V[4][4] = {
{ 10.0f, 0.0f, 0.0f, 0.0f },
{ 0.0f, 100.0f, 0.0f, 0.0f },
{ 0.0f, 0.0f, 100.0f, 0.0f },
{ 0.0f, 0.0f, 0.0f, 1000.0f }
};
static uint32_t accel_downsample_count = 0;
static float accels_accum[3] = { 0.0f, 0.0f, 0.0f };
float dT;
static float S[2] = { 1.0f, 10.0f };
/* Somehow this always assigns to zero. Compiler bug? Race condition? */
S[0] = altitudeHoldSettings.PressureNoise;
S[1] = altitudeHoldSettings.AccelNoise;
G[2] = altitudeHoldSettings.AccelDrift;
AccelStateData accelState;
AccelStateGet(&accelState);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
BaroSensorData baro;
BaroSensorGet(&baro);
/* Downsample accels to stop this calculation consuming too much CPU */
accels_accum[0] += accelState.x;
accels_accum[1] += accelState.y;
accels_accum[2] += accelState.z;
accel_downsample_count++;
if (accel_downsample_count < ACCEL_DOWNSAMPLE) {
continue;
}
accel_downsample_count = 0;
accelState.x = accels_accum[0] / ACCEL_DOWNSAMPLE;
accelState.y = accels_accum[1] / ACCEL_DOWNSAMPLE;
accelState.z = accels_accum[2] / ACCEL_DOWNSAMPLE;
accels_accum[0] = accels_accum[1] = accels_accum[2] = 0;
thisTime = xTaskGetTickCount();
if (init == WAITIING_INIT) {
z[0] = baro.Altitude;
z[1] = 0;
z[2] = accelState.z;
z[3] = 0;
init = INITED;
} else if (init == WAITING_BARO) {
continue;
}
x[0] = baro.Altitude;
// rotate avg accels into earth frame and store it
if (1) {
float q[4], Rbe[3][3];
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f);
} else {
x[1] = -accelState.z + 9.81f;
}
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
timeval = PIOS_DELAY_GetRaw();
P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0];
P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1];
P[0][2] = V[0][2] + dT * V[1][2];
P[0][3] = V[0][3] + dT * V[1][3];
P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0];
P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1];
P[1][2] = V[1][2] + dT * V[2][2];
P[1][3] = V[1][3] + dT * V[2][3];
P[2][0] = V[2][0] + dT * V[2][1];
P[2][1] = V[2][1] + dT * V[2][2];
P[2][2] = V[2][2] + G[2];
P[2][3] = V[2][3];
P[3][0] = V[3][0] + dT * V[3][1];
P[3][1] = V[3][1] + dT * V[3][2];
P[3][2] = V[3][2];
P[3][3] = V[3][3] + G[3];
if (baro_updated) {
K[0][0] = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f;
K[0][1] = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[1][0] = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[1][1] = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[2][0] = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[2][1] = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[3][0] = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[3][1] = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0];
z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1];
z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2];
z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3];
memcpy(z, z_new, sizeof(z_new));
V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f);
V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f);
V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f);
V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f);
V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0];
V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2];
V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2];
V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3];
V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f);
V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f);
V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f);
V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f);
V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f);
V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f);
V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f);
V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f);
baro_updated = false;
} else {
K[0][0] = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[1][0] = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[2][0] = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[3][0] = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0];
z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1];
z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2];
z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3];
memcpy(z, z_new, sizeof(z_new));
V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0];
V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2];
V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2];
V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3];
V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0];
V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2];
V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2];
V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3];
V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f);
V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f);
V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f);
V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f);
V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f);
V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f);
V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f);
V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f);
}
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
altHold.Altitude = z[0];
altHold.Velocity = z[1];
altHold.Accel = z[2];
AltHoldSmoothedSet(&altHold);
AltHoldSmoothedGet(&altHold);
// Verify that we are in altitude hold mode
uint8_t armed;
FlightStatusArmedGet(&armed);
if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) {
running = false;
}
if (!running) {
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
continue;
}
// Compute the altitude error
error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
// Compute integral off altitude error
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
// Only update stabilizationDesired less frequently
if ((thisTime - lastUpdateTime) < 20) {
continue;
}
lastUpdateTime = thisTime;
// Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired);
if (!enterFailSafe) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1;
} else if (stabilizationDesired.Throttle < 0) {
throttleIntegral -= stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
}
} else {
// shutdown motors
stabilizationDesired.Throttle = -1;
}
stabilizationDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
StabilizationDesiredSet(&stabilizationDesired);
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
// reset the failsafe timer
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
AltitudeHoldDesiredGet(&altitudeHoldDesired);
}
} }
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:
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
break;
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
float throttle;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
throttle = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop
throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
// compensate throttle for current attitude
// Rbe[2][2] in the rotation matrix is the rotated down component of a
// 0,0,1 vector
// it is used to scale the throttle, but only up to 4 times the regular
// throttle
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float Rbe[3][3];
Quaternion2R(&attitudeState.q1, Rbe);
if (fabsf(Rbe[2][2]) > 0.25f) {
throttle /= Rbe[2][2];
} else {
throttle /= 0.25f;
}
if (throttle >= 1.0f) {
throttle = 1.0f;
}
if (throttle <= 0.0f) {
throttle = 0.0f;
}
break;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Throttle = throttle;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
} }
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
AltitudeHoldSettingsGet(&altitudeHoldSettings); AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
pid_zero(&pid0);
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
} }

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

@ -845,65 +845,60 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
*/ */
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{ {
const float DEADBAND = 0.25f; const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// Stop updating AltitudeHoldDesired triggering a failsafe condition.
if (cmd->Throttle < 0) {
return;
}
// this is the max speed in m/s at the extents of throttle // this is the max speed in m/s at the extents of throttle
uint8_t throttleRate; float throttleRate;
uint8_t throttleExp; uint8_t throttleExp;
static uint8_t flightMode; static uint8_t flightMode;
static portTickType lastSysTimeAH; static bool newaltitude = true;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
FlightStatusFlightModeGet(&flightMode); FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData; AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData); AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { AltitudeHoldSettingsThrottleExpGet(&throttleExp);
throttleExp = 128; AltitudeHoldSettingsThrottleRateGet(&throttleRate);
throttleRate = 0;
} else {
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
}
StabilizationBankData stabSettings; StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings); StabilizationBankGet(&stabSettings);
thisSysTime = xTaskGetTickCount(); PositionStateData posState;
dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); PositionStateGet(&posState);
lastSysTimeAH = thisSysTime;
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
float currentDown;
PositionStateDownGet(&currentDown);
if (changed) { if (changed) {
// After not being in this mode for a while init at current height newaltitude = true;
altitudeHoldDesiredData.Altitude = 0; }
zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { uint8_t cutOff;
AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired
altitudeHoldDesiredData.SetPoint = cmd->Throttle;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // 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 + x*(256−k)) / 256 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) { altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; newaltitude = true;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) {
// Require the stick to enter the dead band before they can move height altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
// Vario is not "engaged" when throttleRate == 0 altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
zeroed = true; newaltitude = true;
} else if (newaltitude == true) {
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false;
} }
AltitudeHoldDesiredSet(&altitudeHoldDesiredData); AltitudeHoldDesiredSet(&altitudeHoldDesiredData);

7
flight/modules/StateEstimation/filterbaro.c
View File

@ -37,6 +37,7 @@
// Private constants // Private constants
#define STACK_REQUIRED 64 #define STACK_REQUIRED 64
#define INIT_CYCLES 100
// Private types // Private types
struct data { struct data {
@ -67,7 +68,7 @@ static int32_t init(stateFilter *self)
struct data *this = (struct data *)self->localdata; struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f; this->baroOffset = 0.0f;
this->first_run = 100; this->first_run = INIT_CYCLES;
RevoSettingsInitialize(); RevoSettingsInitialize();
RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
@ -82,8 +83,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->first_run) { if (this->first_run) {
// Initialize to current altitude reading at initial location // Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0]; this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0];
this->baroAlt = this->baroOffset; this->baroAlt = 0;
this->first_run--; this->first_run--;
UNSET_MASK(state->updated, SENSORUPDATES_baro); UNSET_MASK(state->updated, SENSORUPDATES_baro);
} }

34
flight/pios/common/pios_ms5611.c
View File

@ -52,6 +52,9 @@ static int32_t lastConversionStart;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command); static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
// Second order temperature compensation. Temperature offset
static int64_t compensation_t2;
// Move into proper driver structure with cfg stored // Move into proper driver structure with cfg stored
static uint32_t oversampling; static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg; static const struct pios_ms5611_cfg *dev_cfg;
@ -74,6 +77,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
uint8_t data[2]; uint8_t data[2];
// reset temperature compensation values
compensation_t2 = 0;
/* Calibration parameters */ /* Calibration parameters */
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++) {
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
@ -190,17 +196,34 @@ int32_t PIOS_MS5611_ReadADC(void)
} else { } else {
int64_t Offset; int64_t Offset;
int64_t Sens; int64_t Sens;
// used for second order temperature compensation
int64_t Offset2 = 0;
int64_t Sens2 = 0;
/* Read the pressure conversion */ /* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1; return -1;
} }
// check if temperature is less than 20°C
if (Temperature < 2000) {
Offset2 = 5 * (Temperature - 2000) >> 1;
Sens2 = Offset2 >> 1;
compensation_t2 = (deltaTemp * deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C
if (Temperature < -1500) {
int64_t tcorr = (Temperature + 1500) * (Temperature + 1500);
Offset2 += 7 * tcorr;
Sens2 += (11 * tcorr) >> 1;
}
} else {
compensation_t2 = 0;
Offset2 = 0;
Sens2 = 0;
}
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2;
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7);
Sens = ((int64_t)CalibData.C[0]) << 15; Sens = ((int64_t)CalibData.C[0]) << 15;
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8); Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2;
Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15; Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15;
} }
return 0; return 0;
@ -211,7 +234,8 @@ int32_t PIOS_MS5611_ReadADC(void)
*/ */
float PIOS_MS5611_GetTemperature(void) float PIOS_MS5611_GetTemperature(void)
{ {
return ((float)Temperature) / 100.0f; // Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f;
} }
/** /**

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

@ -23,7 +23,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate UAVOBJSRCFILENAMES += gyrostate
@ -104,6 +103,7 @@ UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus UAVOBJSRCFILENAMES += oplinkstatus
UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation UAVOBJSRCFILENAMES += poilocation

2
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -133,7 +133,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#if defined(PIOS_INCLUDE_MS5611) #if defined(PIOS_INCLUDE_MS5611)
#include "pios_ms5611.h" #include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = { static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512, .oversampling = MS5611_OSR_4096,
}; };
#endif /* PIOS_INCLUDE_MS5611 */ #endif /* PIOS_INCLUDE_MS5611 */

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

@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate UAVOBJSRCFILENAMES += gyrostate
@ -102,6 +101,7 @@ UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation UAVOBJSRCFILENAMES += poilocation

2
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -129,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#if defined(PIOS_INCLUDE_MS5611) #if defined(PIOS_INCLUDE_MS5611)
#include "pios_ms5611.h" #include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = { static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512, .oversampling = MS5611_OSR_4096,
}; };
#endif /* PIOS_INCLUDE_MS5611 */ #endif /* PIOS_INCLUDE_MS5611 */

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

@ -42,6 +42,7 @@ MODULES += FirmwareIAP
MODULES += StateEstimation MODULES += StateEstimation
#MODULES += Sensors/simulated/Sensors #MODULES += Sensors/simulated/Sensors
MODULES += Airspeed MODULES += Airspeed
MODULES += AltitudeHold
#MODULES += OveroSync #MODULES += OveroSync
# Paths # Paths

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

@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += attitudesimulated UAVOBJSRCFILENAMES += attitudesimulated
@ -107,6 +106,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += ekfstatevariance

1
ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp
View File

@ -137,6 +137,7 @@ void MyTabbedStackWidget::showWidget(int index)
void MyTabbedStackWidget::resizeEvent(QResizeEvent *event) void MyTabbedStackWidget::resizeEvent(QResizeEvent *event)
{ {
QWidget::resizeEvent(event); QWidget::resizeEvent(event);
m_listWidget->setFixedWidth(m_listWidget->verticalScrollBar()->isVisible() ? LIST_VIEW_WIDTH + 20 : LIST_VIEW_WIDTH); m_listWidget->setFixedWidth(m_listWidget->verticalScrollBar()->isVisible() ? LIST_VIEW_WIDTH + 20 : LIST_VIEW_WIDTH);
} }

2
ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h
View File

@ -79,7 +79,7 @@ private:
static const int LIST_VIEW_WIDTH = 80; static const int LIST_VIEW_WIDTH = 80;
protected: protected:
void resizeEvent(QResizeEvent * event); void resizeEvent(QResizeEvent *event);
}; };
#endif // MYTABBEDSTACKWIDGET_H #endif // MYTABBEDSTACKWIDGET_H

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

@ -28211,7 +28211,7 @@ border-radius: 5;</string>
<string notr="true"/> <string notr="true"/>
</property> </property>
<property name="text"> <property name="text">
<string>Integral</string> <string>Velocity Proportional</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -28236,7 +28236,7 @@ border-radius: 5;</string>
<string notr="true"/> <string notr="true"/>
</property> </property>
<property name="text"> <property name="text">
<string>Proportional</string> <string>Altitude Proportional</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -28267,7 +28267,7 @@ border-radius: 5;</string>
<string notr="true"/> <string notr="true"/>
</property> </property>
<property name="text"> <property name="text">
<string>Derivative</string> <string>Velocity Integral</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -28291,8 +28291,9 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0"> <property name="objrelation" stdset="0">
<stringlist> <stringlist>
<string>objname:AltitudeHoldSettings</string> <string>objname:AltitudeHoldSettings</string>
<string>fieldname:Kp</string> <string>fieldname:AltitudePI</string>
<string>scale:0.001</string> <string>element:Kp</string>
<string>scale:0.01</string>
<string>haslimits:yes</string> <string>haslimits:yes</string>
<string>buttongroup:98</string> <string>buttongroup:98</string>
</stringlist> </stringlist>
@ -28316,8 +28317,9 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0"> <property name="objrelation" stdset="0">
<stringlist> <stringlist>
<string>objname:AltitudeHoldSettings</string> <string>objname:AltitudeHoldSettings</string>
<string>fieldname:Ki</string> <string>fieldname:VelocityPI</string>
<string>scale:0.001</string> <string>element:Kp</string>
<string>scale:0.01</string>
<string>haslimits:yes</string> <string>haslimits:yes</string>
<string>buttongroup:98</string> <string>buttongroup:98</string>
</stringlist> </stringlist>
@ -28327,7 +28329,7 @@ border-radius: 5;</string>
<item row="3" column="2"> <item row="3" column="2">
<widget class="QSlider" name="AltKdSlider"> <widget class="QSlider" name="AltKdSlider">
<property name="maximum"> <property name="maximum">
<number>100</number> <number>1000</number>
</property> </property>
<property name="value"> <property name="value">
<number>50</number> <number>50</number>
@ -28341,8 +28343,9 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0"> <property name="objrelation" stdset="0">
<stringlist> <stringlist>
<string>objname:AltitudeHoldSettings</string> <string>objname:AltitudeHoldSettings</string>
<string>fieldname:Kd</string> <string>fieldname:VelocityPI</string>
<string>scale:0.001</string> <string>element:Ki</string>
<string>scale:0.00001</string>
<string>haslimits:yes</string> <string>haslimits:yes</string>
<string>buttongroup:98</string> <string>buttongroup:98</string>
</stringlist> </stringlist>
@ -28908,7 +28911,7 @@ color: rgb(255, 255, 255);
border-radius: 5;</string> border-radius: 5;</string>
</property> </property>
<property name="text"> <property name="text">
<string>Altitude</string> <string>Control Coefficients</string>
</property> </property>
<property name="alignment"> <property name="alignment">
<set>Qt::AlignCenter</set> <set>Qt::AlignCenter</set>
@ -28953,8 +28956,9 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0"> <property name="objrelation" stdset="0">
<stringlist> <stringlist>
<string>objname:AltitudeHoldSettings</string> <string>objname:AltitudeHoldSettings</string>
<string>fieldname:Kp</string> <string>fieldname:AltitudePI</string>
<string>scale:0.001</string> <string>element:Kp</string>
<string>scale:0.01</string>
<string>haslimits:yes</string> <string>haslimits:yes</string>
<string>buttongroup:98</string> <string>buttongroup:98</string>
</stringlist> </stringlist>
@ -28999,8 +29003,9 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0"> <property name="objrelation" stdset="0">
<stringlist> <stringlist>
<string>objname:AltitudeHoldSettings</string> <string>objname:AltitudeHoldSettings</string>
<string>fieldname:Ki</string> <string>fieldname:VelocityPI</string>
<string>scale:0.001</string> <string>element:Kp</string>
<string>scale:0.01</string>
<string>haslimits:yes</string> <string>haslimits:yes</string>
<string>buttongroup:98</string> <string>buttongroup:98</string>
</stringlist> </stringlist>
@ -29037,7 +29042,7 @@ border-radius: 5;</string>
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set> <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>100</number> <number>1000</number>
</property> </property>
<property name="value"> <property name="value">
<number>51</number> <number>51</number>
@ -29045,8 +29050,9 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0"> <property name="objrelation" stdset="0">
<stringlist> <stringlist>
<string>objname:AltitudeHoldSettings</string> <string>objname:AltitudeHoldSettings</string>
<string>fieldname:Kd</string> <string>fieldname:VelocityPI</string>
<string>scale:0.001</string> <string>element:Ki</string>
<string>scale:0.00001</string>
<string>haslimits:yes</string> <string>haslimits:yes</string>
<string>buttongroup:98</string> <string>buttongroup:98</string>
</stringlist> </stringlist>

8
ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp
View File

@ -318,10 +318,10 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf)
out.velEast = velX; out.velEast = velX;
out.velDown = -velZ; out.velDown = -velZ;
// Update gyroscope sensor data // Update gyroscope sensor data - convert from rad/s to deg/s
out.rollRate = rollRate_rad; out.rollRate = rollRate_rad * (180.0 / M_PI);
out.pitchRate = pitchRate_rad; out.pitchRate = pitchRate_rad * (180.0 / M_PI);
out.yawRate = yawRate_rad; out.yawRate = yawRate_rad * (180.0 / M_PI);
// Update accelerometer sensor data // Update accelerometer sensor data
out.accX = accX; out.accX = accX;

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

@ -37,9 +37,9 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \
$$UAVOBJECT_SYNTHETICS/attitudestate.h \ $$UAVOBJECT_SYNTHETICS/attitudestate.h \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
$$UAVOBJECT_SYNTHETICS/debuglogsettings.h \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.h \
$$UAVOBJECT_SYNTHETICS/debuglogcontrol.h \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.h \
@ -135,9 +135,9 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
$$UAVOBJECT_SYNTHETICS/attitudestate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
$$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \
$$UAVOBJECT_SYNTHETICS/debuglogcontrol.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.cpp \
$$UAVOBJECT_SYNTHETICS/debuglogstatus.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogstatus.cpp \

12
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp
View File

@ -670,7 +670,7 @@ void ConfigTaskWidget::autoLoadWidgets()
forceShadowUpdates(); forceShadowUpdates();
/* /*
foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
if (binding->widget()) { if (binding->widget()) {
qDebug() << "Binding :" << binding->widget()->objectName(); qDebug() << "Binding :" << binding->widget()->objectName();
qDebug() << " Object :" << binding->object()->getName(); qDebug() << " Object :" << binding->object()->getName();
@ -684,8 +684,8 @@ void ConfigTaskWidget::autoLoadWidgets()
qDebug() << " Scale :" << shadow->scale(); qDebug() << " Scale :" << shadow->scale();
} }
} }
} }
*/ */
} }
void ConfigTaskWidget::addWidgetToReloadGroups(QWidget *widget, QList<int> *reloadGroupIDs) void ConfigTaskWidget::addWidgetToReloadGroups(QWidget *widget, QList<int> *reloadGroupIDs)
@ -1161,10 +1161,10 @@ void WidgetBinding::setValue(const QVariant &value)
{ {
m_value = value; m_value = value;
/* /*
if (m_object && m_field) { if (m_object && m_field) {
qDebug() << "WidgetBinding" << m_object->getName() << ":" << m_field->getName() << "value =" << value.toString(); qDebug() << "WidgetBinding" << m_object->getName() << ":" << m_field->getName() << "value =" << value.toString();
} }
*/ */
} }
void WidgetBinding::updateObjectFieldFromValue() void WidgetBinding::updateObjectFieldFromValue()

12
shared/uavobjectdefinition/altholdsmoothed.xml
View File

@ -1,12 +0,0 @@
<xml>
<object name="AltHoldSmoothed" singleinstance="true" settings="false" category="State">
<description>The output on the kalman filter on altitude.</description>
<field name="Altitude" units="m" type="float" elements="1"/>
<field name="Velocity" units="m/s" type="float" elements="1"/>
<field name="Accel" units="m/s^2" 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>

4
shared/uavobjectdefinition/altitudefiltersettings.xml
View File

@ -1,9 +1,9 @@
<xml> <xml>
<object name="AltitudeFilterSettings" singleinstance="true" settings="true" category="State"> <object name="AltitudeFilterSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the @ref State Estimator module plugin altitudeFilter</description> <description>Settings for the @ref State Estimator module plugin altitudeFilter</description>
<field name="AccelLowPassKp" units="m/s^2" type="float" elements="1" defaultvalue="0.07"/> <field name="AccelLowPassKp" units="m/s^2" type="float" elements="1" defaultvalue="0.04"/>
<field name="AccelDriftKi" units="m/s^2" type="float" elements="1" defaultvalue="0.0005"/> <field name="AccelDriftKi" units="m/s^2" type="float" elements="1" defaultvalue="0.0005"/>
<field name="BaroKp" units="m" type="float" elements="1" defaultvalue="0.02"/> <field name="BaroKp" units="m" type="float" elements="1" defaultvalue="0.04"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>

7
shared/uavobjectdefinition/altitudeholddesired.xml
View File

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

14
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -1,15 +1,11 @@
<xml> <xml>
<object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control"> <object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref AltitudeHold module</description> <description>Settings for the @ref AltitudeHold module</description>
<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.0001"/> <field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0"/> <field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.2,0.0002,2.0" />
<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.05"/> <field name="CutThrottleWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
<field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/> <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="PressureNoise" units="m" type="float" elements="1" defaultvalue="0.4"/> <field name="ThrottleRate" units="m/s" type="float" elements="1" defaultvalue="5" />
<field name="AccelNoise" units="m/s^2" type="float" elements="1" defaultvalue="5"/>
<field name="AccelDrift" units="m/s^2" type="float" elements="1" defaultvalue="0.001"/>
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>

10
shared/uavobjectdefinition/altitudeholdstatus.xml Normal file
View File

@ -0,0 +1,10 @@
<xml>
<object name="AltitudeHoldStatus" singleinstance="true" settings="false" category="Control">
<description>Status Data from Altitude Hold Control Loops</description>
<field name="VelocityDesired" units="m/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>

4
shared/uavobjectdefinition/revosettings.xml
View File

@ -7,7 +7,9 @@
Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062 Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062
Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. --> Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. -->
<field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/> <field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
<!-- Cooefficients for the polynomial that models the barometer pressure bias as a function of temperature
bias = a + b * temp + c * temp^2 + d * temp^3 -->
<field name="BaroTempCorrectionPolynomial" units="" type="float" elements="3" elementnames="a,b,c,d" defaultvalue="0,0,0,0"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>