Diff: MjHoverBase.cpp

Revision:

3:b03009537d66

Child:

5:bd26aba87b9b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MjHoverBase.cpp	Fri Apr 03 04:11:05 2015 +0000
@@ -0,0 +1,81 @@
+#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