diff --git a/flight/libraries/inc/sanitycheck.h b/flight/libraries/inc/sanitycheck.h index f324486ae..9576d1030 100644 --- a/flight/libraries/inc/sanitycheck.h +++ b/flight/libraries/inc/sanitycheck.h @@ -32,6 +32,14 @@ #include +typedef enum { + FRAME_TYPE_MULTIROTOR, + FRAME_TYPE_HELI, + FRAME_TYPE_FIXED_WING, + FRAME_TYPE_GROUND, + FRAME_TYPE_CUSTOM, +} FrameType_t; + #define SANITYCHECK_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE #define SANITYCHECK_STATUS_ERROR_FLIGHTMODE SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE @@ -45,4 +53,6 @@ extern int32_t configuration_check(); +FrameType_t GetCurrentFrameType(); + #endif /* SANITYCHECK_H */ diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index cda3b443a..83e498447 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -94,26 +94,8 @@ int32_t configuration_check() // Classify airframe type - bool multirotor; - uint8_t airframe_type; + bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); - SystemSettingsAirframeTypeGet(&airframe_type); - switch (airframe_type) { - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX: - case SYSTEMSETTINGS_AIRFRAMETYPE_TRI: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX: - multirotor = true; - break; - default: - multirotor = false; - } // For each available flight mode position sanity check the available // modes @@ -272,3 +254,42 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter return true; } + +FrameType_t GetCurrentFrameType() +{ + uint8_t airframe_type; + + SystemSettingsAirframeTypeGet(&airframe_type); + switch ((SystemSettingsAirframeTypeOptions)airframe_type) { + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX: + case SYSTEMSETTINGS_AIRFRAMETYPE_TRI: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX: + return FRAME_TYPE_MULTIROTOR; + + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING: + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON: + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL: + return FRAME_TYPE_FIXED_WING; + + case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP: + return FRAME_TYPE_HELI; + + case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR: + case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL: + case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE: + return FRAME_TYPE_GROUND; + + case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL: + case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM: + return FRAME_TYPE_CUSTOM; + } + // anyway it should not reach here + return FRAME_TYPE_CUSTOM; +}