diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 58f1deb61..69a83015e 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -159,13 +159,13 @@ SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c SRC += $(OPUAVSYNTHDIR)/systemstats.c SRC += $(OPUAVSYNTHDIR)/systemalarms.c SRC += $(OPUAVSYNTHDIR)/systemsettings.c +SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c SRC += $(OPUAVSYNTHDIR)/actuatordesired.c SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c SRC += $(OPUAVSYNTHDIR)/attituderaw.c SRC += $(OPUAVSYNTHDIR)/attitudeactual.c -SRC += $(OPUAVSYNTHDIR)/attitudedesired.c SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/i2cstats.c diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index 19bc72a36..76c6fde6f 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -48,11 +48,11 @@ #include "guidancesettings.h" #include "attituderaw.h" #include "attitudeactual.h" -#include "attitudedesired.h" #include "positiondesired.h" // object that will be updated by the module #include "positionactual.h" #include "manualcontrolcommand.h" #include "nedaccel.h" +#include "stabilizationdesired.h" #include "stabilizationsettings.h" #include "systemsettings.h" #include "velocitydesired.h" @@ -269,7 +269,7 @@ static void updateVtolDesiredAttitude() VelocityDesiredData velocityDesired; VelocityActualData velocityActual; - AttitudeDesiredData attitudeDesired; + StabilizationDesiredData stabDesired; AttitudeActualData attitudeActual; NedAccelData nedAccel; GuidanceSettingsData guidanceSettings; @@ -295,13 +295,13 @@ static void updateVtolDesiredAttitude() VelocityActualGet(&velocityActual); VelocityDesiredGet(&velocityDesired); - AttitudeDesiredGet(&attitudeDesired); + StabilizationDesiredGet(&stabDesired); VelocityDesiredGet(&velocityDesired); AttitudeActualGet(&attitudeActual); StabilizationSettingsGet(&stabSettings); NedAccelGet(&nedAccel); - attitudeDesired.Yaw = 0; // try and face north + stabDesired.Yaw = 0; // try and face north // Compute desired north command northError = velocityDesired.North - velocityActual.North; @@ -330,14 +330,14 @@ static void updateVtolDesiredAttitude() downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] - nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]); - attitudeDesired.Throttle = bound(downCommand, 0, 1); + stabDesired.Throttle = bound(downCommand, 0, 1); // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // craft should move similarly for 5 deg roll versus 5 deg pitch - attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + + stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); - attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + + stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + eastCommand * cosf(attitudeActual.Yaw * M_PI / 180), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); @@ -345,10 +345,14 @@ static void updateVtolDesiredAttitude() // For now override throttle with manual control. Disable at your risk, quad goes to China. ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - attitudeDesired.Throttle = manualControl.Throttle; + stabDesired.Throttle = manualControl.Throttle; } - AttitudeDesiredSet(&attitudeDesired); + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + + StabilizationDesiredSet(&stabDesired); } /** diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 2e4a32e0e..15caba190 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -39,7 +39,7 @@ #include "stabilizationsettings.h" #include "manualcontrolcommand.h" #include "actuatordesired.h" -#include "attitudedesired.h" +#include "stabilizationdesired.h" #include "flighttelemetrystats.h" // Private constants @@ -72,68 +72,71 @@ static xTaskHandle taskHandle; static ArmState_t armState; // Private functions +static void updateActuatorDesired(ManualControlCommandData * cmd); +static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode); + static void manualControlTask(void *parameters); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent); static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); static bool okToArm(void); #define assumptions1 ( \ - ((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) && \ - ((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) && \ - ((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) \ + ((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ) #define assumptions2 ( \ - ((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \ - ((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \ - ((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \ + ((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \ + ((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \ + ((int)MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \ ) #define assumptions3 ( \ - ((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) && \ - ((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) && \ - ((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) \ + ((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ) #define assumptions4 ( \ - ((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \ - ((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \ - ((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \ + ((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \ + ((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \ + ((int)MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \ ) #define assumptions5 ( \ - ((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) && \ - ((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE) && \ - ((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE) \ + ((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ) #define assumptions6 ( \ - ((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \ - ((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \ - ((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \ + ((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \ + ((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) && \ + ((int)MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO == (int)MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) \ ) -#define ARMING_CHANNEL_ROLL 0 -#define ARMING_CHANNEL_PITCH 1 -#define ARMING_CHANNEL_YAW 2 +#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) \ + ( ((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) \ + ( ((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) \ ) @@ -167,14 +170,10 @@ int32_t ManualControlInitialize() static void manualControlTask(void *parameters) { ManualControlSettingsData settings; - StabilizationSettingsData stabSettings; ManualControlCommandData cmd; - ActuatorDesiredData actuator; - AttitudeDesiredData attitude; portTickType lastSysTime; - - - float flightMode; + + float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; @@ -197,7 +196,6 @@ static void manualControlTask(void *parameters) // Read settings ManualControlSettingsGet(&settings); - StabilizationSettingsGet(&stabSettings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; @@ -262,25 +260,13 @@ static void manualControlTask(void *parameters) else cmd.Accessory3 = 0; - if (flightMode < -FLIGHT_MODE_LIMIT) { - // Position 1 - for(int i = 0; i < 3; i++) { - cmd.StabilizationSettings[i] = settings.Pos1StabilizationSettings[i]; // See assumptions1 - } - cmd.FlightMode = settings.Pos1FlightMode; // See assumptions2 - } else if (flightMode > FLIGHT_MODE_LIMIT) { - // Position 3 - for(int i = 0; i < 3; i++) { - cmd.StabilizationSettings[i] = settings.Pos3StabilizationSettings[i]; // See assumptions5 - } - cmd.FlightMode = settings.Pos3FlightMode; // See assumptions6 - } else { - // Position 2 - for(int i = 0; i < 3; i++) { - cmd.StabilizationSettings[i] = settings.Pos2StabilizationSettings[i]; // See assumptions3 - } - cmd.FlightMode = settings.Pos2FlightMode; // See assumptions4 - } + if (flightMode < -FLIGHT_MODE_LIMIT) + cmd.FlightMode = settings.Pos1FlightMode; + else if (flightMode > FLIGHT_MODE_LIMIT) + cmd.FlightMode = settings.Pos3FlightMode; + else + cmd.FlightMode = settings.Pos2FlightMode; + // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is @@ -457,55 +443,63 @@ static void manualControlTask(void *parameters) // Depending on the mode update the Stabilization or Actuator objects - if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) { - actuator.Roll = cmd.Roll; - actuator.Pitch = cmd.Pitch; - actuator.Yaw = cmd.Yaw; - actuator.Throttle = cmd.Throttle; - ActuatorDesiredSet(&actuator); - } else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) { - switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ROLL]) { - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE: - attitude.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; - break; - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE: - attitude.Roll = cmd.Roll * stabSettings.RollMax; - break; - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE: - attitude.Roll = cmd.Roll; - break; - } - switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_PITCH]) { - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE: - attitude.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; - break; - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE: - attitude.Pitch = cmd.Pitch * stabSettings.PitchMax; - break; - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE: - attitude.Pitch = cmd.Pitch; - break; - } - switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_YAW]) { - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE: - attitude.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - break; - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE: - attitude.Yaw = fmod(cmd.Yaw * 180.0, 360); - break; - case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE: - attitude.Yaw = cmd.Yaw; - break; - } - attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle; - for(int i = 0; i < 3; i++) { - attitude.StabilizationSettings[i] = cmd.StabilizationSettings[i]; - } - AttitudeDesiredSet(&attitude); - } + if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) + updateActuatorDesired(&cmd); + else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) + updateStabilizationDesired(&cmd, &settings, flightMode); } } +static void updateActuatorDesired(ManualControlCommandData * cmd) +{ + ActuatorDesiredData actuator; + ActuatorDesiredGet(&actuator); + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; + actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + ActuatorDesiredSet(&actuator); +} + +static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode) +{ + StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + uint8_t * stab_settings; + if (flightMode < -FLIGHT_MODE_LIMIT) + stab_settings = settings->Pos1StabilizationSettings; + else if (flightMode > FLIGHT_MODE_LIMIT) + stab_settings = settings->Pos3StabilizationSettings; + else + stab_settings = settings->Pos2StabilizationSettings; + + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; + + stabilization.Roll = (stab_settings[0] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Roll : + (stab_settings[0] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Roll * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_ROLL] : + (stab_settings[0] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + 0; // this is an invalid mode + ; + stabilization.Roll = (stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Pitch : + (stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Pitch * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_PITCH] : + (stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode + + stabilization.Roll = (stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Yaw : + (stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Yaw * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] : + (stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) : + 0; // this is an invalid mode + + stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + StabilizationDesiredSet(&stabilization); +} + /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 6340e71e0..ea99d3c70 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -36,7 +36,7 @@ #include "stabilizationsettings.h" #include "actuatordesired.h" #include "ratedesired.h" -#include "attitudedesired.h" +#include "stabilizationdesired.h" #include "attitudeactual.h" #include "attituderaw.h" #include "manualcontrolcommand.h" @@ -119,7 +119,7 @@ static void stabilizationTask(void* parameters) ActuatorDesiredData actuatorDesired; - AttitudeDesiredData attitudeDesired; + StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; AttitudeRawData attitudeRaw; @@ -148,14 +148,14 @@ static void stabilizationTask(void* parameters) lastSysTime = thisSysTime; ManualControlCommandGet(&manualControl); - AttitudeDesiredGet(&attitudeDesired); + StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); AttitudeRawGet(&attitudeRaw); RateDesiredGet(&rateDesired); SystemSettingsGet(&systemSettings); - float *attitudeDesiredAxis = &attitudeDesired.Roll; + float *attitudeDesiredAxis = &stabDesired.Roll; float *attitudeActualAxis = &attitudeActual.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; @@ -163,13 +163,13 @@ static void stabilizationTask(void* parameters) //Calculate desired rate for(int8_t ct=0; ct< MAX_AXES; ct++) { - switch(attitudeDesired.StabilizationSettings[ct]) + switch(stabDesired.StabilizationMode[ct]) { - case ATTITUDEDESIRED_STABILIZATIONSETTINGS_RATE: + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: rateDesiredAxis[ct] = attitudeDesiredAxis[ct]; break; - case ATTITUDEDESIRED_STABILIZATIONSETTINGS_ATTITUDE: + case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], 1); break; } @@ -192,18 +192,35 @@ static void stabilizationTask(void* parameters) } } - switch(attitudeDesired.StabilizationSettings[ct]) + switch(stabDesired.StabilizationMode[ct]) { - case ATTITUDEDESIRED_STABILIZATIONSETTINGS_RATE: - case ATTITUDEDESIRED_STABILIZATIONSETTINGS_ATTITUDE: + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: + case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: { float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct], attitudeRaw.gyros[ct], 0); actuatorDesiredAxis[ct] = bound(command); break; } - case ATTITUDEDESIRED_STABILIZATIONSETTINGS_NONE: - actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); - break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: + //actuatorDesiredAxis[ct] = bound(manualAxis[ct]); + //shouldUpdate = 1; + switch (ct) + { + case ROLL: + actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); + shouldUpdate = 1; + break; + case PITCH: + actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); + shouldUpdate = 1; + break; + case YAW: + actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); + shouldUpdate = 1; + break; + } + break; + } } @@ -218,14 +235,14 @@ static void stabilizationTask(void* parameters) if(shouldUpdate) { - actuatorDesired.Throttle = attitudeDesired.Throttle; + actuatorDesired.Throttle = stabDesired.Throttle; if(dT > 15) actuatorDesired.NumLongUpdates++; ActuatorDesiredSet(&actuatorDesired); } if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE || - !shouldUpdate || (attitudeDesired.Throttle < 0)) + !shouldUpdate || (stabDesired.Throttle < 0)) { ZeroPids(); } diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index fa24d3f53..f9c087500 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -31,9 +31,7 @@ UAVOBJSRCFILENAMES += ahrscalibration UAVOBJSRCFILENAMES += ahrssettings UAVOBJSRCFILENAMES += ahrsstatus UAVOBJSRCFILENAMES += attitudeactual -UAVOBJSRCFILENAMES += attitudedesired UAVOBJSRCFILENAMES += attituderaw -# UAVOBJSRCFILENAMES += attitudesettings CC only UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += batterysettings UAVOBJSRCFILENAMES += firmwareiapobj @@ -59,6 +57,7 @@ UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += positiondesired UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired UAVOBJSRCFILENAMES += stabilizationsettings UAVOBJSRCFILENAMES += systemalarms UAVOBJSRCFILENAMES += systemsettings diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 93a9714b1..8bf0e832c 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -74,6 +74,8 @@ 6528CCE212E40F6700CF5144 /* pios_adxl345.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_adxl345.h; sourceTree = ""; }; 65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = ""; }; 65345C871288668B00A5E4E8 /* guidancesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = guidancesettings.xml; sourceTree = ""; }; + 6536D47B1307962C0042A298 /* stabilizationdesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = stabilizationdesired.xml; sourceTree = ""; }; + 6536D4881307AB950042A298 /* UAVObjects.inc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.pascal; path = UAVObjects.inc; sourceTree = ""; }; 65408AA812BB1648004DACC5 /* i2cstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = i2cstats.xml; sourceTree = ""; }; 6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = ""; }; 654612D812B5E9A900B719D0 /* pios_iap.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_iap.c; sourceTree = ""; }; @@ -2633,7 +2635,6 @@ 65C35E5412EFB2F3004811C2 /* ahrssettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrssettings.xml; sourceTree = ""; }; 65C35E5512EFB2F3004811C2 /* ahrsstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrsstatus.xml; sourceTree = ""; }; 65C35E5612EFB2F3004811C2 /* attitudeactual.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudeactual.xml; sourceTree = ""; }; - 65C35E5712EFB2F3004811C2 /* attitudedesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudedesired.xml; sourceTree = ""; }; 65C35E5812EFB2F3004811C2 /* attituderaw.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attituderaw.xml; sourceTree = ""; }; 65C35E5912EFB2F3004811C2 /* baroaltitude.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = baroaltitude.xml; sourceTree = ""; }; 65C35E5A12EFB2F3004811C2 /* batterysettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = batterysettings.xml; sourceTree = ""; }; @@ -7365,7 +7366,6 @@ 65C35E5412EFB2F3004811C2 /* ahrssettings.xml */, 65C35E5512EFB2F3004811C2 /* ahrsstatus.xml */, 65C35E5612EFB2F3004811C2 /* attitudeactual.xml */, - 65C35E5712EFB2F3004811C2 /* attitudedesired.xml */, 65C35E5812EFB2F3004811C2 /* attituderaw.xml */, 65E410AE12F65AEA00725888 /* attitudesettings.xml */, 65C35E5912EFB2F3004811C2 /* baroaltitude.xml */, @@ -7392,6 +7392,7 @@ 65C35E6E12EFB2F3004811C2 /* positionactual.xml */, 65C35E6F12EFB2F3004811C2 /* positiondesired.xml */, 65C35E7012EFB2F3004811C2 /* ratedesired.xml */, + 6536D47B1307962C0042A298 /* stabilizationdesired.xml */, 65C35E7112EFB2F3004811C2 /* stabilizationsettings.xml */, 65C35E7212EFB2F3004811C2 /* systemalarms.xml */, 65C35E7312EFB2F3004811C2 /* systemsettings.xml */, @@ -7520,6 +7521,7 @@ 65E8EF1E11EEA61E00BBF654 /* OpenPilot */ = { isa = PBXGroup; children = ( + 6536D4881307AB950042A298 /* UAVObjects.inc */, 65E8EF1F11EEA61E00BBF654 /* Makefile */, 65E8EF2011EEA61E00BBF654 /* Makefile.posix */, 65E8EF5B11EEA61E00BBF654 /* System */, diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index f26b2a27b..858fe90c6 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -8,7 +8,6 @@ - diff --git a/shared/uavobjectdefinition/attitudedesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml similarity index 77% rename from shared/uavobjectdefinition/attitudedesired.xml rename to shared/uavobjectdefinition/stabilizationdesired.xml index ac8a42a7b..744967584 100644 --- a/shared/uavobjectdefinition/attitudedesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -1,14 +1,14 @@ - + The desired attitude that @ref StabilizationModule will try and achieve if FlightMode is Stabilized. Comes from @ref ManaulControlModule. - + - +