FlightController.hpp

Go to the documentation of this file.

#ifndef FLIGHTCONTROLLER_HPP
#define FLIGHTCONTROLLER_HPP

#include "Client.hpp"
#include "msp_msg.hpp"

namespace fcu {

typedef unsigned int uint;

enum class FirmwareType {
    MULTIWII,
    CLEANFLIGHT
};

class FlightController {
public:
    FlightController(const std::string &device, const uint baudrate=115200);

    ~FlightController();

    void waitForConnection();

    void initialise();

    bool isFirmware(const FirmwareType firmware_type);

    bool isFirmwareMultiWii() { return isFirmware(FirmwareType::MULTIWII); }

    bool isFirmwareCleanflight() { return isFirmware(FirmwareType::CLEANFLIGHT); }

    template<typename T, typename C>
    msp::client::SubscriptionBase* subscribe(void (C::*callback)(const T&), C *context, const double tp = 0.0) {
        return client.subscribe(callback, context, tp);
    }

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

    msp::client::SubscriptionBase* getSubscription(const msp::ID& id) {
        return client.getSubscription(id);
    }

    bool sendRequest(const uint8_t id) {
        return client.sendRequest(id);
    }

    bool request(msp::Request &request, const double timeout = 0) {
        return client.request(request, timeout);
    }

    bool request_raw(const uint8_t id, msp::ByteVector &data, const double timeout = 0) {
        return client.request_raw(id, data, timeout);
    }

    bool respond(const msp::Response &response, const bool wait_ack=true) {
        return client.respond(response, wait_ack);
    }

    bool respond_raw(const uint8_t id, msp::ByteVector &data, const bool wait_ack=true) {
        return client.respond_raw(id, data, wait_ack);
    }

    void initBoxes();

    std::map<std::string, uint> &getBoxNames() {
        return box_name_ids;
    }

    bool hasCapability(const msp::msg::Capability &cap) const {
        return ident.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 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);

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

    bool arm(const bool arm);

    bool arm_block();

    bool disarm_block();

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

    bool enableRxMSP() {
        return updateFeatures(
            {"RX_MSP"}, // add
            {"RX_PARALLEL_PWM", "RX_PPM", "RX_SERIAL"} // remove
        );
    }

    bool reboot();

    bool writeEEPROM();

private:

    static const uint8_t MAX_MAPPABLE_RX_INPUTS = 8;

    msp::client::Client client;

    std::map<std::string, uint> box_name_ids;

    msp::msg::Ident ident;

    std::set<msp::msg::Sensor> sensors;

    FirmwareType firmware;

    std::vector<uint8_t> channel_map;
};

} // namespace msp

#endif // FLIGHTCONTROLLER_HPP