/home/travis/build/lis-epfl/MAVRIC_Library/status/state.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 state.hpp
 *
 * \author MAV'RIC Team
 * \author Nicolas Dousse
 *
 * \brief Holds the current status of the MAV
 *
 ******************************************************************************/


#ifndef STATE_HPP_
#define STATE_HPP_

#include "cstdint"
#include <cstdbool>

#include "status/mav_modes.hpp"
#include "communication/mavlink_stream.hpp"
#include "drivers/battery.hpp"
#include "communication/mavlink_message_handler.hpp"


class State
{
    friend class State_machine;
public:

    struct conf_t
    {
        Mav_mode mav_mode;                                  
        mav_state_t mav_state;                              

        Mav_mode::custom_mode_t mav_mode_custom;            

        int32_t simulation_mode;                            
        uint8_t autopilot_type;                             
        uint8_t autopilot_name;                             

        uint32_t sensor_present;                            
        uint32_t sensor_enabled;                            
        uint32_t sensor_health;                             

        bool out_of_safety_geofence;                        
        bool out_of_emergency_geofence;                     

        bool reset_position;                                

        double last_heartbeat_msg;                          
        double max_lost_connection;                         

        uint32_t msg_count;                                 

        bool connection_lost;                               
        bool first_connection_set;                          
    };


    State(Mavlink_stream& mavlink_stream_, Battery& battery, conf_t config = default_config());


    void switch_to_active_mode(mav_state_t* mav_state_);


    void connection_status();


    bool set_armed(bool arming);


    inline bool is_armed() const {return mav_mode_.is_armed();};


    inline bool is_manual() const {return mav_mode_.is_manual();};


    inline bool is_hil() const {return mav_mode_.is_hil();};


    inline bool is_stabilize() const {return mav_mode_.is_stabilize();};


    inline bool is_guided() const {return mav_mode_.is_guided();};


    inline bool is_auto() const {return mav_mode_.is_auto();};

    inline bool is_custom() const {return mav_mode_.is_custom();};


    inline bool is_test() const {return mav_mode_.is_test();};


    inline Mav_mode mav_mode() const {return mav_mode_;};


    static inline conf_t default_config();

    static inline conf_t wing_default_config();

    friend bool state_telemetry_set_mode(State* state, Mav_mode mav_mode);
    friend mav_result_t state_telemetry_send_autopilot_capabilities(State* state, const mavlink_command_long_t* packet);

// TODO:
// All this should be private

    mav_state_t mav_state_;                             
    Mav_mode mav_mode_;                                 
    Mav_mode::custom_mode_t mav_mode_custom;            


    uint8_t autopilot_type;                             
    uint8_t autopilot_name;                             

    uint32_t sensor_present;                            
    uint32_t sensor_enabled;                            
    uint32_t sensor_health;                             

    bool out_of_safety_geofence;                                
    bool out_of_emergency_geofence;                                

    bool reset_position;                                

    double last_heartbeat_msg;                          
    double max_lost_connection;                         

    uint32_t msg_count;                                 

    bool connection_lost;                               
    bool first_connection_set;                          

    Battery& battery_;                                  

private:
    Mavlink_stream&   mavlink_stream_;                  
};


State::conf_t State::default_config()
{
    conf_t conf                  = {};

    conf.mav_mode                = Mav_mode::ATTITUDE;
    conf.mav_state               = MAV_STATE_BOOT;
    conf.simulation_mode         = false;
    conf.autopilot_type          = MAV_TYPE_QUADROTOR;
    conf.autopilot_name          = MAV_AUTOPILOT_GENERIC; // TODO: add MAV_AUTOPILOT_MAVRIC to std mavlink and support waypoint commands in QGC for this autopilot type
    conf.sensor_present          = 0b1111110000100111;
    conf.sensor_enabled          = 0b1111110000100111;
    conf.sensor_health           = 0b1111110000100111;
    conf.max_lost_connection     = 60.0f;

    return conf;
}


State::conf_t State::wing_default_config()
{
    conf_t conf            = {};

    conf.mav_mode                = Mav_mode::ATTITUDE;
    conf.mav_state               = MAV_STATE_BOOT;
    conf.simulation_mode         = false;
    conf.autopilot_type          = MAV_TYPE_FIXED_WING;
    conf.autopilot_name          = MAV_AUTOPILOT_GENERIC; // TODO: add MAV_AUTOPILOT_MAVRIC to std mavlink and support waypoint commands in QGC for this autopilot type
    conf.sensor_present          = 0b1111110000100111;
    conf.sensor_enabled          = 0b1111110000100111;
    conf.sensor_health           = 0b1111110000100111;
    conf.max_lost_connection     = 60.0f;

    return conf;
}

#endif //STATE_HPP_