/** * Set an alarm * @param alarm The system alarm to be modified * @param severity The alarm severity * @return 0 if success, -1 if an error */ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity) { SystemAlarmsData alarms; // Check that this is a valid alarm if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) { return -1; } // Lock xSemaphoreTakeRecursive(lock, portMAX_DELAY); // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); if ( alarms.Alarm[alarm] != severity ) { alarms.Alarm[alarm] = severity; SystemAlarmsSet(&alarms); } // Release lock xSemaphoreGiveRecursive(lock); return 0; }
/** * Check if there are any alarms with the given or higher severity * @return 0 if no alarms are found, 1 if at least one alarm is found */ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) { SystemAlarmsData alarms; uint32_t n; // 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; } } // If this point is reached then no alarms found xSemaphoreGiveRecursive(lock); return 0; }
/** * Get an alarm * @param alarm The system alarm to be read * @return Alarm severity */ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm) { SystemAlarmsData alarms; // Check that this is a valid alarm if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) { return 0; } // Read alarm SystemAlarmsGet(&alarms); return alarms.Alarm[alarm]; }
/** * @brief Determine if the aircraft is safe to arm * @returns True if safe to arm, false otherwise */ static bool okToArm(void) { // update checks configuration_check(); // read alarms SystemAlarmsData alarms; SystemAlarmsGet(&alarms); // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { if (SystemAlarmsAlarmToArray(alarms.Alarm)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { continue; } return false; } } StabilizationDesiredStabilizationModeData stabDesired; uint8_t flightMode; FlightStatusFlightModeGet(&flightMode); switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: // Prevent arming if unsafe due to the current Thrust Mode StabilizationDesiredStabilizationModeGet(&stabDesired); if (stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD || stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) { return false; } else { return true; } break; default: return false; break; } }
/** * @brief Determine if the aircraft is safe to arm based on alarms * @returns True if safe to arm, false otherwise */ static bool ok_to_arm(void) { // read alarms SystemAlarmsData alarms; SystemAlarmsGet(&alarms); // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR && i != SYSTEMALARMS_ALARM_GPS && i != SYSTEMALARMS_ALARM_TELEMETRY) { return false; } } return true; }
/** * Indicate there are conditions worth an error LED */ bool indicateError() { SystemAlarmsData alarms; SystemAlarmsGet(&alarms); bool error = false; for (uint32_t i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { switch(i) { case SYSTEMALARMS_ALARM_TELEMETRY: // Suppress most alarms from telemetry. The user can identify if present // from GCS. error |= (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_CRITICAL); break; default: // Warning deserves an error by default error |= (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_WARNING); } } return error; }
/** * @brief Determine if the aircraft is safe to arm * @returns True if safe to arm, false otherwise */ static bool okToArm(void) { // read alarms SystemAlarmsData alarms; SystemAlarmsGet(&alarms); // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) continue; return false; } } return true; }