MSP2/msp2.cpp at main · Intelligent-Embedded-Control-Laboratory/MSP2 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
#include "msp2.h"

#define MSP2_DEBUG_ENABLE 0

void MSP2::begin(HardwareSerial& _MSPSerial, uint32_t baud)
{
    MSPSerial = &_MSPSerial;
    MSPSerial->begin(baud);

    event_flag = MSP2_WAIT_START_BYTES;
}

uint16_t MSP2::read()
{
    while (event_flag == MSP2_WAIT_START_BYTES && MSPSerial->available() >= 8)
    {
        header.start_bytes[0] = (uint8_t)MSPSerial->read();

        if (header.start_bytes[0] == 0x24)
        {
            header.start_bytes[1] = (uint8_t)MSPSerial->read();

            if (header.start_bytes[1] == 0x58)
            {
                MSPSerial->readBytes(&header.type, 6U);

                if (header.payload_size > 9)
                {
                    Serial.printf("[MSP2 Fail]: payload_size = %d\n", header.payload_size);
                }
                else
                {
                    event_flag = MSP2_WAIT_PACKET;
                }
            }
            else
            {
                Serial.printf("[MSP2 Fail]: start_byte[1] = %d\n", header.start_bytes[1]);
            }
        }
        else
        {
            Serial.printf("[MSP2 Fail]: start_byte[0] = %d\n", header.start_bytes[0]);
        }
    }

#if MSP2_DEBUG_ENABLE
    Serial.print("Start byte[0] = ");
    Serial.print(header.start_bytes[0], HEX);
    Serial.print(" ");

    Serial.print("Start byte[1] = ");
    Serial.print(header.start_bytes[1], HEX);
    Serial.printf(" Size: %d", header.payload_size);
    Serial.printf(" Flag: %d", event_flag);
    Serial.printf(" Buffer %d bytes available ", MSPSerial->available());
    Serial.println();
#endif

    if (event_flag == MSP2_WAIT_PACKET && MSPSerial->available() >= (int)(header.payload_size) + 1)
    {
#if MSP2_DEBUG_ENABLE
        Serial.print("Payload Size: ");
        Serial.println((int)header.payload_size);
#endif
        // Parse data
        switch (header.function)
        {
        case MSP2_SENSOR_RANGEFINDER:
            _is_update_rangefinder = true;
            rangefinder.quality = (uint8_t)MSPSerial->read();
            MSPSerial->readBytes((uint8_t*)&rangefinder.distance_mm, header.payload_size - 1);
#if MSP2_DEBUG_ENABLE
            Serial.println("RANGEFINDER!");
#endif
            break;
        case MSP2_SENSOR_OPTIC_FLOW:
            _is_update_optic_flow = true;
            optical_flow.quality = (uint8_t)(MSPSerial->read());
            MSPSerial->readBytes((uint8_t*)(&optical_flow.motionDX), header.payload_size - 1);
#if MSP2_DEBUG_ENABLE
            Serial.println("OPTICAL FLOW!");
#endif
            break;
        default:
            Serial.println("No matched ID!");
            Serial.println(header.function);
        }
        event_flag = MSP2_WAIT_START_BYTES;
        checksum = (uint8_t)(MSPSerial->read());

        // return header.function; // return data id
        return 0x0001;
    }
    return 0x0000; // mean data not yet ready
}