diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 95ea696db..c98a408b6 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -87,103 +87,80 @@ void takeOffLocationHandler(); */ void takeOffLocationHandlerInit(); + +#define STABILIZATIONMODE_TABLE(ENTRY) \ + ENTRY(MANUAL) \ + ENTRY(RATE) \ + ENTRY(ATTITUDE) \ + ENTRY(AXISLOCK) \ + ENTRY(WEAKLEVELING) \ + ENTRY(VIRTUALBAR) \ + ENTRY(ACRO) \ + ENTRY(RATTITUDE) \ + ENTRY(ALTITUDEHOLD) \ + ENTRY(ALTITUDEVARIO) \ + ENTRY(CRUISECONTROL) + +#define EXPAND_FLIGHTSETTINGS_STAB1(x) ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_##x == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_##x) && +#define EXPAND_FLIGHTSETTINGS_STAB2(x) ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_##x == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_##x) && +#define EXPAND_FLIGHTSETTINGS_STAB3(x) ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_##x == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_##x) && +#define EXPAND_FLIGHTSETTINGS_STAB4(x) ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_##x == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_##x) && +#define EXPAND_FLIGHTSETTINGS_STAB5(x) ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_##x == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_##x) && +#define EXPAND_FLIGHTSETTINGS_STAB6(x) ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_##x == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_##x) && + +#define FLIGHTMODE_TABLE(ENTRY) \ + ENTRY(MANUAL) \ + ENTRY(STABILIZED1) \ + ENTRY(STABILIZED2) \ + ENTRY(STABILIZED3) \ + ENTRY(STABILIZED4) \ + ENTRY(STABILIZED5) \ + ENTRY(STABILIZED6) \ + ENTRY(AUTOTUNE) \ + ENTRY(POSITIONHOLD) \ + ENTRY(RETURNTOBASE) \ + ENTRY(LAND) \ + ENTRY(PATHPLANNER) \ + ENTRY(POI) + +#define EXPAND_FLIGHTMODE(x) ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_##x == (int)FLIGHTSTATUS_FLIGHTMODE_##x) && /* * 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)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + STABILIZATIONMODE_TABLE(EXPAND_FLIGHTSETTINGS_STAB1) \ 1 \ ) #define assumptions2 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + STABILIZATIONMODE_TABLE(EXPAND_FLIGHTSETTINGS_STAB2) \ 1 \ ) #define assumptions3 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + STABILIZATIONMODE_TABLE(EXPAND_FLIGHTSETTINGS_STAB3) \ 1 \ ) #define assumptions4 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + STABILIZATIONMODE_TABLE(EXPAND_FLIGHTSETTINGS_STAB4) \ 1 \ ) #define assumptions5 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + STABILIZATIONMODE_TABLE(EXPAND_FLIGHTSETTINGS_STAB5) \ 1 \ ) #define assumptions6 \ ( \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ACRO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ + STABILIZATIONMODE_TABLE(EXPAND_FLIGHTSETTINGS_STAB6) \ 1 \ ) @@ -199,19 +176,7 @@ void takeOffLocationHandlerInit(); #define assumptions_flightmode \ ( \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED4) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED5) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED6) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ - ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) && \ + FLIGHTMODE_TABLE(EXPAND_FLIGHTMODE) \ 1 \ )