diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index cad5d065d..3dc4c53bc 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -589,6 +589,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : @@ -597,6 +598,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : @@ -605,6 +607,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 43bb9b0d5..c49f5c103 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -36,6 +36,7 @@ #include "stabilizationsettings.h" #include "actuatordesired.h" #include "ratedesired.h" +#include "relaytuningsettings.h" #include "stabilizationdesired.h" #include "attitudeactual.h" #include "gyros.h" @@ -231,6 +232,7 @@ static void stabilizationTask(void* parameters) { switch(stabDesired.StabilizationMode[i]) { + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: rateDesiredAxis[i] = attitudeDesiredAxis[i]; @@ -317,6 +319,16 @@ static void stabilizationTask(void* parameters) { switch(stabDesired.StabilizationMode[i]) { + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: + { + RelayTuningSettingsData relay; + RelayTuningSettingsGet(&relay); + float error = rateDesiredAxis[i] - gyro_filtered[i]; + float command = error > 0 ? relay.Amplitude : -relay.Amplitude; + actuatorDesiredAxis[i] = bound(command); + break; + break; + } case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 09d4ca618..8a52b3b23 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -18,9 +18,9 @@ - - - + + + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 02c63cd90..f24dd9eeb 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - +