mavros_uas.h

Go to the documentation of this file.

/*
 * Copyright 2014 Vladimir Ermakov.
 *
 * 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, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#pragma once

#include <mutex>
#include <atomic>
#include <tf/transform_datatypes.h>
#include <mavconn/interface.h>

namespace mavros {

#define UAS_FCU(uasobjptr)                              \
        ((uasobjptr)->fcu_link)

#define UAS_PACK_CHAN(uasobjptr)                        \
        UAS_FCU(uasobjptr)->get_system_id(),            \
        UAS_FCU(uasobjptr)->get_component_id(),         \
        UAS_FCU(uasobjptr)->get_channel()

#define UAS_PACK_TGT(uasobjptr)                         \
        (uasobjptr)->get_tgt_system(),                  \
        (uasobjptr)->get_tgt_component()

class UAS {
public:
        typedef std::lock_guard<std::recursive_mutex> lock_guard;
        typedef std::unique_lock<std::recursive_mutex> unique_lock;

        UAS();
        ~UAS() {};

        void stop(void);

        mavconn::MAVConnInterface::Ptr fcu_link;

        /* -*- HEARTBEAT data -*- */

        void update_heartbeat(uint8_t type_, uint8_t autopilot_) {
                type = type_;
                autopilot = autopilot_;
        }

        void update_connection_status(bool conn_) {
                if (conn_ != connected) {
                        connected = conn_;
                        sig_connection_changed(connected);
                }
        }

        boost::signals2::signal<void(bool)> sig_connection_changed;

        inline bool get_connection_status() {
                return connected;
        }

        inline enum MAV_TYPE get_type() {
                uint8_t type_ = type;
                return static_cast<enum MAV_TYPE>(type_);
        }

        inline enum MAV_AUTOPILOT get_autopilot() {
                uint8_t autopilot_ = autopilot;
                return static_cast<enum MAV_AUTOPILOT>(autopilot_);
        }

        /* -*- FCU target id pair -*- */

        inline uint8_t get_tgt_system() {
                return target_system; // not changed after configuration
        }

        inline uint8_t get_tgt_component() {
                return target_component; // not changed after configuration
        }

        inline void set_tgt(uint8_t sys, uint8_t comp) {
                target_system = sys;
                target_component = comp;
        }

        /* -*- IMU data -*- */

        inline tf::Vector3 get_attitude_angular_velocity() {
                lock_guard lock(mutex);
                return angular_velocity;
        }

        inline void set_attitude_angular_velocity(tf::Vector3 &vec) {
                lock_guard lock(mutex);
                angular_velocity = vec;
        }

        inline tf::Vector3 get_attitude_linear_acceleration() {
                lock_guard lock(mutex);
                return linear_acceleration;
        }

        inline void set_attitude_linear_acceleration(tf::Vector3 &vec) {
                lock_guard lock(mutex);
                linear_acceleration = vec;
        }

        inline tf::Quaternion get_attitude_orientation() {
                lock_guard lock(mutex);
                return orientation;
        }

        inline void set_attitude_orientation(tf::Quaternion &quat) {
                lock_guard lock(mutex);
                orientation = quat;
        }

        /* -*- GPS data -*- */

        inline void set_gps_llae(double latitude, double longitude, double altitude,
                        double eph, double epv) {
                lock_guard lock(mutex);
                gps_latitude = latitude;
                gps_longitude = longitude;
                gps_altitude = altitude;
                gps_eph = eph;
                gps_epv = epv;
        }

        inline double get_gps_latitude() {
                lock_guard lock(mutex);
                return gps_latitude;
        }

        inline double get_gps_longitude() {
                lock_guard lock(mutex);
                return gps_longitude;
        }

        inline double get_gps_altitude() {
                lock_guard lock(mutex);
                return gps_altitude;
        }

        inline double get_gps_eph() {
                lock_guard lock(mutex);
                return gps_eph;
        }

        inline double get_gps_epv() {
                lock_guard lock(mutex);
                return gps_epv;
        }

        inline void set_gps_status(bool fix_status_) {
                fix_status = fix_status_;
        }

        inline bool get_gps_status() {
                return fix_status;
        }

        /* -*- time sync -*- */
        inline void set_time_offset(uint64_t offset) {
                time_offset = offset;
        }

        inline uint64_t get_time_offset(void) {
                return time_offset;
        }

        /* -*- utils -*- */

        inline bool is_ardupilotmega() {
                return MAV_AUTOPILOT_ARDUPILOTMEGA == get_autopilot();
        }

        inline bool is_px4() {
                return MAV_AUTOPILOT_PX4 == get_autopilot();
        }

        std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode);

        bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode);

        //ros::Time fcu_time(uint32_t time_boot_ms);

private:
        std::recursive_mutex mutex;
        std::atomic<uint8_t> type;
        std::atomic<uint8_t> autopilot;
        uint8_t target_system;
        uint8_t target_component;
        std::atomic<bool> connected;
        tf::Vector3 angular_velocity;
        tf::Vector3 linear_acceleration;
        tf::Quaternion orientation;
        double gps_latitude;
        double gps_longitude;
        double gps_altitude;
        double gps_eph;
        double gps_epv;
        std::atomic<bool> fix_status;
        std::atomic<uint64_t> time_offset;
};

}; // namespace mavros