/**
* 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;
}
