diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 7efaa057b..97e355305 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -114,9 +114,9 @@ int32_t configuration_check() case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: if (coptercontrol) { 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; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: if (coptercontrol) { diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index c66f52ae8..0c6dbd2ed 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -40,22 +40,24 @@ #include "altitude.h" #include "barosensor.h" // object that will be updated by the module +#include "revosettings.h" #if defined(PIOS_INCLUDE_HCSR04) #include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants -#define STACK_SIZE_BYTES 500 +#define STACK_SIZE_BYTES 550 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types // Private variables static xTaskHandle taskHandle; - +static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection; // Private functions static void altitudeTask(void *parameters); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -77,6 +79,9 @@ int32_t AltitudeStart() int32_t AltitudeInitialize() { BaroSensorInitialize(); + RevoSettingsInitialize(); + RevoSettingsConnectCallback(&SettingsUpdatedCb); + #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeInitialize(); #endif @@ -103,6 +108,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters) // Undef for normal operation // #define PIOS_MS5611_SLOW_TEMP_RATE 20 + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); + #ifdef PIOS_MS5611_SLOW_TEMP_RATE uint8_t temp_press_interleave_count = 1; #endif @@ -154,12 +161,12 @@ static void altitudeTask(__attribute__((unused)) void *parameters) vTaskDelay(PIOS_MS5611_GetDelay()); PIOS_MS5611_ReadADC(); - temp = PIOS_MS5611_GetTemperature(); 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)) { data.Altitude = altitude; @@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters) } } +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); +} /** * @} * @} diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index f19759524..3d71ee2b2 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -46,33 +46,35 @@ #include #include +#include #include -#include #include #include #include // object that will be updated by the module -#include -#include +#include #include #include #include -#include #include +#include +#include // Private constants -#define MAX_QUEUE_SIZE 2 -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 4 -#define TIMEOUT_TRESHOLD 200000 + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +#define STACK_SIZE_BYTES 1024 +#define DESIRED_UPDATE_RATE_MS 100 // milliseconds // Private types // Private variables -static xTaskHandle altitudeHoldTaskHandle; -static xQueueHandle queue; +static DelayedCallbackInfo *altitudeHoldCBInfo; static AltitudeHoldSettingsData altitudeHoldSettings; +static struct pid pid0, pid1; + // Private functions -static void altitudeHoldTask(void *parameters); +static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); /** @@ -82,8 +84,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); int32_t AltitudeHoldStart() { // Start main task - xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); + SettingsUpdatedCb(NULL); + DelayedCallbackDispatch(altitudeHoldCBInfo); return 0; } @@ -96,319 +98,122 @@ int32_t AltitudeHoldInitialize() { AltitudeHoldSettingsInitialize(); AltitudeHoldDesiredInitialize(); - AltHoldSmoothedInitialize(); + AltitudeHoldStatusInitialize(); // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); return 0; } 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. */ -static void altitudeHoldTask(__attribute__((unused)) void *parameters) +static void altitudeHoldTask(void) { - AltitudeHoldDesiredData altitudeHoldDesired; - StabilizationDesiredData stabilizationDesired; + static float startThrottle = 0.5f; - portTickType thisTime, lastUpdateTime; - UAVObjEvent ev; + // make sure we run only when we are supposed to run + FlightStatusData flightStatus; - // Force update of the settings - SettingsUpdatedCb(&ev); - // Failsafe handling - uint32_t lastAltitudeHoldDesiredUpdate = 0; - bool enterFailSafe = false; - // Listen for updates. - AltitudeHoldDesiredConnectQueue(queue); - BaroSensorConnectQueue(queue); - FlightStatusConnectQueue(queue); - AccelStateConnectQueue(queue); - bool altitudeHoldFlightMode = false; - BaroSensorAltitudeGet(&smoothed_altitude); - running = false; - enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + break; + default: + pid_zero(&pid0); + pid_zero(&pid1); + StabilizationDesiredThrottleGet(&startThrottle); + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; - uint8_t flightMode; - 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); - } + 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: + // 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) { 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); } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index a8df27c2b..b10613160 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -845,65 +845,60 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * */ 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_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 - uint8_t throttleRate; + float throttleRate; uint8_t throttleExp; static uint8_t flightMode; - static portTickType lastSysTimeAH; - static bool zeroed = false; - portTickType thisSysTime; - float dT; + static bool newaltitude = true; FlightStatusFlightModeGet(&flightMode); AltitudeHoldDesiredData altitudeHoldDesiredData; AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { - throttleExp = 128; - throttleRate = 0; - } else { - AltitudeHoldSettingsThrottleExpGet(&throttleExp); - AltitudeHoldSettingsThrottleRateGet(&throttleRate); - } + AltitudeHoldSettingsThrottleExpGet(&throttleExp); + AltitudeHoldSettingsThrottleRateGet(&throttleRate); StabilizationBankData stabSettings; StabilizationBankGet(&stabSettings); - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); - lastSysTimeAH = thisSysTime; + PositionStateData posState; + PositionStateGet(&posState); altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; - float currentDown; - PositionStateDownGet(¤tDown); if (changed) { - // After not being in this mode for a while init at current height - altitudeHoldDesiredData.Altitude = 0; - zeroed = false; - } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { + newaltitude = true; + } + + 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 - // then apply an "exp" f(x,k) = (k*x*x*x + x*(256−k)) / 256 - altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; - } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; - } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { - // Require the stick to enter the dead band before they can move height - // Vario is not "engaged" when throttleRate == 0 - zeroed = true; + // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 + altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) { + 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); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (newaltitude == true) { + altitudeHoldDesiredData.SetPoint = posState.Down; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; + newaltitude = false; } AltitudeHoldDesiredSet(&altitudeHoldDesiredData); diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 739fa5d72..a44927392 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -37,6 +37,7 @@ // Private constants #define STACK_REQUIRED 64 +#define INIT_CYCLES 100 // Private types struct data { @@ -67,7 +68,7 @@ static int32_t init(stateFilter *self) struct data *this = (struct data *)self->localdata; this->baroOffset = 0.0f; - this->first_run = 100; + this->first_run = INIT_CYCLES; RevoSettingsInitialize(); RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); @@ -82,8 +83,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location 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->baroAlt = this->baroOffset; + this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0]; + this->baroAlt = 0; this->first_run--; UNSET_MASK(state->updated, SENSORUPDATES_baro); } diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index 1d1a289e7..db8d16f1b 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -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_WriteCommand(uint8_t command); +// Second order temperature compensation. Temperature offset +static int64_t compensation_t2; + // Move into proper driver structure with cfg stored static uint32_t oversampling; 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]; + // reset temperature compensation values + compensation_t2 = 0; + /* Calibration parameters */ for (int i = 0; i < 6; i++) { PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); @@ -190,17 +196,34 @@ int32_t PIOS_MS5611_ReadADC(void) } else { int64_t Offset; int64_t Sens; + // used for second order temperature compensation + int64_t Offset2 = 0; + int64_t Sens2 = 0; /* Read the pressure conversion */ if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { 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]); - - Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7); + Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; 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; } return 0; @@ -211,7 +234,8 @@ int32_t PIOS_MS5611_ReadADC(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; } /** diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index f035af687..a0e668138 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -23,7 +23,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += gyrostate @@ -104,6 +103,7 @@ UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += poilocation diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 9ee08bf64..15b667214 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -133,7 +133,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_4096, }; #endif /* PIOS_INCLUDE_MS5611 */ diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index e837abfb8..f93429e80 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += gyrostate @@ -102,6 +101,7 @@ UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += poilocation diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index b3a76c386..ed142f951 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -129,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_4096, }; #endif /* PIOS_INCLUDE_MS5611 */ diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 0cb868cf7..000539682 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -42,6 +42,7 @@ MODULES += FirmwareIAP MODULES += StateEstimation #MODULES += Sensors/simulated/Sensors MODULES += Airspeed +MODULES += AltitudeHold #MODULES += OveroSync # Paths diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 75bc24779..44a9828c6 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += attitudesimulated @@ -107,6 +106,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index e465b50f9..2626ca47b 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -137,6 +137,7 @@ void MyTabbedStackWidget::showWidget(int index) void MyTabbedStackWidget::resizeEvent(QResizeEvent *event) { QWidget::resizeEvent(event); + m_listWidget->setFixedWidth(m_listWidget->verticalScrollBar()->isVisible() ? LIST_VIEW_WIDTH + 20 : LIST_VIEW_WIDTH); } diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h index 4b0777e4e..661200383 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -79,7 +79,7 @@ private: static const int LIST_VIEW_WIDTH = 80; protected: - void resizeEvent(QResizeEvent * event); + void resizeEvent(QResizeEvent *event); }; #endif // MYTABBEDSTACKWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 6a4fa8186..5d745f4c1 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -28296,7 +28296,7 @@ border-radius: 5; - Integral + Velocity Proportional Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28321,7 +28321,7 @@ border-radius: 5; - Proportional + Altitude Proportional Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28352,7 +28352,7 @@ border-radius: 5; - Derivative + Velocity Integral Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28376,8 +28376,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kp - scale:0.001 + fieldname:AltitudePI + element:Kp + scale:0.01 haslimits:yes buttongroup:98 @@ -28401,8 +28402,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Ki - scale:0.001 + fieldname:VelocityPI + element:Kp + scale:0.01 haslimits:yes buttongroup:98 @@ -28412,7 +28414,7 @@ border-radius: 5; - 100 + 1000 50 @@ -28426,8 +28428,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kd - scale:0.001 + fieldname:VelocityPI + element:Ki + scale:0.00001 haslimits:yes buttongroup:98 @@ -28993,7 +28996,7 @@ color: rgb(255, 255, 255); border-radius: 5; - Altitude + Control Coefficients Qt::AlignCenter @@ -29038,8 +29041,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kp - scale:0.001 + fieldname:AltitudePI + element:Kp + scale:0.01 haslimits:yes buttongroup:98 @@ -29084,8 +29088,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Ki - scale:0.001 + fieldname:VelocityPI + element:Kp + scale:0.01 haslimits:yes buttongroup:98 @@ -29122,7 +29127,7 @@ border-radius: 5; Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - 100 + 1000 51 @@ -29130,8 +29135,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kd - scale:0.001 + fieldname:VelocityPI + element:Ki + scale:0.00001 haslimits:yes buttongroup:98 diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index 53609951e..e7b409815 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -318,10 +318,10 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) out.velEast = velX; out.velDown = -velZ; - // Update gyroscope sensor data - out.rollRate = rollRate_rad; - out.pitchRate = pitchRate_rad; - out.yawRate = yawRate_rad; + // Update gyroscope sensor data - convert from rad/s to deg/s + out.rollRate = rollRate_rad * (180.0 / M_PI); + out.pitchRate = pitchRate_rad * (180.0 / M_PI); + out.yawRate = yawRate_rad * (180.0 / M_PI); // Update accelerometer sensor data out.accX = accX; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 52dde1b29..e5208baa9 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -37,9 +37,9 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ $$UAVOBJECT_SYNTHETICS/attitudestate.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ - $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ + $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \ $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.h \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.h \ @@ -135,9 +135,9 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ - $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogstatus.cpp \ diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index aa387f932..bc236af75 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -670,7 +670,7 @@ void ConfigTaskWidget::autoLoadWidgets() forceShadowUpdates(); /* - foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { + foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { if (binding->widget()) { qDebug() << "Binding :" << binding->widget()->objectName(); qDebug() << " Object :" << binding->object()->getName(); @@ -684,8 +684,8 @@ void ConfigTaskWidget::autoLoadWidgets() qDebug() << " Scale :" << shadow->scale(); } } - } - */ + } + */ } void ConfigTaskWidget::addWidgetToReloadGroups(QWidget *widget, QList *reloadGroupIDs) @@ -1161,10 +1161,10 @@ void WidgetBinding::setValue(const QVariant &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(); - } - */ + } + */ } void WidgetBinding::updateObjectFieldFromValue() diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml deleted file mode 100644 index b32e977c2..000000000 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - The output on the kalman filter on altitude. - - - - - - - - - diff --git a/shared/uavobjectdefinition/altitudefiltersettings.xml b/shared/uavobjectdefinition/altitudefiltersettings.xml index bc6222ad1..9b3eb0838 100644 --- a/shared/uavobjectdefinition/altitudefiltersettings.xml +++ b/shared/uavobjectdefinition/altitudefiltersettings.xml @@ -1,9 +1,9 @@ Settings for the @ref State Estimator module plugin altitudeFilter - + - + diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index af2dafe94..1b349766f 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,13 +1,14 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through - - + + + - + diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 3a2cf6149..174468344 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,15 +1,11 @@ Settings for the @ref AltitudeHold module - - - - - - - - - + + + + + diff --git a/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml new file mode 100644 index 000000000..f298a45ab --- /dev/null +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -0,0 +1,10 @@ + + + Status Data from Altitude Hold Control Loops + + + + + + + diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 0cf9a4561..8d634d1cd 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -7,7 +7,9 @@ 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. --> - + +