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 @@ + + + +