px4_module.h

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#pragma once

#include <pthread.h>
#include <unistd.h>
#include <stdbool.h>

#include <px4_atomic.h>
#include <px4_time.h>
#include <px4_log.h>
#include <px4_tasks.h>
#include <systemlib/px4_macros.h>

#ifdef __cplusplus

#include <cstring>

extern pthread_mutex_t px4_modules_mutex;

template<class T>
class ModuleBase
{
public:
    ModuleBase() : _task_should_exit{false} {}
    virtual ~ModuleBase() {}

    static int main(int argc, char *argv[])
    {
        if (argc <= 1 ||
            strcmp(argv[1], "-h")    == 0 ||
            strcmp(argv[1], "help")  == 0 ||
            strcmp(argv[1], "info")  == 0 ||
            strcmp(argv[1], "usage") == 0) {
            return T::print_usage();
        }

        if (strcmp(argv[1], "start") == 0) {
            // Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument.
            return start_command_base(argc - 1, argv + 1);
        }

        if (strcmp(argv[1], "status") == 0) {
            return status_command();
        }

        if (strcmp(argv[1], "stop") == 0) {
            return stop_command();
        }

        lock_module(); // Lock here, as the method could access _object.
        int ret = T::custom_command(argc - 1, argv + 1);
        unlock_module();

        return ret;
    }

    static int run_trampoline(int argc, char *argv[])
    {
        int ret = 0;

#ifdef __PX4_NUTTX
        // On NuttX task_create() adds the task name as first argument.
        argc -= 1;
        argv += 1;
#endif

        T *object = T::instantiate(argc, argv);
        _object.store(object);

        if (object) {
            object->run();

        } else {
            PX4_ERR("failed to instantiate object");
            ret = -1;
        }

        exit_and_cleanup();

        return ret;
    }

    static int start_command_base(int argc, char *argv[])
    {
        int ret = 0;
        lock_module();

        if (is_running()) {
            ret = -1;
            PX4_ERR("Task already running");

        } else {
            ret = T::task_spawn(argc, argv);

            if (ret < 0) {
                PX4_ERR("Task start failed (%i)", ret);
            }
        }

        unlock_module();
        return ret;
    }

    static int stop_command()
    {
        int ret = 0;
        lock_module();

        if (is_running()) {
            T *object = _object.load();

            if (object) {
                object->request_stop();

                unsigned int i = 0;

                do {
                    unlock_module();
                    px4_usleep(20000); // 20 ms
                    lock_module();

                    if (++i > 100 && _task_id != -1) { // wait at most 2 sec
                        if (_task_id != task_id_is_work_queue) {
                            px4_task_delete(_task_id);
                        }

                        _task_id = -1;

                        delete _object.load();
                        _object.store(nullptr);

                        ret = -1;
                        break;
                    }
                } while (_task_id != -1);

            } else {
                // In the very unlikely event that can only happen on work queues,
                // if the starting thread does not wait for the work queue to initialize,
                // and inside the work queue, the allocation of _object fails
                // and exit_and_cleanup() is not called, set the _task_id as invalid.
                _task_id = -1;
            }
        }

        unlock_module();
        return ret;
    }

    static int status_command()
    {
        int ret = -1;
        lock_module();

        if (is_running() && _object.load()) {
            T *object = _object.load();
            ret = object->print_status();

        } else {
            PX4_INFO("not running");
        }

        unlock_module();
        return ret;
    }

    virtual int print_status()
    {
        PX4_INFO("running");
        return 0;
    }

    virtual void run()
    {
    }

    static bool is_running()
    {
        return _task_id != -1;
    }

protected:

    virtual void request_stop()
    {
        _task_should_exit.store(true);
    }

    bool should_exit() const
    {
        return _task_should_exit.load();
    }

    static void exit_and_cleanup()
    {
        // Take the lock here:
        // - if startup fails and we're faster than the parent thread, it will set
        //   _task_id and subsequently it will look like the task is running.
        // - deleting the object must take place inside the lock.
        lock_module();

        delete _object.load();
        _object.store(nullptr);

        _task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue.
        unlock_module();
    }

    static int wait_until_running()
    {
        int i = 0;

        do {
            /* Wait up to 1s. */
            px4_usleep(2500);

        } while (!_object.load() && ++i < 400);

        if (i == 400) {
            PX4_ERR("Timed out while waiting for thread to start");
            return -1;
        }

        return 0;
    }

    static T *get_instance()
    {
        return (T *)_object.load();
    }

    static px4::atomic<T *> _object;

    static int _task_id;

    static constexpr const int task_id_is_work_queue = -2;

private:
    static void lock_module()
    {
        pthread_mutex_lock(&px4_modules_mutex);
    }

    static void unlock_module()
    {
        pthread_mutex_unlock(&px4_modules_mutex);
    }

    px4::atomic_bool _task_should_exit{false};
};

template<class T>
px4::atomic<T *> ModuleBase<T>::_object{nullptr};

template<class T>
int ModuleBase<T>::_task_id = -1;


#endif /* __cplusplus */


__BEGIN_DECLS

#ifdef __PX4_NUTTX

static inline void PRINT_MODULE_DESCRIPTION(const char *description) {}
#else

__EXPORT void PRINT_MODULE_DESCRIPTION(const char *description);
#endif

__EXPORT void PRINT_MODULE_USAGE_NAME(const char *executable_name, const char *category);

__EXPORT void PRINT_MODULE_USAGE_SUBCATEGORY(const char *subcategory);

__EXPORT void PRINT_MODULE_USAGE_NAME_SIMPLE(const char *executable_name, const char *category);


__EXPORT void PRINT_MODULE_USAGE_COMMAND_DESCR(const char *name, const char *description);

#define PRINT_MODULE_USAGE_COMMAND(name) \
    PRINT_MODULE_USAGE_COMMAND_DESCR(name, NULL);

#define PRINT_MODULE_USAGE_DEFAULT_COMMANDS() \
    PRINT_MODULE_USAGE_COMMAND("stop"); \
    PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info");


__EXPORT void PRINT_MODULE_USAGE_PARAM_INT(char option_char, int default_val, int min_val, int max_val,
        const char *description, bool is_optional);

__EXPORT void PRINT_MODULE_USAGE_PARAM_FLOAT(char option_char, float default_val, float min_val, float max_val,
        const char *description, bool is_optional);

__EXPORT void PRINT_MODULE_USAGE_PARAM_FLAG(char option_char, const char *description, bool is_optional);

__EXPORT void PRINT_MODULE_USAGE_PARAM_STRING(char option_char, const char *default_val, const char *values,
        const char *description, bool is_optional);

__EXPORT void PRINT_MODULE_USAGE_PARAM_COMMENT(const char *comment);


__EXPORT void PRINT_MODULE_USAGE_ARG(const char *values, const char *description, bool is_optional);


__END_DECLS