geofence.c

Go to the documentation of this file.

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, see <http://www.gnu.org/licenses/>
 */


#include "openpilot.h"
#include <eventdispatcher.h>
#include "misc_math.h"
#include "physical_constants.h"

#include "geofencesettings.h"
#include "positionactual.h"
#include "modulesettings.h"


//
// Configuration
//
#define SAMPLE_PERIOD_MS     250

// Private types

// Private variables

// Private functions
static void settingsUpdated(const UAVObjEvent *ev,
                void *ctx, void *obj, int len);
static void checkPosition(const UAVObjEvent *ev,
                void *ctx, void *obj, int len);

// Private variables
static GeoFenceSettingsData *geofenceSettings;

int32_t GeofenceInitialize(void)
{
        bool module_enabled = false;

#ifdef MODULE_Geofence_BUILTIN
        module_enabled = true;
#else
        uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
        ModuleSettingsAdminStateGet(module_state);
        if (module_state[MODULESETTINGS_ADMINSTATE_GEOFENCE] == MODULESETTINGS_ADMINSTATE_ENABLED) {
                module_enabled = true;
        } else {
                module_enabled = false;
        }
#endif

        if (GeoFenceSettingsInitialize() == -1) {
                module_enabled = false;
                return -1;
        }

        if (module_enabled) {
                // allocate and initialize the static data storage only if module is enabled
                geofenceSettings = (GeoFenceSettingsData *) PIOS_malloc(sizeof(GeoFenceSettingsData));
                if (geofenceSettings == NULL) {
                        module_enabled = false;
                        return -1;
                }

                GeoFenceSettingsConnectCallback(settingsUpdated);
                settingsUpdated(NULL, NULL, NULL, 0);

                return 0;
        }

        return -1;
}

/* stub: module has no module thread */
int32_t GeofenceStart(void)
{
        if (geofenceSettings == NULL) {
                return -1;
        }

        // Schedule periodic task to check position
        UAVObjEvent ev = {
                .obj = PositionActualHandle(),
                .instId = 0,
                .event = 0,
        };
        EventPeriodicCallbackCreate(&ev, checkPosition, SAMPLE_PERIOD_MS);

        return 0;
}

MODULE_INITCALL(GeofenceInitialize, GeofenceStart);

static void checkPosition(const UAVObjEvent *ev,
                void *ctx, void *obj, int len)
{
        (void) ev; (void) ctx; (void) obj; (void) len;
        if (PositionActualHandle()) {
                PositionActualData positionActual;
                PositionActualGet(&positionActual);

                const float distance2 = powf(positionActual.North, 2) + powf(positionActual.East, 2);

                // ErrorRadius is squared when it is fetched, so this is correct
                if (distance2 > geofenceSettings->ErrorRadius) {
                        AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_ERROR);
                } else if (distance2 > geofenceSettings->WarningRadius) {
                        AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_WARNING);
                } else {
                        AlarmsClear(SYSTEMALARMS_ALARM_GEOFENCE);
                }
        }
}

static void settingsUpdated(const UAVObjEvent *ev,
                void *ctx, void *obj, int len)
{
        (void) ev; (void) ctx; (void) obj; (void) len;
        GeoFenceSettingsGet(geofenceSettings);

        // Cache squared distances to save computations
        geofenceSettings->WarningRadius = powf(geofenceSettings->WarningRadius, 2);
        geofenceSettings->ErrorRadius = powf(geofenceSettings->ErrorRadius, 2);
}