FlightController.hpp

Go to the documentation of this file.

#ifndef FLIGHTCONTROLLER_HPP
#define FLIGHTCONTROLLER_HPP

#include "Client.hpp"
#include "FlightMode.hpp"
#include "PeriodicTimer.hpp"
#include "msp_msg.hpp"

namespace fcu {

enum class ControlSource { NONE, SBUS, MSP, OTHER };

class FlightController {
public:
    FlightController();

    ~FlightController();

    bool connect(const std::string &device, const size_t baudrate = 115200,
                 const double &timeout = 0.0, const bool print_info = false);

    bool disconnect();

    bool isConnected() const;

    void setFlightMode(const FlightMode mode);

    FlightMode getFlightMode() const;

    void setControlSource(const ControlSource source);

    ControlSource getControlSource();

    void setRPYT(std::array<double, 4> &&rpyt);

    void generateMSP();

    bool saveSettings();

    bool reboot();

    msp::FirmwareVariant getFwVariant() const;

    int getProtocolVersion() const;

    std::string getBoardName() const;

    void setLoggingLevel(const msp::client::LoggingLevel &level);

    bool setRc(const uint16_t roll, const uint16_t pitch, const uint16_t yaw,
               const uint16_t throttle, const uint16_t aux1 = 1000,
               const uint16_t aux2 = 1000, const uint16_t aux3 = 1000,
               const uint16_t aux4              = 1000,
               const std::vector<uint16_t> auxs = std::vector<uint16_t>());

    bool setRc(const std::vector<uint16_t> channels);

    template <typename T, typename C,
              class = typename std::enable_if<
                  std::is_base_of<msp::Message, T>::value>::type>
    std::shared_ptr<msp::client::SubscriptionBase> subscribe(
        void (C::*callback)(const T &), C *context, const double tp = 0.0) {
        return client_.subscribe(callback, context, tp);
    }

    template <typename T, class = typename std::enable_if<
                              std::is_base_of<msp::Message, T>::value>::type>
    std::shared_ptr<msp::client::SubscriptionBase> subscribe(
        const std::function<void(const T &)> &callback, const double tp = 0.0) {
        return client_.subscribe(callback, tp);
    }

    bool hasSubscription(const msp::ID &id) const {
        return client_.hasSubscription(id);
    }

    std::shared_ptr<msp::client::SubscriptionBase> getSubscription(
        const msp::ID &id) {
        return client_.getSubscription(id);
    }

    bool sendMessage(msp::Message &message, const double timeout = 0) {
        return client_.sendMessage(message, timeout);
    }

    void initBoxes();

    const std::map<std::string, size_t> &getBoxNames() const {
        return box_name_ids_;
    }

    bool hasCapability(const msp::msg::Capability &cap) const {
        return capabilities_.count(cap);
    }

    bool hasBind() const { return hasCapability(msp::msg::Capability::BIND); }

    bool hasDynBal() const {
        return hasCapability(msp::msg::Capability::DYNBAL);
    }

    bool hasFlap() const { return hasCapability(msp::msg::Capability::FLAP); }

    bool hasSensor(const msp::msg::Sensor &sensor) const {
        return sensors_.count(sensor);
    }

    bool hasAccelerometer() const {
        return hasSensor(msp::msg::Sensor::Accelerometer);
    }

    bool hasBarometer() const { return hasSensor(msp::msg::Sensor::Barometer); }

    bool hasMagnetometer() const {
        return hasSensor(msp::msg::Sensor::Magnetometer);
    }

    bool hasGPS() const { return hasSensor(msp::msg::Sensor::GPS); }

    bool hasSonar() const { return hasSensor(msp::msg::Sensor::Sonar); }

    bool isStatusActive(const std::string &status_name);

    bool isArmed() { return isStatusActive("ARM"); }

    bool isStatusFailsafe() { return isStatusActive("FAILSAFE"); }

    bool setMotors(const std::array<uint16_t, msp::msg::N_MOTOR> &motor_values);

    int updateFeatures(
        const std::set<std::string> &add    = std::set<std::string>(),
        const std::set<std::string> &remove = std::set<std::string>());

private:
    // Client instance for managing the actual comms with the flight controller
    msp::client::Client client_;

    // parameters updated by the connect method to cache flight controller info
    std::string board_name_;
    msp::FirmwareVariant fw_variant_;
    int msp_version_;
    std::map<std::string, size_t> box_name_ids_;
    std::set<msp::msg::Sensor> sensors_;
    std::array<uint8_t, msp::msg::MAX_MAPPABLE_RX_INPUTS> channel_map_;
    std::set<msp::msg::Capability> capabilities_;

    // parameters updated by the user, and consumed by MSP control messages
    std::array<double, 4> rpyt_;
    FlightMode flight_mode_;

    std::mutex msp_updates_mutex;

    ControlSource control_source_;

    msp::PeriodicTimer msp_timer_;
};

}  // namespace fcu

#endif  // FLIGHTCONTROLLER_HPP