diff --git a/flight/Libraries/alarms.c b/flight/Libraries/alarms.c index 2e79a156a..a2840004d 100644 --- a/flight/Libraries/alarms.c +++ b/flight/Libraries/alarms.c @@ -46,12 +46,12 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity); int32_t AlarmsInitialize(void) { SystemAlarmsInitialize(); - - lock = xSemaphoreCreateRecursiveMutex(); - //do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that - //AlarmsClearAll(); - //AlarmsDefaultAll(); - return 0; + + lock = xSemaphoreCreateRecursiveMutex(); + //do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that + //AlarmsClearAll(); + //AlarmsDefaultAll(); + return 0; } /** @@ -62,26 +62,23 @@ int32_t AlarmsInitialize(void) */ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity) { - SystemAlarmsData alarms; + SystemAlarmsData alarms; - // Check that this is a valid alarm - if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) - { - return -1; - } - - // Lock - if(xSemaphoreTakeRecursive(lock, portMAX_DELAY) != 0){ + // Check that this is a valid alarm + if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) { return -1; } + // Lock + if (xSemaphoreTakeRecursive(lock, portMAX_DELAY) != 0) { + return -1; + } // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); - if ( alarms.Alarm[alarm] != severity ) - { - alarms.Alarm[alarm] = severity; - SystemAlarmsSet(&alarms); + if (alarms.Alarm[alarm] != severity) { + alarms.Alarm[alarm] = severity; + SystemAlarmsSet(&alarms); } // Release lock @@ -103,20 +100,18 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions SystemAlarmsData alarms; // Check that this is a valid alarm - if (alarm >= SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM) - { + if (alarm >= SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM) { return -1; } // Lock - if(xSemaphoreTakeRecursive(lock, portMAX_DELAY) != 0){ + if (xSemaphoreTakeRecursive(lock, portMAX_DELAY) != 0) { return -1; } // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); - if (alarms.Alarm[alarm] != severity) - { + if (alarms.Alarm[alarm] != severity) { alarms.ExtendedAlarmStatus[alarm] = status; alarms.ExtendedAlarmSubStatus[alarm] = subStatus; alarms.Alarm[alarm] = severity; @@ -135,13 +130,12 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions */ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm) { - SystemAlarmsData alarms; + SystemAlarmsData alarms; - // Check that this is a valid alarm - if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) - { - return 0; - } + // Check that this is a valid alarm + if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) { + return 0; + } // Read alarm SystemAlarmsGet(&alarms); @@ -155,7 +149,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm) */ int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm) { - return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT); + return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT); } /** @@ -163,10 +157,9 @@ int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm) */ void AlarmsDefaultAll() { - uint32_t n; - for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) - { - AlarmsDefault(n); + uint32_t n; + for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { + AlarmsDefault(n); } } @@ -177,7 +170,7 @@ void AlarmsDefaultAll() */ int32_t AlarmsClear(SystemAlarmsAlarmElem alarm) { - if (alarm < SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM){ + if (alarm < SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM) { return ExtendedAlarmsSet(alarm, SYSTEMALARMS_ALARM_OK, 0, 0); } else { return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK); @@ -189,10 +182,9 @@ int32_t AlarmsClear(SystemAlarmsAlarmElem alarm) */ void AlarmsClearAll() { - uint32_t n; - for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) - { - AlarmsClear(n); + uint32_t n; + for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { + AlarmsClear(n); } } @@ -202,7 +194,7 @@ void AlarmsClearAll() */ int32_t AlarmsHasWarnings() { - return hasSeverity(SYSTEMALARMS_ALARM_WARNING); + return hasSeverity(SYSTEMALARMS_ALARM_WARNING); } /** @@ -211,8 +203,9 @@ int32_t AlarmsHasWarnings() */ int32_t AlarmsHasErrors() { - return hasSeverity(SYSTEMALARMS_ALARM_ERROR); -}; + return hasSeverity(SYSTEMALARMS_ALARM_ERROR); +} +; /** * Check if there are any alarms with critical or higher severity @@ -220,8 +213,9 @@ int32_t AlarmsHasErrors() */ int32_t AlarmsHasCritical() { - return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL); -}; + return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL); +} +; /** * Check if there are any alarms with the given or higher severity @@ -229,23 +223,21 @@ int32_t AlarmsHasCritical() */ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) { - SystemAlarmsData alarms; - uint32_t n; + SystemAlarmsData alarms; + uint32_t n; - // Lock + // Lock xSemaphoreTakeRecursive(lock, portMAX_DELAY); // Read alarms SystemAlarmsGet(&alarms); // Go through alarms and check if any are of the given severity or higher - for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) - { - if ( alarms.Alarm[n] >= severity) - { - xSemaphoreGiveRecursive(lock); - return 1; - } + for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { + if (alarms.Alarm[n] >= severity) { + xSemaphoreGiveRecursive(lock); + return 1; + } } // If this point is reached then no alarms found diff --git a/flight/Libraries/sanitycheck.c b/flight/Libraries/sanitycheck.c index b14fe30e6..c98369597 100644 --- a/flight/Libraries/sanitycheck.c +++ b/flight/Libraries/sanitycheck.c @@ -53,102 +53,102 @@ static int32_t check_stabilization_settings(int index, bool multirotor); */ int32_t configuration_check() { - int32_t severity = SYSTEMALARMS_ALARM_OK; - uint8_t alarmstatus = SANITYCHECK_STATUS_ERROR_NONE; - uint8_t alarmsubstatus = 0; - // Get board type - const struct pios_board_info * bdinfo = &pios_board_info_blob; - bool coptercontrol = bdinfo->board_type == 0x04; + int32_t severity = SYSTEMALARMS_ALARM_OK; + uint8_t alarmstatus = SANITYCHECK_STATUS_ERROR_NONE; + uint8_t alarmsubstatus = 0; + // Get board type + const struct pios_board_info * bdinfo = &pios_board_info_blob; + bool coptercontrol = bdinfo->board_type == 0x04; - // Classify airframe type - bool multirotor = true; - uint8_t airframe_type; - 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: - multirotor = true; - break; - default: - multirotor = false; - } + // Classify airframe type + bool multirotor = true; + uint8_t airframe_type; + 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: + multirotor = true; + break; + default: + multirotor = false; + } - // For each available flight mode position sanity check the available - // modes - uint8_t num_modes; - uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM]; - ManualControlSettingsFlightModeNumberGet(&num_modes); - ManualControlSettingsFlightModePositionGet(modes); + // For each available flight mode position sanity check the available + // modes + uint8_t num_modes; + uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM]; + ManualControlSettingsFlightModeNumberGet(&num_modes); + ManualControlSettingsFlightModePositionGet(modes); - for(uint32_t i = 0; i < num_modes; i++) { - switch(modes[i]) { - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: - if (multirotor) { - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: - if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE)) { - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: - if (coptercontrol){ - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - default: - // Uncovered modes are automatically an error - severity = SYSTEMALARMS_ALARM_ERROR; - } - // mark the first encountered erroneous setting in status and substatus - if(severity != SYSTEMALARMS_ALARM_OK && alarmstatus == SANITYCHECK_STATUS_ERROR_NONE) - { - alarmstatus = SANITYCHECK_STATUS_ERROR_FLIGHTMODE; - alarmsubstatus = i; - } + for(uint32_t i = 0; i < num_modes; i++) { + switch(modes[i]) { + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: + if (multirotor) { + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: + if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE)) { + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: + if (coptercontrol){ + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + default: + // Uncovered modes are automatically an error + severity = SYSTEMALARMS_ALARM_ERROR; + } + // mark the first encountered erroneous setting in status and substatus + if(severity != SYSTEMALARMS_ALARM_OK && alarmstatus == SANITYCHECK_STATUS_ERROR_NONE) + { + alarmstatus = SANITYCHECK_STATUS_ERROR_FLIGHTMODE; + alarmsubstatus = i; + } - } + } - // TODO: Check on a multirotor no axis supports "None" - if(severity != SYSTEMALARMS_ALARM_OK) - ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus); - else - AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION); + // TODO: Check on a multirotor no axis supports "None" + if(severity != SYSTEMALARMS_ALARM_OK) + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus); + else + AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION); - return 0; + return 0; } /** @@ -159,39 +159,39 @@ int32_t configuration_check() */ static int32_t check_stabilization_settings(int index, bool multirotor) { - // Make sure the modes have identical sizes - if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM || - MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) - return SYSTEMALARMS_ALARM_ERROR; + // Make sure the modes have identical sizes + if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM || + MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) + return SYSTEMALARMS_ALARM_ERROR; - uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM]; + uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM]; - // Get the different axis modes for this switch position - switch(index) { - case 1: - ManualControlSettingsStabilization1SettingsGet(modes); - break; - case 2: - ManualControlSettingsStabilization2SettingsGet(modes); - break; - case 3: - ManualControlSettingsStabilization3SettingsGet(modes); - break; - default: - return SYSTEMALARMS_ALARM_ERROR; - } + // Get the different axis modes for this switch position + switch(index) { + case 1: + ManualControlSettingsStabilization1SettingsGet(modes); + break; + case 2: + ManualControlSettingsStabilization2SettingsGet(modes); + break; + case 3: + ManualControlSettingsStabilization3SettingsGet(modes); + break; + default: + return SYSTEMALARMS_ALARM_ERROR; + } - // For multirotors verify that nothing is set to "none" - if (multirotor) { - for(uint32_t i = 0; i < NELEMENTS(modes); i++) { - if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) - return SYSTEMALARMS_ALARM_ERROR; - } - } + // For multirotors verify that nothing is set to "none" + if (multirotor) { + for(uint32_t i = 0; i < NELEMENTS(modes); i++) { + if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) + return SYSTEMALARMS_ALARM_ERROR; + } + } - // Warning: This assumes that certain conditions in the XML file are met. That - // MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel - // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE + // Warning: This assumes that certain conditions in the XML file are met. That + // MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel + // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE - return SYSTEMALARMS_ALARM_OK; + return SYSTEMALARMS_ALARM_OK; }