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