MSP.hpp

Go to the documentation of this file.

#ifndef MSP_HPP
#define MSP_HPP

#include "types.hpp"

#include <asio.hpp>
#include <stdexcept>
#include <chrono>
#include <mutex>

namespace msp {

// size of header+crc, e.g. amount of bytes in MSP message that do not belong to payload
// preamble (2) + direction (1) + size (1) + command (1) + crc (1) = 6
static const uint FRAME_SIZE = 6;

// exception to throw when header contains wrong data
class MalformedHeader : public std::runtime_error {
public:
    MalformedHeader(const uint8_t exp, const uint8_t rcv)
        : std::runtime_error(
              "Malformed header: "
              "expected "+std::to_string(exp)+" ("+std::string(1,char(exp))+"), "
              "received "+std::to_string(rcv)+" ("+std::string(1,char(rcv))+")") {}
};

class UnknownMsgId : public std::runtime_error {
private:
    const uint8_t msg_id;
public:
    UnknownMsgId(uint8_t id) : runtime_error(
        "Unknown MSP id! FC refused to process message with id: "+std::to_string(id)), msg_id(id)
    { }

    const uint8_t &getInvalidID() const { return msg_id; }
};

// exception to throw if reported CRC does not match with computed
class WrongCRC : public std::runtime_error {
public:
    WrongCRC(const uint8_t msg_id, const uint8_t exp, const uint8_t rcv)
        : std::runtime_error(
              "CRC not matching: "
              "Message "+std::to_string(uint(msg_id))+", "
              "expected CRC "+std::to_string(exp)+", "
              "received CRC "+std::to_string(rcv))
    { }
};

class NoData : public std::runtime_error {
public:
    NoData() : std::runtime_error("No data available!") { }
};

class NoConnection : public std::runtime_error {
public:
    NoConnection(const std::string &device, const std::string &msg)
        : runtime_error("Device not available: "+device+" ("+msg+")")
    { }
};

struct DataID {
    ByteVector data;    
    uint8_t id;         

    DataID(const ByteVector data, const uint8_t id) : data(data), id(id) {}
};

class MSP {
public:

    MSP();

    MSP(const std::string &device, const uint baudrate=115200);

    bool connect(const std::string &device, const uint baudrate=115200);

    bool request(msp::Request &request);

    bool request_block(msp::Request &request);

    bool request_wait(msp::Request &request, const uint wait_ms, const uint min_payload_size = 0);

    bool respond(const msp::Response &response);

    bool respond_block(const msp::Response &response);

    bool sendData(const uint8_t id, const ByteVector &data = ByteVector(0));

    bool sendData(const msp::ID id, const ByteVector &data = ByteVector(0)) {
        return sendData(uint8_t(id), data);
    }

    bool send(const msp::Response &response) {
        return sendData(response.id(), response.encode());
    }

    DataID receiveData();

    void setWait(unsigned int wait_us) {
        wait = wait_us;
    }

private:
    uint8_t crc(const uint8_t id, const ByteVector &data);

    bool write(const std::vector<uint8_t> &data);

    size_t read(std::vector<uint8_t> &data);

    std::vector<uint8_t> read(std::size_t n_bytes);

    uint8_t read() {
        return read(1).front();
    }

    int hasData();

    void clear();

    std::string device;
    asio::io_service io;     
    asio::serial_port port;  
    std::mutex lock_write;
    std::mutex lock_read;
    unsigned int wait;  
};

} // namespace msp

#endif // MSP_HPP