FlightController.hpp
Go to the documentation of this file.
00001 #ifndef FLIGHTCONTROLLER_HPP
00002 #define FLIGHTCONTROLLER_HPP
00003 
00004 #include "Client.hpp"
00005 #include "msp_msg.hpp"
00006 
00007 namespace fcu {
00008 
00009 typedef unsigned int uint;
00010 
00011 enum class FirmwareType {
00012     MULTIWII,
00013     CLEANFLIGHT
00014 };
00015 
00016 class FlightController {
00017 public:
00018     FlightController(const std::string &device, const uint baudrate=115200);
00019 
00020     ~FlightController();
00021 
00022     void waitForConnection();
00023 
00024     void initialise();
00025 
00032     bool isFirmware(const FirmwareType firmware_type);
00033 
00034     bool isFirmwareMultiWii() { return isFirmware(FirmwareType::MULTIWII); }
00035 
00036     bool isFirmwareCleanflight() { return isFirmware(FirmwareType::CLEANFLIGHT); }
00037 
00045     template<typename T, typename C>
00046     msp::client::SubscriptionBase* subscribe(void (C::*callback)(const T&), C *context, const double tp = 0.0) {
00047         return client.subscribe(callback, context, tp);
00048     }
00049 
00056     bool hasSubscription(const msp::ID& id) {
00057         return client.hasSubscription(id);
00058     }
00059 
00065     msp::client::SubscriptionBase* getSubscription(const msp::ID& id) {
00066         return client.getSubscription(id);
00067     }
00068 
00075     bool sendRequest(const uint8_t id) {
00076         return client.sendRequest(id);
00077     }
00078 
00079     bool request(msp::Request &request, const double timeout = 0) {
00080         return client.request(request, timeout);
00081     }
00082 
00083     bool request_raw(const uint8_t id, msp::ByteVector &data, const double timeout = 0) {
00084         return client.request_raw(id, data, timeout);
00085     }
00086 
00087     bool respond(const msp::Response &response, const bool wait_ack=true) {
00088         return client.respond(response, wait_ack);
00089     }
00090 
00091     bool respond_raw(const uint8_t id, msp::ByteVector &data, const bool wait_ack=true) {
00092         return client.respond_raw(id, data, wait_ack);
00093     }
00094 
00095     void initBoxes();
00096 
00097     std::map<std::string, uint> &getBoxNames() {
00098         return box_name_ids;
00099     }
00100 
00101     bool hasCapability(const msp::msg::Capability &cap) const {
00102         return ident.capabilities.count(cap);
00103     }
00104 
00105     bool hasBind() const {
00106         return hasCapability(msp::msg::Capability::BIND);
00107     }
00108 
00109     bool hasDynBal() const {
00110         return hasCapability(msp::msg::Capability::DYNBAL);
00111     }
00112 
00113     bool hasFlap() const {
00114         return hasCapability(msp::msg::Capability::FLAP);
00115     }
00116 
00117     bool hasSensor(const msp::msg::Sensor &sensor) const {
00118         return sensors.count(sensor);
00119     }
00120 
00121     bool hasAccelerometer() const {
00122         return hasSensor(msp::msg::Sensor::Accelerometer);
00123     }
00124 
00125     bool hasBarometer() const {
00126         return hasSensor(msp::msg::Sensor::Barometer);
00127     }
00128 
00129     bool hasMagnetometer() const {
00130         return hasSensor(msp::msg::Sensor::Magnetometer);
00131     }
00132 
00133     bool hasGPS() const {
00134         return hasSensor(msp::msg::Sensor::GPS);
00135     }
00136 
00137     bool hasSonar() const {
00138         return hasSensor(msp::msg::Sensor::Sonar);
00139     }
00140 
00141     bool isStatusActive(const std::string& status_name);
00142 
00143     bool isArmed() { return isStatusActive("ARM"); }
00144 
00145     bool isStatusFailsafe() { return isStatusActive("FAILSAFE"); }
00146 
00160     bool setRc(const uint16_t roll, const uint16_t pitch,
00161                const uint16_t yaw, const uint16_t throttle,
00162                const uint16_t aux1 = 1000, const uint16_t aux2 = 1000,
00163                const uint16_t aux3 = 1000, const uint16_t aux4 = 1000,
00164                const std::vector<uint16_t> auxs = std::vector<uint16_t>());
00165 
00171     bool setRc(const std::vector<uint16_t> channels);
00172 
00173     bool setMotors(const std::array<uint16_t,msp::msg::N_MOTOR> &motor_values);
00174 
00180     bool arm(const bool arm);
00181 
00186     bool arm_block();
00187 
00192     bool disarm_block();
00193 
00203     int updateFeatures(const std::set<std::string> &add = std::set<std::string>(),
00204                        const std::set<std::string> &remove = std::set<std::string>());
00205 
00213     bool enableRxMSP() {
00214         return updateFeatures(
00215             {"RX_MSP"}, // add
00216             {"RX_PARALLEL_PWM", "RX_PPM", "RX_SERIAL"} // remove
00217         );
00218     }
00219 
00220     bool reboot();
00221 
00222     bool writeEEPROM();
00223 
00224 private:
00225 
00226     static const uint8_t MAX_MAPPABLE_RX_INPUTS = 8;
00227 
00228     msp::client::Client client;
00229 
00230     std::map<std::string, uint> box_name_ids;
00231 
00232     msp::msg::Ident ident;
00233 
00234     std::set<msp::msg::Sensor> sensors;
00235 
00236     FirmwareType firmware;
00237 
00238     std::vector<uint8_t> channel_map;
00239 };
00240 
00241 } // namespace msp
00242 
00243 #endif // FLIGHTCONTROLLER_HPP


msp
Author(s): Christian Rauch
autogenerated on Mon Oct 9 2017 03:02:13