/home/travis/build/lis-epfl/MAVRIC_Library/communication/periodic_telemetry.hpp

/*******************************************************************************
 * Copyright (c) 2009-2016, MAV'RIC 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 of the copyright holder 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 HOLDER 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.
 ******************************************************************************/

/*******************************************************************************
 * \file periodic_telemetry.hpp
 *
 * \author MAV'RIC Team
 * \author Julien Lecoeur
 *
 * \brief Periodic telemetry
 *
 ******************************************************************************/


#ifndef PERIODIC_TELEMETRY_HPP_
#define PERIODIC_TELEMETRY_HPP_

#include <cstdbool>

#include "communication/mavlink_stream.hpp"
#include "communication/mavlink_message_handler.hpp"
#include "runtime/scheduler.hpp"

class Periodic_telemetry
{
public:

    struct conf_t
    {
        Scheduler::conf_t  scheduler_config;   
    };


    static inline conf_t default_config(void);


    template<typename T>
    struct function
    {
        typedef void (*type_t)(const T*, const Mavlink_stream*, mavlink_message_t*);
    };


    Periodic_telemetry(Mavlink_stream& mavlink_stream, Mavlink_message_handler& handler, conf_t config = default_config());


    bool update(void);


    template<typename T>
    bool add(   uint32_t                        task_id,
                uint32_t                        repeat_period,
                typename function<T>::type_t    telemetry_function,
                T*                              telemetry_module,
                Scheduler_task::priority_t      priority    = Scheduler_task::PRIORITY_NORMAL,
                Scheduler_task::timing_mode_t   timing_mode = Scheduler_task::PERIODIC_RELATIVE,
                Scheduler_task::run_mode_t      run_mode    = Scheduler_task::RUN_REGULAR);


    bool sort(void);


    static void toggle_telemetry_stream(Periodic_telemetry* scheduler, uint32_t sysid, const mavlink_message_t* msg);


protected:

    struct telemetry_entry_t
    {
        function<void>::type_t  function;           
        void*                   module;             
        Mavlink_stream*         mavlink_stream;     
    };

    virtual uint32_t max_count(void) = 0;


    virtual Scheduler& scheduler(void) = 0;


    virtual telemetry_entry_t* list(void) = 0;


private:

    Mavlink_stream&             mavlink_stream_;                    
    uint32_t                    count_;                             

    static bool send_message(telemetry_entry_t* telemetry_entry);
};


template<uint32_t N = 10>
class Periodic_telemetry_T: public Periodic_telemetry
{
public:

    Periodic_telemetry_T(Mavlink_stream& mavlink_stream, Mavlink_message_handler& handler, conf_t config):
        Periodic_telemetry(mavlink_stream, handler, config),
        scheduler_(config.scheduler_config)
    {};

protected:

    uint32_t max_count(void)
    {
        return N;
    }

    Scheduler& scheduler(void)
    {
        return scheduler_;
    }

    telemetry_entry_t* list(void)
    {
        return list_;
    }

private:
    Scheduler_T<N>     scheduler_;          
    telemetry_entry_t    list_[N];            
};



Periodic_telemetry::conf_t Periodic_telemetry::default_config(void)
{
    Periodic_telemetry::conf_t conf  = {};

    conf.scheduler_config.schedule_strategy = Scheduler::ROUND_ROBIN;
    conf.scheduler_config.debug             = false;

    return conf;
};

#include "periodic_telemetry.hxx"

#endif /* PERIODIC_TELEMETRY_HPP_ */