File content as of revision 3:b03009537d66:

#include "MjHover.h"
#include <vector>
using namespace std;

namespace matsujirushi {

MjHoverBase::MjHoverBase(I2C* i2c, uint8_t address, DigitalInOut* ts_n, DigitalInOut* reset_n)
{
    this->i2c = i2c;
    this->address = address & 0xfe;
    this->ts_n = ts_n;
    this->reset_n = reset_n;
}

void MjHoverBase::Reset()
{
    this->ts_n->input();
    this->reset_n->output();
    this->reset_n->write(0);
    wait_ms(1);
    this->reset_n->input();
    wait_ms(3000);
}

bool MjHoverBase::WriteMessage(uint8_t id, const uint8_t* payload, uint8_t payloadSize)
{
    if (4 + payloadSize > 255)
    {
        return false;
    }
    
    vector<uint8_t> message(4 + payloadSize);
    message[0] = 4 + payloadSize;   // Msg. Size
    message[1] = 0;                 // Flags
    message[2] = 0;                 // Seq.
    message[3] = id;                // ID
    memcpy(&message[4], payload, payloadSize);
    
    if (this->i2c->write(this->address, (const char*)&message[0], message.size()) != 0)
    {
        return false;
    }
    
    return true;
}

bool MjHoverBase::ReadMessage(uint8_t* id, uint8_t* payload, uint8_t payloadSize, uint8_t* readPayloadSize)
{
    if (!this->DataAvailable())
    {
        return false;
    }
    
    this->i2c->start();
    
    this->i2c->write(this->address | 1);

    uint8_t messageSize = this->i2c->read(1);
    vector<uint8_t> message(messageSize);
    message[0] = messageSize;
    uint8_t* messagePtr = &message[1];
    for (int i = 1; i < messageSize; i++)
    {
        *messagePtr++ = this->i2c->read(i < messageSize - 1 ? 1 : 0);
    }
    
    this->i2c->stop();
    
    if (payloadSize + 4 < messageSize)
    {
        return false;
    }

    *id = message[3];
    memcpy(payload, &message[4], messageSize - 4);
    *readPayloadSize = messageSize - 4;
    
    return true;
}

} // namespace matsujirushi