diff --git a/flight/Modules/ManualControl/inc/manualcontrol.h b/flight/Modules/ManualControl/inc/manualcontrol.h
index 608c12d1d..5cca1a623 100644
--- a/flight/Modules/ManualControl/inc/manualcontrol.h
+++ b/flight/Modules/ManualControl/inc/manualcontrol.h
@@ -1,7 +1,7 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
- * @{
+ * @{
* @addtogroup ManualControlModule Manual Control Module
* @{
*
@@ -45,4 +45,64 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
int32_t ManualControlInitialize();
+
+/*
+ * These are assumptions we make in the flight code about the order of settings and their consistency between
+ * objects. Please keep this synchronized to the UAVObjects
+ */
+#define assumptions1 ( \
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
+)
+
+#define assumptions3 ( \
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
+)
+
+#define assumptions5 ( \
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
+)
+
+#define ARMING_CHANNEL_ROLL 0
+#define ARMING_CHANNEL_PITCH 1
+#define ARMING_CHANNEL_YAW 2
+
+#define assumptions7 ( \
+( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
+)
+
+#define assumptions8 ( \
+( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
+( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \
+)
+
+#define assumptions_flightmode ( \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
+)
+
#endif // MANUALCONTROL_H
diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c
index e5e486f91..476ab6b6a 100644
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@@ -87,58 +87,6 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
-#define assumptions1 ( \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
- )
-
-#define assumptions3 ( \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
- )
-
-#define assumptions5 ( \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
- ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
- )
-
-
-
-#define ARMING_CHANNEL_ROLL 0
-#define ARMING_CHANNEL_PITCH 1
-#define ARMING_CHANNEL_YAW 2
-
-#define assumptions7 ( \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
- )
-
-#define assumptions8 ( \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
- ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \
- )
-
-
-#define assumptions_flightmode ( \
- ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
- ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
- ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
- ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
- ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
- ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
- )
-
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
/**
@@ -374,18 +322,21 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+ (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+ (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+ (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
0; // this is an invalid mode
diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c
index 459e3cf12..8d320c510 100644
--- a/flight/Modules/Stabilization/stabilization.c
+++ b/flight/Modules/Stabilization/stabilization.c
@@ -82,6 +82,9 @@ float gyro_filtered[3] = {0,0,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;
+
pid_type pids[PID_MAX];
// Private functions
@@ -217,6 +220,20 @@ static void stabilizationTask(void* parameters)
axis_lock_accum[i] = 0;
break;
+ case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
+ {
+ float weak_leveling = local_error[i] * weak_leveling_kp;
+
+ if(weak_leveling > weak_leveling_max)
+ weak_leveling = weak_leveling_max;
+ if(weak_leveling < -weak_leveling_max)
+ weak_leveling = -weak_leveling_max;
+
+ rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
+
+ axis_lock_accum[i] = 0;
+ break;
+ }
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);
axis_lock_accum[i] = 0;
@@ -257,6 +274,7 @@ static void stabilizationTask(void* parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
+ case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]);
actuatorDesiredAxis[ct] = bound(command);
@@ -392,6 +410,10 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
+ // Settings for weak leveling
+ weak_leveling_kp = settings.WeakLevelingKp;
+ weak_leveling_max = settings.MaxWeakLevelingRate;
+
// 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
// based on a time (in ms) rather than a fixed multiplier. The error between
diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml
index 375de146b..c3e973a26 100644
--- a/shared/uavobjectdefinition/manualcontrolsettings.xml
+++ b/shared/uavobjectdefinition/manualcontrolsettings.xml
@@ -13,9 +13,9 @@
-
-
-
+
+
+
diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml
index f8e6e173d..c1e013be3 100644
--- a/shared/uavobjectdefinition/stabilizationdesired.xml
+++ b/shared/uavobjectdefinition/stabilizationdesired.xml
@@ -6,7 +6,7 @@
-
+
diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml
index 581817e7e..9176b125a 100644
--- a/shared/uavobjectdefinition/stabilizationsettings.xml
+++ b/shared/uavobjectdefinition/stabilizationsettings.xml
@@ -15,9 +15,13 @@
+
+
+
+