/home/travis/build/lis-epfl/MAVRIC_Library/status/mav_modes.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 mav_mode.hpp
 *
 * \author MAV'RIC Team
 * \author Julien Lecoeur
 *
 * \brief Place where the mav modes and flags are defined
 *
 ******************************************************************************/


#ifndef MAV_MODE_H
#define MAV_MODE_H

#include <cstdint>
#include <cstdbool>

#include "communication/mavlink_stream.hpp"
#include "util/print_util.hpp"


typedef enum MAV_STATE mav_state_t;

class Mav_mode
{
public:
    typedef uint8_t mode_bits_t;
    typedef uint8_t bitmask_t;
    typedef uint32_t custom_mode_t;


    /*
     * \brief enum for defining custom mode
     */
    typedef enum
    {
        CUSTOM_BASE_MODE = 0,
        CUST_CRITICAL_CLIMB_TO_SAFE_ALT = 1,        
        CUST_CRITICAL_FLY_TO_HOME_WP = 2,           
        CUST_CRITICAL_LAND = 4,                     

        CUST_DESCENT_TO_SMALL_ALTITUDE = 8,         
        CUST_DESCENT_TO_GND = 16,                   

        CUST_COLLISION_AVOIDANCE = 32,              

        CUST_BATTERY_LOW = 64,                      
        CUST_FENCE_1 = 128,                         
        CUST_FENCE_2 = 256,                         
        CUST_HEARTBEAT_LOST = 512,                  
        CUST_REMOTE_LOST = 1024,                    
        CUST_GPS_BAD = 2048                         
    } custom_mode_list_t;


    /* Control modes (predefined mode taking into account manual, stabilize, guided, and auto flag) */
    enum ctrl_mode_t
    {
        RATE =          MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
        ATTITUDE =      MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + MAV_MODE_FLAG_STABILIZE_ENABLED,
        VELOCITY =      MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + MAV_MODE_FLAG_STABILIZE_ENABLED + MAV_MODE_FLAG_GUIDED_ENABLED,
        POSITION_HOLD = MAV_MODE_FLAG_STABILIZE_ENABLED + MAV_MODE_FLAG_GUIDED_ENABLED,
        AUTO =       MAV_MODE_FLAG_STABILIZE_ENABLED + MAV_MODE_FLAG_GUIDED_ENABLED + MAV_MODE_FLAG_AUTO_ENABLED
    };

    /* describes which flags are used for ctrl_mode */
    static const bitmask_t CTRL_MODE_BITFIELD = 0b01011100;

    Mav_mode() : bits_(0){};

    Mav_mode(const mode_bits_t& bits) : bits_(bits){};

    Mav_mode(const Mav_mode& mav_mode) : bits_(mav_mode.bits_){};

    inline mode_bits_t bits() const {return bits_;};

    inline const uint8_t* bits_ptr() const {return static_cast<const uint8_t*>(&bits_);};


    inline bool is_armed() const {return is_flag_set(MAV_MODE_FLAG_SAFETY_ARMED);};

    inline bool is_manual() const {return is_flag_set(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED);};

    inline bool is_hil() const {return is_flag_set(MAV_MODE_FLAG_HIL_ENABLED);};

    inline bool is_stabilize() const {return is_flag_set(MAV_MODE_FLAG_STABILIZE_ENABLED);};

    inline bool is_guided() const {return is_flag_set(MAV_MODE_FLAG_GUIDED_ENABLED);};

    inline bool is_auto() const {return is_flag_set(MAV_MODE_FLAG_AUTO_ENABLED);};

    inline bool is_test() const {return is_flag_set(MAV_MODE_FLAG_TEST_ENABLED);};

    inline bool is_custom() const {return is_flag_set(MAV_MODE_FLAG_CUSTOM_MODE_ENABLED);};

    inline ctrl_mode_t ctrl_mode() const {return (ctrl_mode_t)(bits_ & CTRL_MODE_BITFIELD);};

    inline void set_armed_flag(bool value) { set_flag(MAV_MODE_FLAG_SAFETY_ARMED, value);};

    inline void set_manual_flag(bool value) { set_flag(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, value);};

    inline void set_hil_flag(bool value) { set_flag(MAV_MODE_FLAG_HIL_ENABLED, value);};

    inline void set_stabilize_flag(bool value) { set_flag(MAV_MODE_FLAG_STABILIZE_ENABLED, value);};

    inline void set_guided_flag(bool value) { set_flag(MAV_MODE_FLAG_GUIDED_ENABLED, value);};

    inline void set_auto_flag(bool value) { set_flag(MAV_MODE_FLAG_AUTO_ENABLED, value);};

    inline void set_test_flag(bool value) { set_flag(MAV_MODE_FLAG_TEST_ENABLED, value);};

    inline void set_custom_flag(bool value)  { set_flag(MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, value);};

    inline void set_ctrl_mode(ctrl_mode_t ctrl_mode) {bits_ = (bits_ & ~CTRL_MODE_BITFIELD) + (ctrl_mode & CTRL_MODE_BITFIELD);};

    bool operator == (const Mav_mode& mav_mode) const {return equal(mav_mode);};

private:
    mode_bits_t bits_;

    inline bool is_flag_set(MAV_MODE_FLAG flag) const {return ((bits_ & flag) == flag);};

    inline void set_flag(MAV_MODE_FLAG flag, bool value)  { bits_ = value ? bits_ | flag : bits_ & ~flag;};

     inline bool equal(Mav_mode mav_mode) const {return bits_ == mav_mode.bits_;};

    inline void set_mode(mode_bits_t bits) {bits_ = bits;};

};


typedef enum
{
    ARM_ACTION_DISARMING    = -1,   
    ARM_ACTION_NONE         = 0,    
    ARM_ACTION_ARMING       = 1,    
} arm_action_t;


#endif //MAV_MODE_H